
    w0hs                     t    d dl Z d dlZd dlZd dlmZ ddZd ZdeeffdZ	ddefdZ
ddZd	 Zd
 Zd Zd Zy)    N)minimize_scalarc                 t   t        j                  t        j                  | ||            }t        j                  t        j                  | ||            }t        | ||      }t        j                  |||      }t        j                  |||      }t        j                  |      t        j                  |      fS )z
    Fits the cubic splines to, x,y points but and reparametrize them according to arch-length
    i.e. for example if s is time it will reparametrize them cubic splines to path progress
    )nppoly1dpolyfitcompute_arc_lengths)	sxydegreeinitial_poly_xinitial_poly_y
arclengthsx_polyy_polys	            ?/home/martin/Skola/nxp_cup/acados/nxp_cup/helpers/processing.pyfit_arclen_polyr      s     YYrzz!Q78NYYrzz!Q78N %QGJZZ
Av.FZZ
Av.F99Vbii///    c                      |j                         |       } |j                         |       } |j                  d      |       } |j                  d      |       }||z  ||z  z
  |dz  |dz  z   dz  z  }|S )zS
    Computes curvature kappa for given s values using the fitted polynomials.
       g      ?)deriv)s_valuesr   r   dx_dsdy_dsd2x_ds2d2y_ds2	curvatures           r   compute_curvaturer      s    
 FLLN8$EFLLN8$Efll1oh'Gfll1oh'G 57?2uzEQJ7NTY6ZZIr   returnc                 X    |\   fd}t        ||d   |d   fd      }|j                  j                         }  |      } |      }|z
  }	|z
  }
t         |      }|
t	        j
                  |      z  |	t	        j                  |      z  z
  }||j                         fS )z
    Finds the closest s parameter on the curve to a given target point (x, y).
    Also returns the signed distance to the closest point.
    c                 F     |       } |       }|z
  dz  |z
  dz  z   S Nr    )r	   x_sy_sr   x_targetr   y_targets      r   distance_functionz)find_closest_s.<locals>.distance_function/   s2    QiQih1$h1'<<<r   r   bounded)boundsmethod)r   r
   itemcompute_tangent_angler   cossin)r   r   target_points_ranger(   result	s_closest	x_closest	y_closestdxdypsisigned_distancer&   r'   s   ``           @@r   find_closest_sr;   (   s    
 &Hh=
 .
GBK7PYbcFI y!Iy!I	I	B	I	B
	
:C266#;&bffSk)99Oo**,,,r   c                    | ||        ||       f}| ||       ||      f}t        j                  |d   |d   z
  dz  |d   |d   z
  dz  z         }||kD  rt        t        j                  |dz  |z              }t        j                  | ||dz         }	d}
|\  }}| }t        dt        |	            D ]5  }|	|   } ||       ||      }}|
t        ||||||f||f|      z  }
|}|}|}7 |
S |S )a  
    Compute the arc length between two points, recursively splitting the interval if necessary.

    Args:
    - s0, s1: The s-parameter values for the start and end points.
    - poly_x, poly_y: Parametric functions to compute x, y coordinates for given s values.
    - xy1, xy2: The (x, y) coordinates of the start and end points.
    - max_increase: The maximum allowed distance between consecutive points.

    Returns:
    - The arc length between s0 and s1.
    r   r      g?)r   sqrtintceillinspacerangelencompute_arc_length)s0s1r   r   xy1xy2max_arc_increasedistnum_intervalsrefined_s_values
arc_lengthx_starty_starts_startis_endx_endy_ends                     r   rD   rD   D   sD    {bz6":&
{bz6":&77CFSVO)SVc!f_,BBCD BGGD3J1A$ABC ;;r2}q/@A
q#./0 	A$Q'E "%=&-5E ,WeVVgW^M_.3U^-=? ?J GGG	  r   c           
          dg}d} || d          || d         f}t        dt        |             D ]I  }| |dz
     | |   }	} ||	       ||	      f}
t        ||	||||
|      }||z  }|j                  |       |
}K |S )Ng        r   r=   )rI   )rB   rC   rD   append)r	   r   r   max_arc_steparc_lengthscumulative_lengthstart_xyrQ   rE   rF   end_xyrM   s               r   r   r   {   s    %KqtfQqTl+H1c!f 1q51Q4B*fRj)'Bxcop
Z',- r   c                     | t         j                  kD  r*| dt         j                  z  z  } | t         j                  kD  r*| t         j                   k  r+| dt         j                  z  z  } | t         j                   k  r+| S r"   )mathpi)r
   s    r   
wrap_angler_      s_    
dgg+	Q[ dgg+
twwh,	Q[ twwh,Hr   c                      | j                         |      } |j                         |      }t        j                  ||      }|j                         S )aG  
    Computes the tangent direction (angle) of a cubic spline curve at a given arc length s.

    Parameters:
    - x_spline: CubicSpline object for x(s)
    - y_spline: CubicSpline object for y(s)
    - s: the arc length value where we want to compute the direction

    Returns:
    - theta: The tangent angle in radians
    )r   r   arctan2r-   )r   r   r	   r   r   thetas         r   r.   r.      sF     FLLN1EFLLN1E JJue$E::<r   c                 2   |\  }}}}}|d   }||z  }|| k\  r|| k\  ||k  z  }||   }	||   }
||   }na|| k\  }||k  }t        j                  ||   ||   f      }	t        j                  ||   ||   f      }
t        j                  ||   ||   f      }|	|
|fS )a  
    Extracts track points where Sref is within [min_s, max_s], handling circular track cases.

    Parameters:
        min_s (float): Minimum s value.
        max_s (float): Maximum s value.
        track (tuple): Track data as (Sref, xRef, yRef, psiRef, kappa).

    Returns:
        tuple: (s, x, y) where each array contains only the filtered values, sorted in circular order.
    r)   )r   concatenate)min_smax_stracks_refx_refy_ref_	track_lenmask
s_filtered
x_filtered
y_filteredmask1mask2s                 r   get_track_pointsrs      s     !&E5%Ab	IIE~5E>24[
4[
4[
 %%^^U5\5<$@A
^^U5\5<$@A
^^U5\5<$@A
 z:--r   c                     t        t        |||       |z         } ||       t        j                  |      |z  z
  } ||       t        j                  |      |z  z   }|||fS )N)r_   r.   r   r0   r/   )sinialphar   r   r9   r
   r   s           r   transform_from_poly_projrx      s]    
*FFB?%G
HCr
RVVC[2%%Ar
RVVC[2%%Aa9r   )   )NN皙?)rz   )	itertoolsr]   numpyr   scipy.optimizer   r   r   floatr;   rD   r   r_   r.   rs   rx   r#   r   r   <module>r      sS       *0$-eU^ -84\a 4n$, .Fr   