import copy
import json
import math
import datetime
from xml.etree.ElementTree import ElementTree
from collections import namedtuple
import numpy as np
import numbers

# cosine in degress (math could be <a href="http://numpy.scipy.org/">numpy</a>
import xmltodict

cosd = lambda x: np.cos(np.radians(x))
# sine in degrees
sind = lambda x: np.sin(np.radians(x))
# tangent, in degrees
tand = lambda x: np.tan(np.radians(x))
# arc tan in degrees (2 arg)
arctand2 = lambda y, x: np.degrees(np.arctan2(y, x))
# arc tan in degrees (1 arg)
arctand = lambda x: np.degrees(np.arctan(x))

reference_ellipsoid_params = {
    'a': 6378137,  # 椭球长半轴 (m)
    'b': 6356752,  # 椭球短半轴 (m)
    # 其他椭球参数...
}
LIGHTSPEED = 299792458
# 第一偏心率的平方
e2 = (reference_ellipsoid_params['a']**2 - reference_ellipsoid_params['b']**2) / reference_ellipsoid_params['a']**2

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

    def get_starttime(self):
        '''
        返回卫星轨道时间起算点
        '''
        return self.starttime

    def ReconstructionSatelliteOrbit(self, GPSPoints_list):
        '''
        重建卫星轨道，使用多项式拟合法
        args:
            GPSPoints_list:GPS 卫星轨道点
        return：
            SatelliteOrbitModel 卫星轨道模型
        '''
        self.SatelliteOrbitModel = None

    def SatelliteSpaceState(self, time_float):
        '''
        根据时间戳，返回对应时间的卫星的轨迹状态
        args:
            time_float:时间戳
        return：
            State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
        '''
        return None

class SatelliteOrbitFitPoly(SatelliteOrbit):
    '''
    继承于SatelliteOribit类，为拟合多项式实现方法
    '''

    def __init__(self) -> None:
        super().__init__()
        self.modelName = "多项式"
        self.polynum = 4

    def ReconstructionSatelliteOrbit(self, GPSPoints_list, starttime):
        if len(GPSPoints_list) == 2:
            self.polynum = 1
            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((self.polynum + 1, 6), dtype=np.float64)  # 四次项
            X = np.ones((record_count, self.polynum + 1), dtype=np.float64)  # 记录时间坐标
            # 将点记录转换为自变量矩阵、因变量矩阵

            for i in range(record_count):
                GPSPoint = GPSPoints_list[i]
                time_ = GPSPoint[0] - self.starttime  # 为了保证精度，对时间进行缩放
                X[i, :] = np.array([1, time_])
                state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6)  # 空间坐标
            self.model_f = []
            for i in range(6):
                Y = state_arr[:, i].reshape(-1, 1)
                A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]

            self.A_arr = copy.deepcopy(A_arr.copy())
            return self.A_arr
        elif len(GPSPoints_list) > 6:
            self.polynum = 4
            # 多项式的节点数，理论上是超过5个可以起算，这里为了精度选择10个点起算。
            # 多项式 XA=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((self.polynum + 1, 6), dtype=np.float64)  # 四次项
            X = np.ones((record_count, self.polynum + 1), dtype=np.float64)  # 记录时间坐标
            # 将点记录转换为自变量矩阵、因变量矩阵

            for i in range(record_count):
                GPSPoint = GPSPoints_list[i]
                time_ = GPSPoint[0] - self.starttime  # 为了保证精度，对时间进行缩放
                X[i, :] = np.array([1, time_, time_ ** 2, time_ ** 3, time_ ** 4])
                state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6)  # 空间坐标
            self.model_f = []
            for i in range(6):
                Y = state_arr[:, i].reshape(-1, 1)
                A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]

            self.A_arr = copy.deepcopy(A_arr.copy())
            ''' 测试误差            
            from matplotlib import pyplot
            label_list=['x','y','z','vx','vy','vz']
            color_list=['r','g','b','gold','gray','pink']
            pyplot.figure()
            for i in range(6):
                Y = state_arr[:, i]
                Y_predict=self.model_f[i](X)
                pyplot.subplot(int("23{}".format(i+1)))
                d=Y-Y_predict
                pyplot.plot(X,d,label=label_list[i],color=color_list[i])
                pyplot.title("max:{}".format(np.max(d)))
                #self.model_f.append(interpolate.interp1d(X,Y,kind='cubic',fill_value='extrapolate'))
            pyplot.legend()
            pyplot.show()
            '''
            return self.A_arr
        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.model_f is None:
            return None

        result_arr = np.zeros((1, 7))

        time_float = time_float - self.starttime
        result_arr[0, 0] = time_float
        # time_arr[0, 4] = time_arr[0, 3] * time_float ** 4
        time_float = np.array([1, time_float, time_float ** 2, time_float ** 3, time_float ** 4]).reshape(1, 5)
        result_arr = np.matmul(time_float, self.A_arr)
        return [time_float, result_arr]

    def getSatelliteSpaceState(self, time):
        '''
        矩阵求解
        根据时间戳矩阵，返回对应时刻的卫星空间状态（位置，速度），且会自动计算与起算时间之差
        args:
            time_array:nparray nx1 时间戳
        return:
            SatellitSpaceStateArray:nparray nx6 状态信息
        '''
        time_float = time.timestamp()
        time_float = time_float - self.starttime
        #
        px = 0
        py = 0
        pz = 0
        vx = 0
        vy = 0
        vz = 0
        for ii in range(self.polynum):
            px += self.A_arr[ii][0] * time_float ** ii
            py += self.A_arr[ii][1] * time_float ** ii
            pz += self.A_arr[ii][2] * time_float ** ii
            vx += self.A_arr[ii][3] * time_float ** ii
            vy += self.A_arr[ii][4] * time_float ** ii
            vz += self.A_arr[ii][5] * time_float ** ii
        sv = [px, py, pz, vx, vy, vz]
        return sv
        # if self.model_f is None:
        #     return None  # 返回None,表示没有结果
        # if self.polynum == 4:
        #     n = time_array.shape[0]
        #     result_arr_ = np.zeros((n, 6), dtype=np.float64)
        #     time_float = time_array - self.starttime
        #     time_float = time_float.reshape(-1)  # nx1
        #     time_arr = np.ones((time_float.shape[0], 5))  # nx5
        #     time_arr[:, 1] = time_float
        #     time_arr[:, 2] = time_float ** 2
        #     time_arr[:, 3] = time_float ** 3
        #     time_arr[:, 4] = time_float ** 4
        #     result_arr_ = np.matmul(time_arr, self.A_arr)  # nx5  5x6
        #     # time_arr[0, 4] = time_arr[0, 3] * time_float ** 4
        #     # result_arr=result_arr_
        #     return result_arr_  # 位置矩阵
        # else:
        #     n = time_array.shape[0]
        #     result_arr_ = np.zeros((n, 6), dtype=np.float64)
        #     time_float = time_array - self.starttime
        #     time_float = time_float.reshape(-1)  # nx1
        #     time_arr = np.ones((time_float.shape[0], self.polynum + 1))  # nx5
        #     time_arr[:, 1] = time_float
        #     result_arr_ = np.matmul(time_arr, self.A_arr)  # nx5  5x6
        #     # time_arr[0, 4] = time_arr[0, 3] * time_float ** 4
        #     # result_arr=result_arr_
        #     return result_arr_  # 位置矩阵

class SARgps:

    @staticmethod
    def xyz_to_llh(xyz):
        """xyz_to_llh(xyz): returns llh=(lat (deg), lon (deg), h (meters)) for the instance ellipsoid \n
        given the coordinates of a point at xyz=(z,y,z) (meters). \n
        Based on closed form solution of H. Vermeille, Journal of Geodesy (2002) 76:451-454. \n
        Handles simple list or tuples (xyz represents a single point) or a list of lists or tuples (xyz represents several points)"""

        a2 = reference_ellipsoid_params['a'] ** 2
        e4 = e2 ** 2
        # just to cast back to single list once done
        onePosition = False
        if isinstance(xyz[0], numbers.Real):
            xyz = [xyz]
            onePosition = True

        r_llh = [0] * 3
        d_llh = [[0] * 3 for i in range(len(xyz))]
        for i in range(len(xyz)):
            xy2 = xyz[i][0] ** 2 + xyz[i][1] ** 2
            p = (xy2) / a2
            q = (1. - e2) * xyz[i][2] ** 2 / a2
            r = (p + q - e4) / 6.
            s = e4 * p * q / (4. * r ** 3)
            t = (1. + s + math.sqrt(s * (2. + s))) ** (1. / 3.)
            u = r * (1. + t + 1. / t)
            v = math.sqrt(u ** 2 + e4 * q)
            w = e2 * (u + v - q) / (2. * v)
            k = math.sqrt(u + v + w ** 2) - w
            D = k * math.sqrt(xy2) / (k + e2)

            r_llh[0] = math.atan2(xyz[i][2], D)
            r_llh[1] = math.atan2(xyz[i][1], xyz[i][0])
            r_llh[2] = (k + e2 - 1.) * math.sqrt(D ** 2 + xyz[i][2] ** 2) / k

            d_llh[i][0] = math.degrees(r_llh[0])
            d_llh[i][1] = math.degrees(r_llh[1])
            d_llh[i][2] = r_llh[2]
        if onePosition:
            return d_llh[0]
        else:
            return d_llh

    @staticmethod
    def llh_to_xyz(llh):

        """llh_to_xyz(llh): returns (z,y,z) (meters) coordinates of a point given the point at \nllh=(lat (deg), lon (deg), h (meters)) for the instance ellipsoid\n
        Handles simple list or tuples (xyz represents a single point) or a list of lists or tuples (xyz represents several points)
        """

        # just to cast back to single list once done
        onePosition = False
        if isinstance(llh[0], numbers.Real):
            llh = [llh]
            onePosition = True

        r_v = [[0] * 3 for i in range(len(llh))]

        for i in range(len(llh)):
            r_lat = math.radians(llh[i][0])
            r_lon = math.radians(llh[i][1])
            hgt = llh[i][2]

            r_re = reference_ellipsoid_params['a'] / math.sqrt(1.0 - e2 * math.sin(r_lat) ** 2)

            r_v[i][0] = (r_re + hgt) * math.cos(r_lat) * math.cos(r_lon)
            r_v[i][1] = (r_re + hgt) * math.cos(r_lat) * math.sin(r_lon)
            r_v[i][2] = (r_re * (1.0 - e2) + hgt) * math.sin(r_lat)
        if onePosition:
            return r_v[0]
        else:
            return r_v

    def convertToDateTime(self, string):
        dt = datetime.datetime.strptime(string, "%Y-%m-%dT%H:%M:%S.%f")
        return dt

    def normal_radius_of_curvature(self, lat):
        """East Radius (eastRad): Normal radius of curvature (N), meters for
        latitude in degrees """
        return (
                reference_ellipsoid_params['a'] ** 2 /
                ((reference_ellipsoid_params['a'] * cosd(lat)) ** 2 + (reference_ellipsoid_params['b'] * sind(lat)) ** 2) ** 0.5
        )


    def LatLonHgt2XYZ(self, lat, lon, h):
        """LatLonHgt2XYZ(lat, lon, h) --> (x, y, z)

        lat       is the latitude (deg)
        lon       is the longitude (deg)
        h         is the heigh (m)

        (x, y, z) is a tuple of ECEF coordinates (m)
        """
        N = self.normal_radius_of_curvature(lat)
        cos_lat = cosd(lat)
        X = cos_lat * cosd(lon) * (N + h)
        Y = cos_lat * sind(lon) * (N + h)
        Z = sind(lat) * ((1 - e2) * N + h)
        return (X, Y, Z)

    def FindInfomationFromJson(self, HeaderFile_dom_json, node_path_list):
        """
        在Json文件中，按照指定路径解析出制定节点
        """
        result_node = HeaderFile_dom_json
        for nodename in node_path_list:
            result_node = result_node[nodename]
        return result_node

    def ReconstructionSatelliteOrbit(self, GPSPoints_list, starttime):
        if len(GPSPoints_list) == 2:
            self.polynum = 1
            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((self.polynum + 1, 6), dtype=np.float64)  # 四次项
            X = np.ones((record_count, self.polynum + 1), dtype=np.float64)  # 记录时间坐标
            # 将点记录转换为自变量矩阵、因变量矩阵

            for i in range(record_count):
                GPSPoint = GPSPoints_list[i]
                time_ = GPSPoint[0] - self.starttime  # 为了保证精度，对时间进行缩放
                X[i, :] = np.array([1, time_])
                state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6)  # 空间坐标
            self.model_f = []
            for i in range(6):
                Y = state_arr[:, i].reshape(-1, 1)
                A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]

            self.A_arr = copy.deepcopy(A_arr.copy())
            return self.A_arr
        elif len(GPSPoints_list) > 6:
            # 多项式的节点数，理论上是超过5个可以起算，这里为了精度选择10个点起算。
            # 多项式 XA=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((self.polynum, 6), dtype=np.float64)  # 四次项
            X = np.ones((record_count, self.polynum), dtype=np.float64)  # 记录时间坐标
            # 将点记录转换为自变量矩阵、因变量矩阵

            for i in range(record_count):
                GPSPoint = GPSPoints_list[i]
                time_ = GPSPoint[0] - self.starttime  # 为了保证精度，对时间进行缩放
                X[i, :] = np.array(list(map(lambda ii: time_ ** ii, range(self.polynum))))
                state_arr[i, :] = np.array(GPSPoint[1:], dtype=np.float64).reshape(1, 6)  # 空间坐标
            self.model_f = []
            for i in range(6):
                Y = state_arr[:, i].reshape(-1, 1)
                A_arr[:, i] = np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T, X)), X.T), Y)[:, 0]

            self.A_arr = A_arr.copy()
            return self.A_arr
        else:
            self.A_arr = None
            return None

    def getGPSModel(self, xml_path):
        GPSLists = []
        root = ElementTree(file=xml_path).getroot()
        with open(xml_path, 'r', encoding='utf-8') as fp:
            HeaderFile_dom_str = fp.read()
        HeaderFile_dom = xmltodict.parse(HeaderFile_dom_str)  # 将XML转成json文本
        HeaderFile_dom_json = json.loads(json.dumps(HeaderFile_dom))  # 将json字符串，转成json对象（对应于python中的dict）
        GPSNode = ['level1Product','platform','orbit', 'stateVec']
        GPSNodeNames = ['timeUTC', 'posX', 'posY', 'posZ', 'velX', 'velY', 'velZ']
        GPSTime = "%Y-%m-%dT%H:%M:%S.%f"
        GPSPoints = self.FindInfomationFromJson(HeaderFile_dom_json, GPSNode)
        dataStartTime = self.convertToDateTime(
            root.find("productInfo").find("sceneInfo").find("start").find("timeUTC").text).timestamp()
        dataStopTime = self.convertToDateTime(
            root.find("productInfo").find("sceneInfo").find("stop").find("timeUTC").text).timestamp()
        centerTime = (dataStopTime - dataStartTime) / 2 + dataStartTime

        for GPSPoint in GPSPoints:
            GPSPoint = [
                datetime.datetime.strptime(GPSPoint[GPSNodeNames[0]], GPSTime).timestamp(),  # TimeStamp
                float(GPSPoint[GPSNodeNames[1]]),  # Xp
                float(GPSPoint[GPSNodeNames[2]]),  # Yp
                float(GPSPoint[GPSNodeNames[3]]),  # Zp
                float(GPSPoint[GPSNodeNames[4]]),  # Vx
                float(GPSPoint[GPSNodeNames[5]]),  # Vy
                float(GPSPoint[GPSNodeNames[6]])]  # VZ
            GPSLists.append(GPSPoint)
        SatelliteOrbitModel = SatelliteOrbitFitPoly()
        if SatelliteOrbitModel.ReconstructionSatelliteOrbit(GPSLists, centerTime) is None:
            return None
        return SatelliteOrbitModel

    def calBaseline(self, slantRange_m, azimuthTime_m, azimuthTime_a, SatelliteOrbit_m, SatelliteOrbit_a, wvl, mas_xml, aux_xml):
        # start = datetime.datetime.now()
        aux_referenceTime = self.getReferenceTime(aux_xml)
        Bpar = np.zeros((len(azimuthTime_m), len(slantRange_m)), dtype=np.float32)
        Bperp = np.zeros((len(azimuthTime_m), len(slantRange_m)), dtype=np.float32)
        for ii, taz in enumerate(azimuthTime_m):
            referenceSV = SatelliteOrbit_m.getSatelliteSpaceState(taz)
            mxyz = np.array(referenceSV[0:3])
            mvel = np.array(referenceSV[3:6])
            for jj, rng in enumerate(slantRange_m):
                # start = datetime.datetime.now()
                llh = self.rdr2geo(referenceSV, taz, rng, wvl, SatelliteOrbit_m, azimuthTime_m, slantRange_m, mas_xml)

                tarxyz_temp = self.LatLonHgt2XYZ(llh[0], llh[1], llh[2])
                targxyz = np.array((tarxyz_temp[0], tarxyz_temp[1], tarxyz_temp[2]))

                slvTime, slvrng = self.geo2rdr(llh, azimuthTime_a, SatelliteOrbit_a, wvl, aux_referenceTime)

                secondarySV = SatelliteOrbit_a.getSatelliteSpaceState(slvTime)
                sxyz = np.array(secondarySV[0:3])
                aa = np.linalg.norm(sxyz - mxyz)
                costheta = (rng * rng + aa * aa - slvrng * slvrng) / (2. * rng * aa)

                Bpar[ii, jj] = aa * costheta

                perp = aa * np.sqrt(1 - costheta * costheta)
                direction = np.sign(np.dot(np.cross(targxyz - mxyz, sxyz - mxyz), mvel))
                Bperp[ii, jj] = direction * perp
        # end = datetime.datetime.now()
        # deffT = (end - start).total_seconds()
        # print('消耗时间为： ' + str(deffT))
        return Bpar, Bperp

    def rdr2geo(self, sv, taz, rng, wvl, SatelliteOrbit_m, azimuthTime, slantRange, mas_xml, height=0., side=-1):

        hdg = self.getENUHeading(SatelliteOrbit_m, taz)
        # sv = SatelliteOrbit_m.getSatelliteSpaceState(taz)
        pos = sv[0:3]
        vel = sv[3:6]
        # vmag = np.linalg.norm(vel)
        dopfact = 0.0
        # dopfact = self.getDoppler(taz, rng, azimuthTime, slantRange, mas_xml) * 0.5 * wvl * rng / vmag
        satLLH = self.xyz_to_llh(pos)
        self.setSCH(satLLH[0], satLLH[1], hdg)
        radius = self.pegRadCur

        satVec = np.array(pos)
        velVec = np.array(vel)

        ###Setup TCN basis
        clat = np.cos(np.radians(satLLH[0]))
        slat = np.sin(np.radians(satLLH[0]))
        clon = np.cos(np.radians(satLLH[1]))
        slon = np.sin(np.radians(satLLH[1]))
        nhat = np.array([-clat * clon, -clat * slon, -slat])
        temp = np.cross(nhat, velVec)
        chat = temp / np.linalg.norm(temp)
        temp = np.cross(chat, nhat)
        that = temp / np.linalg.norm(temp)
        vhat = velVec / np.linalg.norm(velVec)

        zsch = height

        # for ii in range(10):
            ###Near nadir tests
        if (satLLH[2] - zsch) >= rng:
            return None

        a = (satLLH[2] + radius)
        b = (radius + zsch)

        costheta = 0.5 * (a / rng + rng / a - (b / a) * (b / rng))
        sintheta = np.sqrt(1 - costheta * costheta)

        gamma = rng * costheta
        alpha = dopfact - gamma * np.dot(nhat, vhat) / np.dot(vhat, that)
        beta = -side * np.sqrt(rng * rng * sintheta * sintheta - alpha * alpha)

        delta = alpha * that + beta * chat + gamma * nhat

        targVec = satVec + delta

        targLLH = self.xyz_to_llh(list(targVec))
        # targXYZ = self.llh_to_xyz([targLLH[0], targLLH[1], height])
        # targSCH = self.xyz_to_sch(targXYZ)
        #
        # zsch = targSCH[2]
        #
        # rdiff = rng - np.linalg.norm(np.array(satVec) - np.array(targXYZ))

        return targLLH

    def geo2rdr(self, llh, azimuthTime_a, SatelliteOrbit_a, wvl, aux_referenceTime):
        xyz = self.llh_to_xyz(llh)
        tguess = azimuthTime_a[0]
        if wvl == 0:
            outOfBounds = False
            for ii in range(51):
                try:
                    sv = SatelliteOrbit_a.getSatelliteSpaceState(tguess)  # 获取卫星的 位置、速度
                except Exception as e:
                    print(e)
                    outOfBounds = True
                    break
                pos = np.array(sv[0:3])  # 卫星坐标
                vel = np.array(sv[3:6])  # 卫星速度
                dr = xyz - pos
                rng = np.linalg.norm(dr)  # 求斜距
                dopfact = np.dot(dr, vel)  # fd 公式
                fdop = 0  # doppler(DTU.seconds_since_midnight(tguess), rng) * wvl * 0.5
                fdopder = 0  # (0.5 * wvl * doppler(DTU.seconds_since_midnight(tguess), rng + 10.0) - fdop) / 10.0
                fn = dopfact - fdop * rng
                c1 = -np.dot(vel, vel)
                c2 = (fdop / rng + fdopder)
                fnprime = c1 + c2 * dopfact
                tguess = tguess - datetime.timedelta(seconds=fn / fnprime)
            if outOfBounds:
                raise Exception('Interpolation time out of bounds')
        else:
            dt = 0.0001
            outOfBounds = False
            for ii in range(51):
                try:
                    sv = SatelliteOrbit_a.getSatelliteSpaceState(tguess + datetime.timedelta(seconds=dt))  # 获取卫星的 位置、速度
                except Exception as e:
                    print(e)
                    outOfBounds = True
                    break

                pos1 = np.array(sv[0:3])  # 卫星坐标
                vel1 = np.array(sv[3:6])  # 卫星速度
                dr1 = pos1 - xyz
                rng1 = np.linalg.norm(dr1)  # 斜距

                FdTheory1 = -2 / (rng1 * wvl) * np.dot(dr1, vel1)
                try:
                    sv = SatelliteOrbit_a.getSatelliteSpaceState(tguess)
                except Exception as e:
                    print(e)
                    outOfBounds = True
                    break
                pos2 = np.array(sv[0:3])  # 卫星坐标
                vel2 = np.array(sv[3:6])  # 卫星速度
                dr2 = pos2 - xyz
                rng = np.linalg.norm(dr2)  # 斜距
                FdTheory2 = -2 / (rng * wvl) * np.dot(dr2, vel2)
                TSR = rng * 2 / LIGHTSPEED - aux_referenceTime  # nx1

                FdNumerical = 0

                fdopper_grad = (FdTheory1 - FdTheory2) / dt
                inc_t = (FdTheory2 - FdNumerical) / fdopper_grad
                tguess = tguess - datetime.timedelta(seconds=inc_t)
                if abs(inc_t) < 1e-6:
                    break
                else:
                    t_prev_guess = tguess
            if outOfBounds:
                raise Exception('Interpolation time out of bounds')
        return tguess, rng

    def getDoppler(self, taz, rng, azimuthTime, slantRange, xml_path):
        with open(xml_path, 'r', encoding='utf-8') as fp:
            HeaderFile_dom_str = fp.read()
        HeaderFile_dom = xmltodict.parse(HeaderFile_dom_str)  # 将XML转成json文本
        HeaderFile_dom_json = json.loads(json.dumps(HeaderFile_dom))
        DopplerRates = self.FindInfomationFromJson(HeaderFile_dom_json, ['level1Product','processing',"geometry", 'dopplerRate', 'dopplerRatePolynomial', 'coefficient'])
        dr = float(self.FindInfomationFromJson(HeaderFile_dom_json, ['level1Product','productInfo','imageDataInfo', 'imageRaster','columnSpacing']).get('#text'))
        DopplerRate = []
        for doppler in DopplerRates:
            DopplerRate.append(float(doppler.get('#text')))
        dcoeffs = []
        for ind, val in enumerate(DopplerRate):
            dcoeffs.append(val / (dr ** ind))
        coeffs = self.getCoeffs(dcoeffs)

        dopplerFat = 0.
        meanAzimuth = azimuthTime[0].timestamp()
        normAzimuth = azimuthTime[-1].timestamp() - meanAzimuth
        meanRange = slantRange[0]
        normRange = slantRange[-1]

        y = (taz - meanAzimuth) / normAzimuth
        x = (rng - meanRange) / normRange
        for ii, row in enumerate(coeffs):
            yfact = y ** ii
            for jj, col in enumerate(row):
                dopplerFat += coeffs[ii][jj] * yfact * (x**jj)
        return dopplerFat

    def getCoeffs(self, dcoeffs):
        coeffs = [[0. for i in j] for j in dcoeffs]
        for ii, row in enumerate(dcoeffs):
            for jj, col in enumerate(row):
                coeffs[ii][jj] = float(col)
        return coeffs

    def getReferenceTime(self, xml_path):
        root = ElementTree(file=xml_path).getroot()
        referenceTime = float(root.find("processing").find("geometry").find("dopplerRate").find("dopplerRatePolynomial").find("referencePoint").text)
        return referenceTime

    def getENUHeading(self,SatelliteOrbit, time):
        '''
        Compute heading at given azimuth time using single state vector.
        If time is not provided, mid point of orbit is used.
        '''

        vec1 = SatelliteOrbit.getSatelliteSpaceState(time)
        llh1 = self.xyz_to_llh(vec1[0:3])

        enumat = self.enubasis(llh1)
        venu = np.dot(enumat.xyz_to_enu, vec1[3:6])
        hdg = np.arctan2(venu[0, 0], venu[0, 1])
        return np.degrees(hdg)

    def enubasis(self, posLLH):
        """
        xyzenuMat = elp.enubasis(posLLH)
        Given an instance elp of an Ellipsoid LLH position (as a list) return the
        transformation matrices from the XYZ frame to the ENU frame and the
        inverse from the ENU frame to the XYZ frame.
        The returned object is a namedtuple with numpy matrices in elements
        named 'enu_to_xyz' and 'xyz_to_enu'
        enu_to_xyzMat = (elp.enubasis(posLLH)).enu_to_xyz
        xyz_to_enuMat = (elp.enubasis(posLLH)).xyz_to_enu
        """

        import numpy
        r_lat = numpy.radians(posLLH[0])
        r_lon = numpy.radians(posLLH[1])

        r_clt = numpy.cos(r_lat)
        r_slt = numpy.sin(r_lat)
        r_clo = numpy.cos(r_lon)
        r_slo = numpy.sin(r_lon)

        r_enu_to_xyzMat = numpy.matrix([
            [-r_slo, -r_slt * r_clo, r_clt * r_clo],
            [r_clo, -r_slt * r_slo, r_clt * r_slo],
            [0.0, r_clt, r_slt]])

        r_xyz_to_enuMat = r_enu_to_xyzMat.transpose()
        enuxyzMat = namedtuple("enuxyzMat", "enu_to_xyz  xyz_to_enu")
        res = enuxyzMat(r_enu_to_xyzMat, r_xyz_to_enuMat)
        return res

    def setSCH(self, pegLat, pegLon, pegHdg, pegHgt=0.0):
        """
        Set up an SCH coordinate system at the given peg point.
        Set a peg point on the ellipse at pegLat, pegLon, pegHdg (in degrees).
        Set the radius of curvature and the transformation matrix and offset
        vector needed to transform between (s,c,h) and ecef (x,y,z).
        """
        self.pegLat = pegLat
        self.pegLon = pegLon
        self.pegHdg = pegHdg
        self.pegHgt = pegHgt
        self.pegLLH = [pegLat, pegLon, pegHgt]

        # determine the radius of curvature at the peg point, i.e, the
        # the radius of the SCH sphere
        self.pegRadCur = self.radiusOfCurvature(self.pegLLH, pegHdg)

        # # determine the rotation matrix (from radar_to_xyz.F)
        # r_lat = np.radians(pegLat)
        # r_lon = np.radians(pegLon)
        # r_hdg = np.radians(pegHdg)
        # r_clt = np.cos(r_lat)
        # r_slt = np.sin(r_lat)
        # r_clo = np.cos(r_lon)
        # r_slo = np.sin(r_lon)
        # r_chg = np.cos(r_hdg)
        # r_shg = np.sin(r_hdg)
        #
        # ptm11 = r_clt * r_clo
        # ptm12 = -r_shg * r_slo - r_slt * r_clo * r_chg
        # ptm13 = r_slo * r_chg - r_slt * r_clo * r_shg
        # ptm21 = r_clt * r_slo
        # ptm22 = r_clo * r_shg - r_slt * r_slo * r_chg
        # ptm23 = -r_clo * r_chg - r_slt * r_slo * r_shg
        # ptm31 = r_slt
        # ptm32 = r_clt * r_chg
        # ptm33 = r_clt * r_shg
        #
        # self.pegRotMatNP = np.matrix(
        #     [[ptm11, ptm12, ptm13],
        #      [ptm21, ptm22, ptm23],
        #      [ptm31, ptm32, ptm33]]
        # )
        # self.pegRotMatInvNP = self.pegRotMatNP.transpose()
        #
        # self.pegRotMat = self.pegRotMatNP.tolist()
        # self.pegRotMatInv = self.pegRotMatInvNP.tolist()
        #
        # # find the translation vector as a column matrix
        # self.pegXYZ = self.llh_to_xyz(self.pegLLH)
        # self.pegXYZNP = np.matrix(self.pegXYZ).transpose()
        #
        # # Outward normal to ellispoid at the peg point
        # self.pegNormal = [r_clt * r_clo, r_clt * r_slo, r_slt]
        # self.pegNormalNP = np.matrix(self.pegNormal).transpose()
        #
        # # Offset Vector - to center of SCH sphere
        # self.pegOVNP = self.pegXYZNP - self.pegRadCur * self.pegNormalNP
        # self.pegOV = self.pegOVNP.transpose().tolist()[0]
        return

    def radiusOfCurvature(self, llh, hdg=0):
        """
        radiusOfCurvature(llh,[hdg]): returns Radius of Curvature (meters)
        in the direction specified by hdg for the instance ellipsoid
        given a position llh=(lat (deg), lon (deg), h (meters)).
        If no heading is given the default is 0, or North.
        """

        r_lat = math.radians(llh[0])
        r_hdg = math.radians(hdg)

        reast = self.eastRadiusOfCurvature(llh)
        rnorth = self.northRadiusOfCurvature(llh)

        # radius of curvature for point on ellipsoid
        rdir = (reast * rnorth) / (
                reast * math.cos(r_hdg) ** 2 + rnorth * math.sin(r_hdg) ** 2)

        # add height of the llh point
        return rdir + llh[2]

    def eastRadiusOfCurvature(self, llh):
        """eastRadiusOfCurvature(llh): returns Radius of Curvature (meters) \nin the East direction for the instance
        ellipsoid \ngiven a position llh=(lat (deg), lon (deg), h (meters))"""

        r_lat = math.radians(llh[0])

        reast = reference_ellipsoid_params['a'] / math.sqrt(1.0 - e2 * math.sin(r_lat) ** 2)
        return reast
    ##
    # Compute the radius of curvature in the north direction on an ellipsoid
    def northRadiusOfCurvature(self, llh):
        """northRadiusOfCurvature(llh): returns Radius of Curvature (meters) \nin the North direction for the instance
        ellipsoid \ngiven a position llh=(lat (deg), lon (deg), h (meters))"""

        r_lat = math.radians(llh[0])

        rnorth = (reference_ellipsoid_params['a'] * (1.0 - e2)) / (1.0 - e2 * math.sin(r_lat) ** 2) ** (1.5)
        return rnorth

    # 距离方程计算
    @staticmethod
    def compute_distance(slant_range, doppler_frequency):
        c = 3 * 10**8  # 光速
        distance = (c * doppler_frequency) / (2 * slant_range)
        return distance


    # 高度方程（假设直接投影到地球表面）
    @staticmethod
    def compute_height(slant_range, incident_angle, reference_ellipsoid_params):
        a = reference_ellipsoid_params['a']
        b = reference_ellipsoid_params['b']
        q = np.sqrt((slant_range**2))
        w = ((a**2) * (np.sin(incident_angle)**2))
        height = np.sqrt((slant_range**2) - ((a**2) * (np.sin(incident_angle)**2))) - b
        return height


    # 计算P点在参考椭球面上的等斜距投影点P0的坐标
    @staticmethod
    def compute_P0_coordinates(latitude_P, longitude_P, height_P0):
        a = reference_ellipsoid_params['a']
        e_squared = 1 - ((reference_ellipsoid_params['b']**2) / (reference_ellipsoid_params['a']**2))

        N = a / math.sqrt(1 - e_squared * math.sin(math.radians(latitude_P))**2)
        X = (N + height_P0) * math.cos(math.radians(latitude_P)) * math.cos(math.radians(longitude_P))
        Y = (N + height_P0) * math.cos(math.radians(latitude_P)) * math.sin(math.radians(longitude_P))
        Z = (N * (1 - e_squared) + height_P0) * math.sin(math.radians(latitude_P))
        return X, Y, Z


if __name__ == '__main__':
    sGps = SARgps()