import math
from isceobj.Planet.Ellipsoid import Ellipsoid
from isceobj.Planet.AstronomicalHandbook import PlanetsData
from isceobj.Util.mathModule import MathModule as MM

class SCH(object):
    """A Class to convert between SCH and XYZ coordinates"""
    
    def __init__(self,peg=None):
        self.peg = peg
        self.r_ov = [0 for i in range(3)]
        self.M = [[0 for i in range(3)] for j in range(3)]
        self.invM = [[0 for i in range(3)] for j in range(3)]
        self.__initialize()
   
    def __initialize(self):
        self.initializeTranslationVector()
        self.initializeRotationMatrix()

    def initializeRotationMatrix(self):
        lat = math.radians(self.peg.getLatitude())
        lon = math.radians(self.peg.getLongitude())
        heading = math.radians(self.peg.getHeading())
                
        self.M[0][0] = math.cos(lat)*math.cos(lon)
        self.M[0][1] = -math.sin(heading)*math.sin(lon) - math.sin(lat)*math.cos(lon)*math.cos(heading)
        self.M[0][2] = math.sin(lon)*math.cos(heading) - math.sin(lat)*math.cos(lon)*math.sin(heading)
        self.M[1][0] = math.cos(lat)*math.sin(lon)
        self.M[1][1] = math.cos(lon)*math.sin(heading) - math.sin(lat)*math.sin(lon)*math.cos(heading)
        self.M[1][2] = -math.cos(lon)*math.cos(heading) - math.sin(lat)*math.sin(lon)*math.sin(heading)
        self.M[2][0] = math.sin(lat)
        self.M[2][1] = math.cos(lat)*math.cos(heading)
        self.M[2][2] = math.cos(lat)*math.sin(heading)
        
        self.invM = MM.matrixTranspose(self.M)
        
    def initializeTranslationVector(self):
        lat = math.radians(self.peg.getLatitude())
        lon = math.radians(self.peg.getLongitude())        
        radcur = self.peg.getRadiusOfCurvature() # Get the radius of curvature at the peg point
                       
        r_up = [0 for i in range(3)]
        r_p = [0 for i in range(3)]
                           
        r_up[0
                ] = math.cos(lat)*math.cos(lon)
        r_up[1] = math.cos(lat)*math.sin(lon)
        r_up[2] = math.sin(lat)
        
        # The Cartesian vector at the peg latitude and longitude at zero height
        r_p = self._calculateXYZ()
                
        for i in range(3):
            self.r_ov[i] = r_p[i] - radcur*r_up[i]

    def _calculateXYZ(self):
        """
        Calculate the cartesian coordinate of the point assuming the WGS-84 ellipsoid (to be fixed)
        """
        ellipsoid = Ellipsoid(a=PlanetsData.ellipsoid['Earth']['WGS-84'][0],
                              e2=PlanetsData.ellipsoid['Earth']['WGS-84'][1])
        llh = [self.peg.getLatitude(),self.peg.getLongitude(),0.0]
        xyz = ellipsoid.llh_to_xyz(llh)
        return xyz
                        
    def xyz_to_sch(self,xyz):        
        radcur = self.peg.getRadiusOfCurvature() # Get the radius of curvature at the peg point
        ellipsoid = Ellipsoid(a=radcur,e2=0.0)
        
        
                
        schvt = [0 for i in range(3)]
        rschv = [0 for i in range(3)]
        
        for i in range(3):
            schvt[i] = xyz[i] - self.r_ov[i]
        
        schv = MM.matrixVectorProduct(self.invM,schvt)        
        llh = ellipsoid.xyz_to_llh(schv)
        
        rschv[0] = radcur*math.radians(llh[1])
        rschv[1] = radcur*math.radians(llh[0])
        rschv[2] = llh[2]
        
        return rschv
    
    def sch_to_xyz(self,sch):        
        radcur = self.peg.getRadiusOfCurvature() # Get the radius of curvature at the peg point
        ellipsoid = Ellipsoid(a=radcur,e2=0.0)
        
        xyz = [0 for i in range(3)]
        llh = [0 for i in range(3)]
                
        llh[0] = math.degrees(sch[1]/radcur)
        llh[1] = math.degrees(sch[0]/radcur)
        llh[2] = sch[2]
        
        schv = ellipsoid.llh_to_xyz(llh)
        schvt = MM.matrixVectorProduct(self.M,schv)
        
        for i in range(3):
            xyz[i] = schvt[i] + self.r_ov[i]
            
        return xyz

    def vxyz_to_vsch(self,sch,vxyz):
        """
        Convert from cartesian velocity to sch velocity
        """
        schbasis = LocalSCH(peg=self.peg,sch=sch)
        vsch = schbasis.xyz_to_localsch(vxyz)

        return vsch

    def vsch_to_vxyz(self,sch,vsch):
        """
        Convert from sch velocity to cartesian velocity
        """
        schbasis = LocalSCH(peg=self.peg,sch=sch)
        vxyz = schbasis.localsch_to_xyz(vsch)

        return vxyz

class LocalSCH(SCH):
#    It almost might be better to define an SCH 'Location' object 
#    that can convert things between its local tangent plane and back

    def __init__(self,peg=None,sch=None):
        SCH.__init__(self,peg=peg)
        self.sch = sch
        self.sch2xyz = [[0 for i in range(3)] for j in range(3)]
        self.xyz2sch = [[0 for i in range(3)] for j in range(3)]

        self.__initialize()

    def __initialize(self):
        s = self.sch[0]/self.peg.getRadiusOfCurvature()
        c = self.sch[1]/self.peg.getRadiusOfCurvature()

        schxyzp = [[0 for i in range(3)] for j in range(3)]
        schxyzp[0][0] = -math.sin(s)
        schxyzp[0][1] = -math.sin(c)*math.cos(s)
        schxyzp[0][1] = math.cos(s)*math.cos(c)
        schxyzp[1][0] = math.cos(s)
        schxyzp[1][1] = -math.sin(c)*math.sin(s)
        schxyzp[1][2] = math.sin(s)*math.cos(c)
        schxyzp[2][0] = 0.0
        schxyzp[2][1] = math.cos(c)
        schxyzp[2][2] = math.sin(c)

        self.sch2xyz = MM.multiplyMatrices(self.M,schxyzp)
        self.xyz2sch = MM.matrixTranspose(self.sch2xyz)

    def xyz_to_localsch(self,xyz):
        sch = MM.matrixVectorProduct(self.xyz2sch,xyz)

        return sch

    def localsch_to_xyz(self,sch):
        xyz = MM.matrixVectorProduct(self.sch2xyz,sch)

        return xyz