from ._clothoids_cpp import ClothoidCurve, G2solve3arc

import numpy as np

CLOTHOID_FUNCTION_WINDOW = frozenset(("X","XD","XDD","XDDD","Y","YD","YDD","YDDD",
                                "Theta","ThetaD","ThetaDD","ThetaDDD"))

CLOTHOID_PROPERTY_WINDOW = frozenset(("length","dk","ThetaStart","ThetaEnd","XStart",
                                "XEnd","YStart","YEnd","KappaStart","KappaEnd"))

class Clothoid(object):
    """
    An object representing a single clothoid curve.  
    Pickling and unpickling is supported.
    The class constructor is meant for internal use for interfacing with the C++ layer.  To initialize a Clothoid, use one of the classmethods instead.
    
    """
    def __init__(self,clothoid_curve):
        if type(clothoid_curve) == type(self):
            #Create a copy of the underlying C++ clothoid when constructor is called with a Python Clothoid
            self._ClothoidCurve = ClothoidCurve(clothoid_curve._ClothoidCurve)
        else:
            #No need to create a copy when a C++ clothoid is passed directly by the classmethods or G2solver
            self._ClothoidCurve = clothoid_curve
    
    @classmethod
    def StandardParams(cls,x0,y0,t0,k0,kd,s_f):
        """
        A method to initialize a Clothoid given a starting point, starting tangent, starting curvature, curvature rate, and final length.

        """
        temp_clothoid = ClothoidCurve()
        temp_clothoid.build(x0,y0,t0,k0,kd,s_f)
        return cls(temp_clothoid) 
    
    @classmethod
    def G1Hermite(cls,x0,y0,t0,x1,y1,t1):
        """
        A method to numerically compute the solution to the G1 Hermite interpolation problem and initialize a Clothoid object with the solution parameters.
        """
        temp_clothoid = ClothoidCurve()
        temp_clothoid.build_G1(x0,y0,t0,x1,y1,t1)
        return cls(temp_clothoid)
    
    def __getattr__(self,name):
        if name in CLOTHOID_FUNCTION_WINDOW:
            return getattr(self._ClothoidCurve,name)
        if name in CLOTHOID_PROPERTY_WINDOW:
            return getattr(self._ClothoidCurve,name)() #mimic property getter syntax
        return super().__getattribute__(name)()
    
    def __str__(self):
        return 'Clothoid: ' + ''.join(map(lambda m,n:m + ':' + str(getattr(self,n)) + ' ',
                                             ('x0','y0','y0','k0','kd','s'),('XStart','YStart','ThetaStart','KappaStart','dk','length')))
    
    def __repr__(self):
        return str(self)
    
    def __getstate__(self):
        return self.Parameters
    
    def __setstate__(self,state):
        temp_clothoid = ClothoidCurve()
        self._ClothoidCurve = temp_clothoid.build(*state)
    
    @property
    def Parameters(self):
        """
        Complete data describing the calling Clothoid

        :getter: Returns the initialization parameters of a clothoid, fit to be used as args to StandardParams
        :setter: Parameters cannot be modified
        :type: tuple
        """
        return (self.XStart,self.YStart,self.ThetaStart,self.KappaStart,self.dk,self.length)

    def SampleXY(self,npts):
        """
        A method to return a vector of X coordinates and Y coordinates generated by evaluating the Clothoid at npts equally spaced points along its length.

        Roughly shorthand for:

        ::

            def SampleXY(self,npts):
                sample_points = [self.length * m/(npts-1) for m in range(0,npts)]
                X = [self.X(i) for i in sample_points]
                Y = [self.Y(i) for i in sample_points]
                return (X,Y)
        """
        return [[j(i) for i in np.linspace(0,self.length,npts)] for j in (self.X,self.Y)] #TODO: move sampling to c++ layer for loop efficiency?

    def Scale(self,sfactor,center = (0,0)):
        """Returns a copy of the calling clothoid subjected to a scaling transform with a scale of sfactor and a stationary point at center"""
        if sfactor == 0:
            return Clothoid.StandardParams(0,0,0,0,0,0)
        temp_clothoid = Clothoid(self)
        temp_clothoid._ClothoidCurve._scale(sfactor) ##DANGER WILL ROBINSON : MUTATING STATE DIRECTLY##
        if center == 'start':
            return temp_clothoid
        s = np.array([temp_clothoid.XStart,temp_clothoid.YStart])
        c = np.array(center)
        dxy = (sfactor-1)*(s-c)
        temp_clothoid._ClothoidCurve._translate(*dxy) ##DANGER WILL ROBINSON : MUTATING STATE DIRECTLY##
        return temp_clothoid
    
    def Translate(self,xoff,yoff):
        """
        Returns a copy of the calling clothoid subjected to a pure translation transform described by a vector (xoff,yoff)
        """
        temp_clothoid = Clothoid(self)
        temp_clothoid._ClothoidCurve._translate(xoff,yoff) ##DANGER WILL ROBINSON : MUTATING STATE DIRECTLY##
        return temp_clothoid
    
    def Rotate(self,angle,center = (0,0)):
        """
        Returns a copy of the calling clothoid subjected to a pure rotation transform of angle and a stationary point at center
        """
        cx,cy = center
        temp_clothoid = Clothoid(self)
        temp_clothoid._ClothoidCurve._rotate(angle,cx,cy) ##DANGER WILL ROBINSON : MUTATING STATE DIRECTLY##
        return temp_clothoid
    
    def Reverse(self):
        """
        Returns a copy of the calling clothoid with the direction of the arc length parameter reversed
        """
        temp_clothoid = Clothoid(self)
        temp_clothoid._ClothoidCurve._reverse() ##DANGER WILL ROBINSON : MUTATING STATE DIRECTLY##
        return temp_clothoid
    
    def Trim(self,s_begin,s_end):
        """
        Returns a copy of the subsection of the calling clothoid that lies between s_begin and s_end
        """
        temp_clothoid = Clothoid(self)
        temp_clothoid._ClothoidCurve._trim(s_begin,s_end) ##DANGER WILL ROBINSON : MUTATING STATE DIRECTLY##
        return temp_clothoid
    
    def Flip(self,axis = 'y'):
        """
        Returns a copy of the calling clothoid that has been flipped symmetrically along a specified axis
        
        currently supported options are:
        
        * 'y'
        * 'x'
        * 'start'
        
        Where 'start' represents a line tangent to the clothoid at its starting point.
        """
        xp = self.XStart
        yp = self.YStart
        th = self.ThetaStart
        dx = np.cos(th)
        dy = np.sin(th)
        if axis == 'y':
            return Clothoid.StandardParams(-xp,yp,np.arctan2(dy,-dx),-self.KappaStart,-self.dk,self.length)
        if axis == 'x':
            return Clothoid.StandardParams(xp,-yp,np.arctan2(-dy,dx),-self.KappaStart,-self.dk,self.length)
        if axis == 'start':
            return Clothoid.StandardParams(xp,yp,th,-self.KappaStart,-self.dk,self.length)
        

def SolveG2(x0,y0,t0,k0,x1,y1,t1,k1):
    """Returns a tuple of three Clothoids that form a G2 continuous path that interpolates two cartesian endpoints, two tangents, and two curvatures"""
    solver = G2solve3arc()
    solver.build(x0,y0,t0,k0,x1,y1,t1,k1,0,0)
    return tuple(map(Clothoid,(solver.getS0(),solver.getSM(),solver.getS1())))
    
    