''' 卫星轨道模型
根据卫星轨道节点，生成对应的卫星轨道模型。
整个卫星轨道模型封装成一个单独类。
'''

from os import times
from time import time
import numpy as np
import util

def ReconstructionSatelliteOrbit(GPSPoints_list,starttime=1262275200.0):
    '''
    构建卫星轨道
    args:
        GPSPoints_list:卫星轨道点
        starttime:起算时间，默认设置为2010年1月1日，0时0分0秒 
    '''
    if len(GPSPoints_list)>16:
        SatelliteOrbitModel=SatelliteOrbit_FitPoly()
        if SatelliteOrbitModel.ReconstructionSatelliteOrbit(GPSPoints_list,starttime=starttime) is None:
            return None
        return SatelliteOrbitModel

class SatelliteOrbit(object):
    def __init__(self) -> None:
        super().__init__()
        self.__starttime=1262275200.0

    pass
    def get_starttime(self):
        '''
        返回卫星轨道时间起算点
        '''
        return self.__starttime
    def ReconstructionSatelliteOrbit(self,GPSPoints_list):
        '''
        重建卫星轨道，使用多项式拟合法。
        args:
            GPSPoints_list:GPS卫星轨道点。
        return：
            SatelliteOrbitModel 卫星轨道模型
        '''
        self.SatelliteOrbitModel=None
        pass
    
    pass
    def SatelliteSpaceState(self,time_float):
        '''
        根据时间戳，返回对应时间的卫星的轨迹状态
        args:
            time_float:时间戳
        return：
            State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
        '''
        return None
    
class SatelliteOrbit_FitPoly(SatelliteOrbit):
    '''
    继承于SatelliteOribit类，为拟合多项式实现方法
    '''
    def __init__(self) -> None:
        super().__init__()

    def ReconstructionSatelliteOrbit(self,GPSPoints_list,starttime=1262275200.0):
        if len(GPSPoints_list)>16: 
            # 多项式的节点数，理论上是超过4个可以起算，这里为了精度选择16个点起算。
            # 多项式 AX=Y ==> A=（X'X)^X'Y，其中 A 为待求系数，X为变量，Y为因变量
            # 这里使用三次项多项式，共有6组参数。
            # 声明自变量，因变量，系数矩阵
            self.__starttime=starttime
            record_count=len(GPSPoints_list)
            time_arr=np.zeros((record_count,1),dtype=np.float64) # 使用np.float64只是为了精度高些，如果32位也能满足需求，请用32
            state_arr=np.zeros((record_count,6),dtype=np.float64)
            A_arr=np.zeros((5,6),dtype=np.float64)
            # 将点记录转换为自变量矩阵、因变量矩阵
            for i in range(record_count):
                GPSPoint=GPSPoints_list[i]
                time_arr[i,0]=GPSPoint[0]-self.__starttime
                state_arr[i,:]=np.array(GPSPoint[1:]).reshape(1,6)
            # 使用最小二乘法，求解系数
            X=np.concatenate([np.ones((record_count,1),dtype=np.float64),time_arr,time_arr**2,time_arr**3,time_arr**4],axis=1)
            
            for i in range(6):
                Y=state_arr[:,i]
                A_arr[:,i]=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y).reshape(1,5) # 
            self.A_arr=A_arr 
            
            return A_arr # shape 6x4
        else:
            self.A_arr=None
            return None
    def SatelliteSpaceState(self,time_float):
        '''
        根据时间戳，返回对应时间的卫星的轨迹状态，会自动计算与起算时间之差
        args:
            time_float:时间戳
        return：
            State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
        '''
        if self.A_arr is None: 
            return None
        time_float=time_float-self.__starttime
        time_arr=np.ones((1,5),dtype=np.float)
        time_arr[0,0]=time_arr[0,0]
        time_arr[0,1]=time_arr[0,1]*time_float
        time_arr[0,2]=time_arr[0,2]*time_float**2
        time_arr[0,3]=time_arr[0,3]*time_float**3
        time_arr[0,4]=time_arr[0,4]*time_float**4
        SatelliteState_xyz_Vxyz_arr=np.matmul(time_arr,self.A_arr).reshape(1,6)
        return np.concatenate([np.array(time_float+self.__starttime).reshape(1,1),SatelliteState_xyz_Vxyz_arr],axis=1)
    def getSatelliteSpaceState(self,time_array):
        '''
        根据时间戳矩阵，返回对应时刻的卫星空间状态（位置，速度），且会自动计算与起算时间之差
        args:
            time_array:nparray nx1 时间戳
        return:
            SatellitSpaceStateArray:nparray nx6 状态时间
        '''
        if self.A_arr is None:
            return None # 返回None,表示没有结果
        A=np.transpose(self.A_arr,(1,0)) # shape 4x6
        timeRelative1=time_array-self.__starttime # 返回相对值
        n=timeRelative1.shape[0]
        timeRelative=np.ones((n,5))
        timeRelative[:,1]=timeRelative1
        timeRelative[:,2]=timeRelative1**2
        timeRelative[:,3]=timeRelative1**3 
        timeRelative[:,4]=timeRelative1**4 
        SatelliteSpaceStateArray=np.matmul(timeRelative,A) #  nx6
        return SatelliteSpaceStateArray # 位置矩阵

        pass