#!/usr/bin/env python3

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Copyright 2010 California Institute of Technology. ALL RIGHTS RESERVED.
# 
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
# 
# http://www.apache.org/licenses/LICENSE-2.0
# 
# LT1AB 重复轨道模式
#
# Author: Chenzenghui
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~




from xml.etree.ElementTree import ElementTree
import datetime
import isceobj
from isceobj.Util import Poly1D
from isceobj.Scene.Frame import Frame
from isceobj.Planet.Planet import Planet
from isceobj.Orbit.Orbit import StateVector, Orbit
from isceobj.Orbit.OrbitExtender import OrbitExtender
from isceobj.Planet.AstronomicalHandbook import Const
from iscesys.Component.Component import Component
from iscesys.DateTimeUtil.DateTimeUtil import DateTimeUtil as DTUtil
import os
import numpy as np
import math
import datetime
import time
from math import sin,cos
from scipy.optimize import leastsq
import numpy as np
from isceobj.Util.Poly2D import Poly2D 



sep = "\n"
tab = "    "
lookMap = { 'RIGHT' : -1,
            'LEFT' : 1}

TIFF = Component.Parameter(
    'tiff',
    public_name='TIFF',
    default='',
    type=str,
    mandatory=True,
    doc='RadarSAT2 tiff imagery file'
)

XML = Component.Parameter(
    'xml',
    public_name='XML',
    default='',
    type=str,
    mandatory=True,
    doc='RadarSAT2 xml metadata file'
)

ORBIT_DIRECTORY = Component.Parameter(
        'orbitDirectory',
        public_name = 'orbit directory',
        default=None,
        type=str,
        mandatory=False,
        doc='Directory with Radarsat2 precise orbits')

ORBIT_FILE = Component.Parameter(
        'orbitFile',
        public_name = 'orbit file',
        default = None,
        type = str,
        mandatory = False,
        doc = 'Precise orbit file to use')

from .Sensor import Sensor


#######################################################################################################
# 增加轨道求解模型，为了方便轨道模型的求解，这里对轨道模型进行修改
# 这里采用《Insar原理和应用 》（刘国祥著）中基于空间定位的轨道参数计算方法一章中的内容（Gabriel and Goldstein）
# 注意为了方便计算，采用基准时间的方法
######################################################

def FindInfomationFromJson(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 GetVectorNorm(Vecter):
    """
    得到向量的模
    """
    Vecter = Vecter.reshape(-1,1)
    Vecter_Norm_pow = np.matmul(Vecter.T,Vecter)
    return np.sqrt(Vecter_Norm_pow)


def XYZOuterM2(A, B):
    """
    外积（叉乘）,日后版本换成可以任意维度的外积运算方程
    args:
        A:nx3
        B:nx3
    """
    cnt = A.shape[0]
    C = np.zeros((cnt, 3))
    C[:, 0] = A[:, 1] * B[:, 2] - A[:, 2] * B[:, 1]
    C[:, 1] = A[:, 2] * B[:, 0] - A[:, 0] * B[:, 2]
    C[:, 2] = A[:, 0] * B[:, 1] - A[:, 1] * B[:, 0]
    return C


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:
            # 多项式的节点数，理论上是超过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 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
 
        # 
        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
         
        return [time_float,[px,py,pz,vx,vy,vz]]

    def getTimeOrbitStamp(self,UTCStartTime_float):
        sv=_StateVector()
        temp_sv=self.SatelliteSpaceState(UTCStartTime_float)
        sv.timeStamp=datetime.datetime.fromtimestamp(UTCStartTime_float)
        sv.xPosition = temp_sv[1][0,0]
        sv.yPosition = temp_sv[1][0,1]
        sv.zPosition = temp_sv[1][0,2]
        sv.xVelocity = temp_sv[1][0,3]
        sv.yVelocity = temp_sv[1][0,4]
        sv.zVelocity = temp_sv[1][0,5]        
        return sv
    
    def getTimeOrbits(self,UTCStartTime,UTCEndTime,orbitnum=1000):
        # 
        startTime_stamp=datetime.datetime.strptime(UTCStartTime,"%Y-%m-%dT%H:%M:%S.%f").timestamp()-1
        endTime_stamp=datetime.datetime.strptime(UTCEndTime,"%Y-%m-%dT%H:%M:%S.%f").timestamp()+1
        if startTime_stamp>endTime_stamp:
            raise
        delta_t=(endTime_stamp-startTime_stamp)/orbitnum
        extractOrbits=[]
        # 
        temptime=startTime_stamp
        while temptime<endTime_stamp:
            temptime=temptime+delta_t
            newOrbit=self.getTimeOrbitStamp(temptime)
            extractOrbits.append(newOrbit)
        newOrbit=self.getTimeOrbitStamp(endTime_stamp)
        extractOrbits.append(newOrbit)
        return extractOrbits # 扩展的轨道节点

def ReconstructionSatelliteOrbit(stateVectors, starttime):
    '''
    构建卫星轨道
    args:
        GPSPoints_list:卫星轨道点
        starttime:起算时间
    '''
    GPSPoint_list=[]
    for sv in stateVectors:
        GPSPoint=[sv.timeStamp.timestamp(),
                      sv.xPosition,
                      sv.yPosition,
                      sv.zPosition,
                      sv.xVelocity,
                      sv.yVelocity,
                      sv.zVelocity
                      ]
        GPSPoint_list.append(GPSPoint)  
    SatelliteOrbitModel = SatelliteOrbitFitPoly()
    if SatelliteOrbitModel.ReconstructionSatelliteOrbit(GPSPoint_list, starttime=starttime) is None:
        return None
    
    
    
    print("orbit test")
    distance=[]
    for gpsPoint in GPSPoint_list:
        temp_sv=SatelliteOrbitModel.SatelliteSpaceState(gpsPoint[0])
        sv=np.array(temp_sv[1])
        temp_distance=sv-gpsPoint[1:]
        distance.append(temp_distance)
    distance=np.array(distance)
    print("orbit max:",np.max(distance))
    print("orbit min:",np.min(distance))
    return SatelliteOrbitModel

########
# 函数列表
########
def poly1dfunc(p,x): # 一次函数
    k,b=p
    return k*x+b

def poly2dfunc(p,x): # 二次函数
    k1,k2,b=p
    return b+k1*x+k2*x*x

def poly3dfunc(p,x):
    k1,k2,k3,b=p
    return b+k1*x+k2*x*x+k3*x*x*x

def poly4dfunc(p,x):
    k1,k2,k3,k4,b=p
    return b+k1*x+k2*x**2+k3*x**3+k4*x**4

def poly5dfunc(p,x):
    k1,k2,k3,k4,k5,b=p
    return b+k1*x+k2*x**2+k3*x**3+k4*x**4+k5*x**5

def poly1derror(p,x,y):
    return poly1dfunc(p,x)-y

def poly2derror(p,x,y):
    return poly2dfunc(p,x)-y

def poly3derror(p,x,y):
    return poly3dfunc(p,x)-y

def poly4derror(p,x,y):
    return poly4dfunc(p,x)-y

def poly5derror(p,x,y):
    return poly5dfunc(p,x)-y

class orbitVector:
    def __init__(self,UTCTimes,vx,vy,vz,px,py,pz,dateformat="%Y-%m-%dT%H:%M:%S.%fZ"):
        self.UTCTime=datetime.datetime.strptime(UTCTimes,dateformat) # 字符串转UTC时间
        self.time_stamp=self.UTCTime.timestamp()  # 时间戳
        self.vx=vx # Vx
        self.vy=vy
        self.vz=vz
        self.px=px
        self.py=py # Ys
        self.pz=pz
        self.orbitVector2GG() # 将坐标进行变换
        self.Check()
        pass

    def orbitVector2GG(self):
        Cx=self.py*self.vz-self.pz*self.vy
        Cy=self.pz*self.vx-self.px*self.vz
        Cz=self.px*self.vy-self.py*self.vx
        C=(Cx**2+Cy**2+Cz**2)**0.5
        self.rho=(self.px**2+self.py**2+self.pz**2)**0.5
        self.I=math.acos(Cz/C)
        self.omega=math.asin(self.pz/(self.rho*math.sin(self.I)))
        self.Omega=math.atan(-Cx/Cy)
        return [self.rho,self.I,self.omega,self.Omega]
    
    def Check(self):
        rho=self.rho
        I=self.I
        omega=self.omega
        Omega=self.Omega
        Xp=rho*(cos(omega)*cos(Omega)-sin(omega)*sin(Omega)*cos(I))
        Yp=rho*(cos(omega)*sin(Omega)+sin(omega)*cos(Omega)*cos(I))
        Zp=rho*sin(Omega)*sin(I)
        print("计算插值:",self.UTCTime,self.px,self.py,self.pz,self.vx,self.vy,self.vz,"|",Xp-self.px,Yp-self.py,Zp-self.pz)

    
class SARorbit(object):
    # 作为自定义轨道计算基类
    # 定义几个基本方法：addvector(),createOrbit(),getTime(),getTimes()
    # 注意均为UTC
    def __init__(self):
        self.vectors=[]
        self.baseTime=None
    def addvector(self,UTCTime,vx,vy,vz,px,py,pz):
        neworbit_point=orbitVector(UTCTime,vx,vy,vz,px,py,pz)
        self.vectors.append(neworbit_point)
        if len(self.vectors)==1:
            self.baseTime=neworbit_point.time_stamp-1 # 基线
        else:
            if self.baseTime>neworbit_point.time_stamp:
                self.baseTime=neworbit_point.time_stamp-1
            
    def createOrbit(self):
        pass

    def getTimeOrbit(self,UTCTime):
        return None

    def getTimeOrbitStamp(self,StampTime):
        utcStr=datetime.datetime.fromtimestamp(StampTime).strftime("%Y-%m-%dT%H:%M:%S.%fZ")
        return self.getTimeOrbit(utcStr)

    def getTimeOrbits(self,UTCStartTime,UTCEndTime,orbitnum=100):
        # 
        startTime_stamp=datetime.datetime.strptime(UTCStartTime,"%Y-%m-%dT%H:%M:%S.%fZ").timestamp()-10
        endTime_stamp=datetime.datetime.strptime(UTCEndTime,"%Y-%m-%dT%H:%M:%S.%fZ").timestamp()+10
        if startTime_stamp>endTime_stamp:
            raise
        delta_t=(endTime_stamp-startTime_stamp)*1000/orbitnum
        delta_t=int(delta_t)/1000 # 获取
        extractOrbits=[]
        # 
        temptime=startTime_stamp
        while temptime<endTime_stamp:
            temptime=temptime+delta_t
            newOrbit=self.getTimeOrbitStamp(temptime)
            extractOrbits.append(newOrbit)
        newOrbit=self.getTimeOrbitStamp(endTime_stamp)
        extractOrbits.append(newOrbit)
        return extractOrbits # 扩展的轨道节点
        pass

class polyorbit(SARorbit):
    def __init__(self):
        super(polyorbit,self).__init__()

    def createOrbit(self):
        if len(self.vectors)<8:
            raise
        ts,xs,ys,zs,vx,vy,vz=[],[],[],[],[],[],[]
        for orbit_point in self.vectors:
            ts.append(orbit_point.time_stamp-self.baseTime)
            xs.append(orbit_point.px)
            ys.append(orbit_point.py)
            zs.append(orbit_point.pz)
            vx.append(orbit_point.vx)
            vy.append(orbit_point.vy)
            vz.append(orbit_point.vz)
        ts=np.array(ts)
        xs=np.array(xs)
        ys=np.array(ys)
        zs=np.array(zs)
        vx=np.array(vx)
        vy=np.array(vy)
        vz=np.array(vz)
        self.p_xs=leastsq(poly5derror,[100,100,100,100,100,100],args=(ts,xs))
        self.p_ys=leastsq(poly5derror,[100,100,100,100,100,100],args=(ts,ys))
        self.p_zs=leastsq(poly5derror,[100,100,100,100,100,100],args=(ts,zs))
        self.p_vxs=leastsq(poly5derror,[100,100,100,100,100,100],args=(ts,vx))
        self.p_vy=leastsq(poly5derror,[100,100,100,100,100,100],args=(ts,vy))
        self.p_vz=leastsq(poly5derror,[100,100,100,100,100,100],args=(ts,vz))
        return 
    def getTimeOrbit(self,UTCTime):
        staptime=datetime.datetime.strptime(UTCTime,"%Y-%m-%dT%H:%M:%S.%fZ").timestamp()-self.baseTime
        xs=poly5dfunc(self.p_xs,staptime)
        ys=poly5dfunc(self.p_ys,staptime)
        zs=poly5dfunc(self.p_zs,staptime)
        vx=poly5dfunc(self.p_vx,staptime)
        vy=poly5dfunc(self.p_vy,staptime)
        vz=poly5dfunc(self.p_vz,staptime)
        utmstr=datetime.datetime.strptime(UTCTime,"%Y-%m-%dT%H:%M:%S.%fZ").strftime("%Y-%m-%d %H:%M:%S.%f")
        neworbit_point=orbitVector(utmstr,vx,vy,vz,xs,ys,zs,dateformat="%Y-%m-%d %H:%M:%S.%f")
        return neworbit_point

#######################################################################################################

class LT1ABLT1ABREPEAT(Sensor):
    """
        A Class representing RADARSAT 2 data
    """

    family='LT1ABLT1ABREPEAT'
    parameter_list = (XML, TIFF, ORBIT_DIRECTORY, ORBIT_FILE ) + Sensor.parameter_list

    def __init__(self, family='', name=''):
        super().__init__(family if family else  self.__class__.family, name=name)
        self.product = _Product()
        self.frame = Frame()
        self.frame.configure()
        

    def getFrame(self):
        return self.frame

    def parse(self):
        try:
            fp = open(self.xml,'r')
        except IOError as strerr:
            print("IOError: %s" % strerr)
            return
        self._xml_root = ElementTree(file=fp).getroot()
        self.product.set_from_etnode(self._xml_root) # 解析meta xml
        self.populateMetadata()
        self.extractDoppler()
        fp.close()

    def populateMetadata(self):
        """
            Create metadata objects from the metadata files
        """
        mission = self.product.mission
        swath = self.product.swath
        frequency = self.product.frequency
        orig_prf =self.product.orig_prf
        rangePixelSize = self.product.rangePixelSize
        rangeSamplingRate = Const.c/(2*rangePixelSize)
        pulseLength =self.product.pulseLength
        pulseBandwidth = self.product.pulseBandwidth
        polarization = self.product.polarization
        lookSide = self.product.lookSide
        facility = self.product.facility
        version = self.product.version
        lines = self.product.lines
        samples = self.product.samples
        startingRange = self.product.startingRange
        incidenceAngle = self.product.incidenceAngle
        
        
        # some RS2 scenes have oversampled SLC images because processed azimuth bandwidth larger than PRF EJF 2015/08/15
        azimuthPixelSize = self.product.azimuthPixelSize  # ground spacing in meters
        totalProcessedAzimuthBandwidth = self.product.totalProcessedAzimuthBandwidth
        prf = orig_prf * np.ceil(totalProcessedAzimuthBandwidth / orig_prf) # effective PRF can be double original, suggested by Piyush
        print('-------------------------------------')
        print("mission",mission)
        print("swath",swath)
        print("frequency",frequency)
        print("orig_prf",orig_prf)
        print("prf",prf)
        print("rangePixelSize",rangePixelSize)
        print("rangeSamplingRate",rangeSamplingRate)
        print("pulseLength",pulseLength)
        print("pulseBandwidth",pulseBandwidth)
        print("polarization",polarization)
        print("lookSide",lookSide)
        print("lines",lines)
        print("samples",samples)
        print("startingRange",startingRange)
        print("incidenceAngle",incidenceAngle)
        print(self.product.imageGenerationParameters.dopplerCentroid.dopplerCentroidCoefficients)
        print(self.product.imageGenerationParameters.dopplerRateValues.dopplerRateValuesCoefficients)
        print("effective PRF %f, original PRF %f" % (prf, orig_prf) )
        print('-------------------------------------------')
        

        #lineFlip =  (self.product.imageAttributes.rasterAttributes.lineTimeOrdering.upper() == 'DECREASING')
        dataStartTime = self.product.dataStartTime
        dataStopTime = self.product.dataStopTime

        passDirection = self.product.passDirection
        
        
        ####Populate platform
        platform = self.frame.getInstrument().getPlatform()
        platform.setPlanet(Planet(pname="Earth"))
        platform.setMission(mission)
        platform.setPointingDirection(lookSide)
        platform.setAntennaLength(15.0)

        ####Populate instrument
        instrument = self.frame.getInstrument()
        instrument.setRadarFrequency(frequency)
        instrument.setPulseRepetitionFrequency(prf)
        instrument.setPulseLength(pulseLength)
        instrument.setChirpSlope(pulseBandwidth/pulseLength)
        instrument.setIncidenceAngle(incidenceAngle)
        #self.frame.getInstrument().setRangeBias(0)
        instrument.setRangePixelSize(rangePixelSize)
        instrument.setRangeSamplingRate(rangeSamplingRate)
        instrument.setBeamNumber(swath)
        instrument.setPulseLength(pulseLength)


        #Populate Frame
        #self.frame.setSatelliteHeight(height)
        self.frame.setSensingStart(dataStartTime)
        self.frame.setSensingStop(dataStopTime)
        diffTime = DTUtil.timeDeltaToSeconds(dataStopTime - dataStartTime)/2.0
        sensingMid = dataStartTime + datetime.timedelta(microseconds=int(diffTime*1e6))
        self.frame.setSensingMid(sensingMid)
        self.frame.setPassDirection(passDirection)
        self.frame.setPolarization(polarization)
        self.frame.setStartingRange(startingRange)
        self.frame.setFarRange(startingRange + (samples-1)*rangePixelSize)
        self.frame.setNumberOfLines(lines)
        self.frame.setNumberOfSamples(samples)
        self.frame.setProcessingFacility("LAMP") # 处理软件
        self.frame.setProcessingSoftwareVersion(version)
        self.frame.setPassDirection(passDirection)
        self.frame.setProcessingSystem("LT1AB") # 陆探数据
        self.frame.getOrbit().setOrbitSource("LT1AB.META.XML")
        
        
        print("=======  修改轨道代码部分：  =================")
        if (self.orbitFile is None) and (self.orbitDirectory is None):
            self.extractOrbit()
            
            
        elif (self.orbitDirectory is not None):
            self.orbitFile = findPreciseOrbit(self.orbitDirectory, self.frame.getOrbit().getOrbitSource(), self.frame.sensingStart.year)

        if self.orbitFile is not None:
            self.extractPreciseOrbit(self.orbitFile, self.frame.sensingStart, self.frame.sensingStop)
        

        # save the Doppler centroid coefficients, converting units from product.xml file
        # units in the file are quadratic coefficients in Hz, Hz/sec, and Hz/(sec^2)
        # ISCE expects Hz, Hz/(range sample), Hz((range sample)^2
        # note that RS2 Doppler values are estimated at time dc.dopplerCentroidReferenceTime,
        # so the values might need to be adjusted for ISCE usage
        # added EJF 2015/08/17
        dc = self.product.dopplerCentroid
        poly = dc.dopplerCentroidCoefficients
        # need to convert units
        poly[1] = poly[1]/rangeSamplingRate
        poly[2] = poly[2]/rangeSamplingRate**2
        self.doppler_coeff = poly

        # similarly save Doppler azimuth fm rate values, converting units
        # units in the file are quadratic coefficients in Hz, Hz/sec, and Hz/(sec^2)
        # Guessing that ISCE expects Hz, Hz/(range sample), Hz((range sample)^2
        # note that RS2 Doppler values are estimated at time dc.dopplerRateReferenceTime,
        # so the values might need to be adjusted for ISCE usage
        # added EJF 2015/08/17
        dr = self.product.dopplerRateValues
        fmpoly = dr.dopplerRateValuesCoefficients
        # need to convert units
        fmpoly[1] = fmpoly[1]/rangeSamplingRate
        fmpoly[2] = fmpoly[2]/rangeSamplingRate**2
        self.azfmrate_coeff = fmpoly

        # now calculate effective PRF from the azimuth line spacing after we have the orbit info EJF 2015/08/15
        # this does not work because azimuth spacing is on ground. Instead use bandwidth ratio calculated above  EJF
#        SCHvelocity = self.frame.getSchVelocity()
#        SCHvelocity = 7550.75  # hard code orbit velocity for now m/s
#        prf = SCHvelocity/azimuthPixelSize
#        instrument.setPulseRepetitionFrequency(prf)

    def extractOrbit(self):
        '''
        Extract the orbit state vectors from the XML file.
        '''

        # Initialize orbit objects
        # Read into temp orbit first.frame
        # Radarsat 2 needs orbit extensions.
        print("构建新的轨道程序代码：")
        

        self.frame.getOrbit().setOrbitSource('Header: ' + self.frame.getOrbit().getOrbitSource())
        stateVectors = self.product.OrbitModelInfo.stateVectors

        
        tempOrbit= ReconstructionSatelliteOrbit(stateVectors,self.product.orbitstarttime.timestamp())
        As_arr=tempOrbit.A_arr.tolist()
        """ 构建轨道 """
        self.frame.getOrbit().setsessionMode(self.product.mission)
        print("卫星型号")
        print(self.frame.orbit.sessionMode)
        self.frame.getOrbit().setPolyParams(tempOrbit.polynum,tempOrbit.starttime,As_arr)
        self.frame.getOrbit().setDoppler(self.product.dopplerRateValues.dopplerRateReferenceTime,
                                          self.product.dopplerRateValues.dopplerRateValuesCoefficients)
        """ """
        newOrb=self.frame.getOrbit().getTimeOrbits(self.frame.sensingStart.strftime("%Y-%m-%dT%H:%M:%S.%f"), 
                                        self.frame.sensingStop.strftime("%Y-%m-%dT%H:%M:%S.%f"),
                                        orbitnum=1000)
        for svect in newOrb:
            self.frame.getOrbit().addStateVector(svect)

        print('Successfully read state vectors from product XML')

    def extractPreciseOrbit(self, orbitfile, tstart, tend):
        '''
        Extract precise orbits for given time-period from orbit file.
        '''

        self.frame.getOrbit().setOrbitSource('File: ' + orbitfile)

        tmin = tstart - datetime.timedelta(seconds=30.)
        tmax = tstart + datetime.timedelta(seconds=30.)

        fid = open(orbitfile, 'r')
        for line in fid:
            if not line.startswith('; Position'):
                continue
            else:
                break

        for line in fid:
            if not line.startswith(';###END'):
                tstamp = convertRSTimeToDateTime(line)
                
                if (tstamp >= tmin) and (tstamp <= tmax):
                    sv = StateVector()
                    sv.configure()
                    sv.setTime( tstamp)
                    sv.setPosition( [float(x) for x in fid.readline().split()])
                    sv.setVelocity( [float(x) for x in fid.readline().split()])

                    self.frame.getOrbit().addStateVector(sv)
                else:
                    fid.readline()
                    fid.readline()

                dummy = fid.readline()
                if not dummy.startswith(';'):
                    raise Exception('Expected line to start with ";". Got {0}'.format(dummy))

        fid.close()
        print('Successfully read {0} state vectors from {1}'.format( len(self.frame.getOrbit()._stateVectors), orbitfile))

    def extractImage(self, verbose=True):
        '''
        Use gdal to extract the slc.
        '''

        try:
            from osgeo import gdal
        except ImportError:
            raise Exception('GDAL python bindings not found. Need this for RSAT2 / TandemX / Sentinel1A.')

        self.parse()

        width = self.frame.getNumberOfSamples()
        lgth  = self.frame.getNumberOfLines()
        lineFlip = False #(self.product.imageAttributes.rasterAttributes.lineTimeOrdering.upper() == 'DECREASING')
        pixFlip = False #(self.product.imageAttributes.rasterAttributes.pixelTimeOrdering.upper() == 'DECREASING')

        src = gdal.Open(self.tiff.strip(), gdal.GA_ReadOnly)
        cJ = np.complex64(1.0j)

        ####Images are small enough that we can do it all in one go - Piyush
        real = src.GetRasterBand(1).ReadAsArray(0,0,width,lgth)
        imag = src.GetRasterBand(2).ReadAsArray(0,0,width,lgth)

        if (real is None) or (imag is None):
            raise Exception('Input Radarsat2 SLC seems to not be a 2 band Int16 image.')

        data = real+cJ*imag

        real = None
        imag = None
        src = None

        if lineFlip:
            if verbose:
                print('Vertically Flipping data')
            data = np.flipud(data)

        if pixFlip:
            if verbose:
                print('Horizontally Flipping data')
            data = np.fliplr(data)

        data.tofile(self.output)

        ####
        slcImage = isceobj.createSlcImage()
        slcImage.setByteOrder('l')
        slcImage.setFilename(self.output)
        slcImage.setAccessMode('read')
        slcImage.setWidth(width)
        slcImage.setLength(lgth)
        slcImage.setXmin(0)
        slcImage.setXmax(width)
        #slcImage.renderHdr()
        self.frame.setImage(slcImage)


    def extractDoppler(self):
        '''
        self.parse()
        Extract doppler information as needed by mocomp
        '''
        ins = self.frame.getInstrument()
        dc = self.product.dopplerCentroid
        quadratic = {}

        r0 = self.frame.startingRange
        fs = ins.getRangeSamplingRate()
        tNear = 2*r0/Const.c

        tMid = tNear + 0.5*self.frame.getNumberOfSamples()/fs
        t0 = dc.dopplerCentroidReferenceTime
        poly = dc.dopplerCentroidCoefficients

        fd_mid = 0.0
        for kk in range(len(poly)):
            fd_mid += poly[kk] * (tMid - t0)**kk

        ####For insarApp
        quadratic['a'] = fd_mid / ins.getPulseRepetitionFrequency()
        quadratic['b'] = 0.
        quadratic['c'] = 0.


        ####For roiApp
        ####More accurate


        coeffs = poly
        dr = self.frame.getInstrument().getRangePixelSize()
        rref = 0.5 * Const.c * t0
        r0 = self.frame.getStartingRange()
        norm = 0.5*Const.c/dr

        dcoeffs = []
        for ind, val in enumerate(coeffs):
            dcoeffs.append( val / (norm**ind))

        poly = Poly1D.Poly1D()
        poly.initPoly(order=len(coeffs)-1)
        poly.setMean( (rref - r0)/dr - 1.0)
        poly.setCoeffs(dcoeffs)

        pix = np.linspace(0, self.frame.getNumberOfSamples(), num=len(coeffs)+1)
        evals = poly(pix)
        fit = np.polyfit(pix,evals, len(coeffs)-1)
        self.frame._dopplerVsPixel = list(fit[::-1])
        self.frame._dopplerVsPixel_LT1AB = list([self.product.dopplerRateValues.dopplerRateReferenceTime]
                                          +self.product.dopplerRateValues.dopplerRateValuesCoefficients)
        
        
        print('Doppler Fit: ', fit[::-1])


        # print("---- radar ------------------------------------------print")
        # print("****" * 10)
        # print("t0",t0)
        # print('lightspeed',Const.c)
        # print('rref',rref)
        # print('dr',dr)
        # print('dcoeff',dcoeffs)
        # print('r0',r0)
        # print("****" * 10)
        # print('pix',pix)
        # print('evals',evals)
        # print('fit',fit)
        # print('Doppler Fit: ', fit[::-1])
        # print('---------------------------------------------------')

        # """ 测试 多普勒"""
        # print("=======  Test LTInSAR =========")
        # """ 测试 轨道情况"""
        
        
        # coeff = self.frame._dopplerVsPixel_LT1AB
        # doppler = Poly2D()
        # doppler._meanRange = self.frame.startingRange
        # doppler._normRange = self.frame.instrument.rangePixelSize
        # doppler.initPoly(azimuthOrder=0, rangeOrder=len(coeff)-1, coeffs=[coeff])
        
        # lookSide = self.frame.instrument.platform.pointingDirection
        # planet = Planet(pname='Earth')
        # wvl = self.frame.instrument.getRadarWavelength()
        # test_lla=[47.75,130.82,159.50]
        # taz, rgm = self.frame.orbit.geo2rdr(test_lla, side=lookSide, doppler=doppler, wvl=wvl)
        
        # print("taz",taz)
        # print('n11',datetime.datetime.strptime("2023-03-27T21:28:27.551796","%Y-%m-%dT%H:%M:%S.%f"))
        # print("rgm",rgm)
        # print('r11',4.96786423292669768E-03*Const.c/2)
        
        # print("lon,lat,ati",test_lla)
        # print("==============================================")        
        return quadratic

class LT1ABNamespace(object):
    def __init__(self):
        self.uri = ""

    def elementName(self,element):
        return element

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

class _Product(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.productId = None
        self.documentId = None
        self.mission=None
        self.swath=None
        self.frequency=None
        self.orig_prf=None # 原始PRF --- 参考PRF
        self.rangePixelSize=None # 距离向分辨率
        self.pulseLength=None
        self.pulseBandwidth=None
        self.polarization=None
        self.lookSide=None
        self.facility=None
        self.version=None
        self.lines=None # 行数
        self.samples=None  # 列数   
        self.startingRange=None     
        self.incidenceAngle=None      
        self.azimuthPixelSize=None   
        self.totalProcessedAzimuthBandwidth=None      
        self.prf=None      
        
        """
        入射角
        """
        self.topRight_IncidenceAngle=None
        self.topLeft_IncidenceAngle=None
        self.bottomRight_IncidenceAngle=None
        self.bottomLeft_IncidenceAngle=None
        
        self.Near_incidenceAngle=None
        self.Far_incidenceAngle=None
        
        """ 斜距 """
        self.slantRangeTimeToFirstRangeSample=None
        self.startingRange=None
        
        """ 轨道"""
        self.passDirection=None
        
        """ 成像 """
        self.dataStartTime=None
        self.dataStopTime=None
        self.orbitstarttime=None
        
        """ 多普勒系数 """
        self.dopplerCentroid=_DopplerCentroid()
        self.dopplerRateValues = _DopplerRateValues()
        
        
        
        self.OrbitModelInfo=_OrbitInformation()
        self.sourceAttributes = _SourceAttributes()
        self.imageGenerationParameters = _ImageGenerationParameters()
        self.imageAttributes = _ImageAttributes()

    def set_from_etnode(self,node):
        self.productId=node.find("productInfo").find("generationInfo").find("logicalProductID").text
        self.documentId=node.find("generalHeader").find("referenceDocument").text
        self.mission=node.find("generalHeader").find("mission").text
        self.swath=node.find("productInfo").find("acquisitionInfo").find("imagingMode").text
        self.frequency=float(node.find("processing").find("processingParameter").find("rangeCompression").find("chirps").find("referenceChirp").find("centerFrequency").text)
        
        self.rangePixelSize=float(node.find("productInfo").find("imageDataInfo").find("imageRaster").find("columnSpacing").text)
        self.pulseLength=float(node.find("processing").find("processingParameter").find("rangeCompression").find("chirps").find("referenceChirp").find("pulseLength").text)
        self.pulseBandwidth=float(node.find("processing").find("processingParameter").find("rangeCompression").find("chirps").find("referenceChirp").find("pulseBandwidth").text)
        self.polarization=node.find("productInfo").find("acquisitionInfo").find("polarisationMode").text
        self.lookSide=lookMap[node.find("productInfo").find("acquisitionInfo").find("lookDirection").text.upper()]
        self.facility=node.find("productInfo").find("generationInfo").find("level1ProcessingFacility").text
        self.version="V1.1"
        self.lines=int(node.find("productInfo").find("imageDataInfo").find("imageRaster").find("numberOfRows").text)
        self.samples=int(node.find("productInfo").find("imageDataInfo").find("imageRaster").find("numberOfColumns").text)
        sceneCorners=node.find("productInfo").find("sceneInfo").findall("sceneCornerCoord")
        for sceneCorner in sceneCorners:
            incidenceAngle_temp=float(sceneCorner.find("incidenceAngle").text)
            if sceneCorner.attrib["name"]=="topRight":
                self.topRight_IncidenceAngle=incidenceAngle_temp
            elif sceneCorner.attrib["name"]=="topLeft":
                self.topLeft_IncidenceAngle=incidenceAngle_temp
            elif sceneCorner.attrib["name"]=="bottomRight":
                self.bottomRight_IncidenceAngle=incidenceAngle_temp            
            elif sceneCorner.attrib["name"]=="bottomLeft":
                self.bottomLeft_IncidenceAngle=incidenceAngle_temp      
        self.Near_incidenceAngle=self.topRight_IncidenceAngle
        self.Near_incidenceAngle=self.Near_incidenceAngle   if self.Near_incidenceAngle < self.topLeft_IncidenceAngle else self.topLeft_IncidenceAngle
        self.Near_incidenceAngle=self.Near_incidenceAngle   if self.Near_incidenceAngle < self.bottomRight_IncidenceAngle else self.bottomRight_IncidenceAngle
        self.Near_incidenceAngle=self.Near_incidenceAngle   if self.Near_incidenceAngle < self.bottomLeft_IncidenceAngle else self.bottomLeft_IncidenceAngle
        
        self.Far_incidenceAngle=self.topRight_IncidenceAngle
        self.Far_incidenceAngle=self.Far_incidenceAngle   if self.Far_incidenceAngle > self.topLeft_IncidenceAngle else self.topLeft_IncidenceAngle
        self.Far_incidenceAngle=self.Far_incidenceAngle   if self.Far_incidenceAngle > self.bottomRight_IncidenceAngle else self.bottomRight_IncidenceAngle
        self.Far_incidenceAngle=self.Far_incidenceAngle   if self.Far_incidenceAngle > self.bottomLeft_IncidenceAngle else self.bottomLeft_IncidenceAngle
        
        self.incidenceAngle=float((node.find("productInfo").find("sceneInfo").find("sceneCenterCoord").find("incidenceAngle").text))
        
        """ 斜距 """
        self.slantRangeTimeToFirstRangeSample=float(node.find("productInfo").find("sceneInfo").find("rangeTime").find("firstPixel").text)
        self.startingRange=self.slantRangeTimeToFirstRangeSample * (Const.c/2)
        
        self.azimuthPixelSize=float(node.find("productInfo").find("imageDataInfo").find("imageRaster").find("rowSpacing").text) 
        self.totalProcessedAzimuthBandwidth=float(node.find("processing").find("processingParameter").find("totalProcessedAzimuthBandwidth").text) # 方位向
        self.prf=float(node.find("instrument").find("settings").find("settingRecord").find("PRF").text)
        

        """ 成像 """
        self.dataStartTime=self.convertToDateTime(node.find("productInfo").find("sceneInfo").find("start").find("timeUTC").text)
        self.dataStopTime=self.convertToDateTime(node.find("productInfo").find("sceneInfo").find("stop").find("timeUTC").text)        
        
        self.orig_prf=(self.lines-1)/((self.dataStopTime-self.dataStartTime).total_seconds())
        
        
        for z in node.getchildren():
            if z.tag == self.elementName('platform'):
                self.OrbitModelInfo.set_from_etnode(z.find("orbit"))
            elif z.tag == self.elementName('imageGenerationParameters'):
                self.imageGenerationParameters.set_from_etnode(z)
            elif z.tag == self.elementName('imageAttributes'):
                self.imageAttributes.set_from_etnode(z)

        """ 轨道 """
        self.passDirection=node.find("productInfo").find("missionInfo").find("orbitDirection").text
        self.orbitstarttime=(self.dataStopTime-self.dataStartTime)/2+self.dataStartTime
        
        """ 多普勒系数 """
        self.dopplerCentroid.set_from_etnode(node.find("processing").find("doppler").find("dopplerCentroid").find("dopplerEstimate").find("basebandDoppler"))
        self.dopplerRateValues.set_from_etnode(node.find("processing").find("geometry").find("dopplerRate").find("dopplerRatePolynomial"))   


    def __str__(self):
        retstr  = "Product:"+sep+tab
        retlst  = ()
        retstr += "productID=%s"+sep+tab
        retlst += (self.productId,)
        retstr += "documentIdentifier=%s"+sep
        retlst += (self.documentId,)
        retstr += "%s"+sep
        retlst += (str(self.sourceAttributes),)
        retstr += "%s"+sep
        retlst += (str(self.imageGenerationParameters),)
        retstr += ":Product"
        return retstr % retlst

class _SourceAttributes(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.satellite = None
        self.sensor = None
        self.inputDatasetId = None
        self.imageId = None
        self.inputDatasetFacilityId = None
        self.beamModeId = None
        self.beamModeMnemonic = None
        self.rawDataStartTime = None
        self.radarParameters = _RadarParameters()
        self.rawDataAttributes = _RawDataAttributes()
        self.orbitAndAttitude = _OrbitAndAttitude()

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('satellite'):
                self.satellite = z.text
            elif z.tag == self.elementName('sensor'):
                self.sensor = z.text
            elif z.tag == self.elementName('inputDatasetId'):
                self.inputDatasetId = z.text
            elif z.tag == self.elementName('imageID'):
                self.imageId = z.text
            elif z.tag == self.elementName('inputDatasetFacilityId'):
                self.inputDatasetFacilityId = z.text
            elif z.tag == self.elementName('beamModeID'):
                self.beamModeId = z.text
            elif z.tag == self.elementName('beamModeMnemonic'):
                self.beamModeMnemonic = z.text
            elif z.tag == self.elementName('rawDataStartTime'):
                self.rawDataStartTime = self.convertToDateTime(z.text)
            elif z.tag == self.elementName('radarParameters'):
                self.radarParameters.set_from_etnode(z)
            elif z.tag == self.elementName('rawDataAttributes'):
                self.rawDataAttributes.set_from_etnode(z)
            elif z.tag == self.elementName('orbitAndAttitude'):
                self.orbitAndAttitude.set_from_etnode(z)

    def __str__(self):
        retstr  = "SourceAttributes:"+sep+tab
        retlst  = ()
        retstr += "satellite=%s"+sep+tab
        retlst += (self.satellite,)
        retstr += "sensor=%s"+sep+tab
        retlst += (self.sensor,)
        retstr += "inputDatasetID=%s"+sep
        retlst += (self.inputDatasetId,)
        retstr += "%s"
        retlst += (str(self.radarParameters),)
        retstr += "%s"
        retlst += (str(self.rawDataAttributes),)
        retstr += "%s"
        retlst += (str(self.orbitAndAttitude),)
        retstr += ":SourceAttributes"
        return retstr % retlst

class _RadarParameters(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.acquisitionType = None
        self.beams = None
        self.polarizations = None
        self.pulses = None
        self.rank = None
        self.settableGains = []
        self.radarCenterFrequency = None
        self.prf = None
        self.pulseLengths = []
        self.pulseBandwidths = []
        self.antennaPointing = None
        self.adcSamplingRate = []
        self.yawSteeringFlag = None
        self.geodeticFlag = None
        self.rawBitsPerSample = None
        self.samplesPerEchoLine = None
        self.referenceNoiseLevels = [_ReferenceNoiseLevel()]*3

    def set_from_etnode(self,node):
        i = 0
        for z in node.getchildren():
            if z.tag == self.elementName('acquisitionType'):
                self.acquisitionType = z.text
            elif z.tag == self.elementName('beams'):
                self.beams = z.text
            elif z.tag == self.elementName('polarizations'):
                self.polarizations = z.text
            elif z.tag == self.elementName('pulses'):
                self.pulses = z.text
            elif z.tag == self.elementName('rank'):
                self.rank = z.text
            elif z.tag == self.elementName('settableGain'):
                self.settableGains.append(z.text)
            elif z.tag == self.elementName('radarCenterFrequency'):
                self.radarCenterFrequency = float(z.text)
            elif z.tag == self.elementName('pulseRepetitionFrequency'):
                self.prf = float(z.text)
            elif z.tag == self.elementName('pulseLength'):
                self.pulseLengths.append(float(z.text))
            elif z.tag == self.elementName('pulseBandwidth'):
                self.pulseBandwidths.append(float(z.text))
            elif z.tag == self.elementName('antennaPointing'):
                self.antennaPointing = z.text
            elif z.tag == self.elementName('adcSamplingRate'):
                self.adcSamplingRate.append(float(z.text))
            elif z.tag == self.elementName('yawSteeringFlag'):
                self.yawSteeringFlag = z.text
            elif z.tag == self.elementName('rawBitsPerSample'):
                self.rawBitsPerSample = int(z.text)
            elif z.tag == self.elementName('samplesPerEchoLine'):
                self.samplesPerEchoLine = int(z.text)
            elif z.tag == self.elementName('referenceNoiseLevels'):
                self.referenceNoiseLevels[i].set_from_etnode(z)
                i += 1

    def __str__(self):
        retstr = "RadarParameters:"+sep+tab
        retlst = ()
        retstr += "acquisitionType=%s"+sep+tab
        retlst += (self.acquisitionType,)
        retstr += "beams=%s"+sep+tab
        retlst += (self.beams,)
        retstr += "polarizations=%s"+sep+tab
        retlst += (self.polarizations,)
        retstr += "pulses=%s"+sep+tab
        retlst += (self.pulses,)
        retstr += "rank=%s"+sep
        retlst += (self.rank,)
        retstr += ":RadarParameters"+sep
        return retstr % retlst

class _ReferenceNoiseLevel(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.pixelFirstNoiseValue = None
        self.stepSize = None
        self.numberOfNoiseLevelValues = None
        self.noiseLevelValues = []

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('pixelFirstNoiseValue'):
                self.pixelFirstNoiseValue = int(z.text)
            elif z.tag == self.elementName('stepSize'):
                self.stepSize = int(z.text)
            elif z.tag == self.elementName('numberOfNoiseLevelValues'):
                self.numberOfNoiseLevelValues = int(z.text)
            elif z.tag == self.elementName('noiseLevelValues'):
                self.noiseLevelValues = list(map(float,z.text.split()))

    def __str__(self):
        retstr  = "ReferenceNoiseLevel:"+sep+tab
        retlst  = ()
        retstr += "pixelFirstNoiseValue=%s"+sep+tab
        retlst += (self.pixelFirstNoiseValue,)
        retstr += "stepSize=%s"+sep+tab
        retlst += (self.stepSize,)
        retstr += "numberOfNoiseLevelValues=%s"+sep+tab
        retlst += (self.numberOfNoiseLevelValues,)
        retstr += "noiseLevelValues=%s"+sep+tab
        retlst += (self.noiseLevelValues,)
        retstr += sep+":ReferenceNoiseLevel"
        return retstr % retlst

class _RawDataAttributes(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.numberOfInputDataGaps = None
        self.gapSize = None
        self.numberOfMissingLines = None
        self.rawDataAnalysis = [_RawDataAnalysis]*4

    def set_from_etnode(self,node):
        pass

    def __str__(self):
        return ""

class _RawDataAnalysis(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)

    def set_from_etnode(self,node):
        pass

class _OrbitAndAttitude(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.orbitInformation = _OrbitInformation()
        self.attitudeInformation = _AttitudeInformation()

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('orbitInformation'):
                self.orbitInformation.set_from_etnode(z)
            elif z.tag == self.elementName('attitudeInformation'):
                self.attitudeInformation.set_from_etnode(z)

    def __str__(self):
        retstr = "OrbitAndAttitude:"+sep
        retlst = ()
        retstr += "%s"
        retlst += (str(self.orbitInformation),)
        retstr += "%s"
        retlst += (str(self.attitudeInformation),)
        retstr += ":OrbitAndAttitude"+sep
        return retstr % retlst

class _OrbitInformation(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.passDirection = None
        self.orbitDataSource = None
        self.orbitDataFile = None
        self.stateVectors = []
 
        
        
        
    def set_from_etnode(self,node): # orbit
        self.orbitDataSource=node.find("orbitHeader").find("sensor").text
        for z in node.getchildren():
            if z.tag == self.elementName('stateVec'):
                sv = _StateVector()
                sv.set_from_etnode(z)
                self.stateVectors.append(sv)

    def __str__(self):
        retstr = "OrbitInformation:"+sep+tab
        retlst = ()
        retstr += "passDirection=%s"+sep+tab
        retlst += (self.passDirection,)
        retstr += "orbitDataSource=%s"+sep+tab
        retlst += (self.orbitDataSource,)
        retstr += "orbitDataFile=%s"+sep
        retlst += (self.orbitDataFile,)
        retstr += ":OrbitInformation"+sep
        return retstr % retlst
    
    
class _StateVector(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.timeStamp = None
        self.xPosition = None
        self.yPosition = None
        self.zPosition = None
        self.xVelocity = None
        self.yVelocity = None
        self.zVelocity = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('timeUTC'):
                self.timeStamp = self.convertToDateTime(z.text)
            elif z.tag == self.elementName('posX'):
                self.xPosition = float(z.text)
            elif z.tag == self.elementName('posY'):
                self.yPosition = float(z.text)
            elif z.tag == self.elementName('posZ'):
                self.zPosition = float(z.text)
            elif z.tag == self.elementName('velX'):
                self.xVelocity = float(z.text)
            elif z.tag == self.elementName('velY'):
                self.yVelocity = float(z.text)
            elif z.tag == self.elementName('velZ'):
                self.zVelocity = float(z.text)

    def __str__(self):
        retstr = "StateVector:"+sep+tab
        retlst = ()
        retstr += "timeStamp=%s"+sep+tab
        retlst += (self.timeStamp,)
        retstr += "xPosition=%s"+sep+tab
        retlst += (self.xPosition,)
        retstr += "yPosition=%s"+sep+tab
        retlst += (self.yPosition,)
        retstr += "zPosition=%s"+sep+tab
        retlst += (self.zPosition,)
        retstr += "xVelocity=%s"+sep+tab
        retlst += (self.xVelocity,)
        retstr += "yVelocity=%s"+sep+tab
        retlst += (self.yVelocity,)
        retstr += "zVelocity=%s"+sep+tab
        retlst += (self.zVelocity,)
        retstr += sep+":StateVector"
        return retstr % retlst

class _AttitudeInformation(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.attitudeDataSource = None
        self.attitudeOffsetApplied = None
        self.attitudeAngles = []

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('attitudeDataSource'):
                self.attitudeDataSource = z.text
            elif z.tag == self.elementName('attitudeOffsetApplied'):
                self.attitudeOffsetApplied = z.text
            elif z.tag == self.elementName('attitudeAngles'):
                aa = _AttitudeAngles()
                aa.set_from_etnode(z)
                self.attitudeAngles.append(aa)

    def __str__(self):
        retstr = "AttitudeInformation:"+sep+tab
        retlst = ()
        retstr += "attitudeDataSource=%s"+sep+tab
        retlst += (self.attitudeDataSource,)
        retstr += "attitudeOffsetApplied=%s"+sep+tab
        retlst += (self.attitudeOffsetApplied,)
        retstr += "%s"+sep+tab
        retlst += (map(str,self.attitudeAngles),)
        retstr += ":AttitudeInformation"+sep
        return retstr % retlst

class _AttitudeAngles(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.timeStamp = None
        self.yaw = None
        self.roll = None
        self.pitch = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('timeStamp'):
                self.timeStamp = self.convertToDateTime(z.text)
            elif z.tag == self.elementName('yaw'):
                self.yaw = float(z.text)
            elif z.tag == self.elementName('roll'):
                self.roll = float(z.text)
            elif z.tag == self.elementName('pitch'):
                self.pitch = float(z.text)

    def __str__(self):
        retstr = "AttitudeAngles:"+sep+tab
        retlst = ()
        retstr += "timeStamp=%s"+sep+tab
        retlst += (self.timeStamp,)
        retstr += "yaw=%s"+sep+tab
        retlst += (self.yaw,)
        retstr += "roll=%s"+sep+tab
        retlst += (self.roll,)
        retstr += "pitch=%s"+sep+tab
        retlst += (self.pitch,)
        retstr += sep+":AttitudeAngles"
        return retstr % retlst

class _ImageGenerationParameters(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.generalProcessingInformation = _GeneralProcessingInformation()
        self.sarProcessingInformation = _SarProcessingInformation()
        self.dopplerCentroid = _DopplerCentroid()
        self.dopplerRateValues = _DopplerRateValues()
        self.chirp = []
        self.slantRangeToGroundRange = _SlantRangeToGroundRange()
        self.payloadCharacteristicsFile = []

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('generalProcessingInformation'):
                self.generalProcessingInformation.set_from_etnode(z)
            elif z.tag == self.elementName('sarProcessingInformation'):
                self.sarProcessingInformation.set_from_etnode(z)
            elif z.tag == self.elementName('dopplerCentroid'):
                self.dopplerCentroid.set_from_etnode(z)
            elif z.tag == self.elementName('dopplerRateValues'):
                self.dopplerRateValues.set_from_etnode(z)
            elif z.tag == self.elementName('slantRangeToGroundRange'):
                self.slantRangeToGroundRange.set_from_etnode(z)

    def __str__(self):
        retstr = "ImageGenerationParameters:"+sep
        retlst = ()
        retstr += "%s"
        retlst += (str(self.generalProcessingInformation),)
        retstr += "%s"
        retlst += (str(self.sarProcessingInformation),)
        retstr += "%s"
        retlst += (str(self.dopplerCentroid),)
        retstr += "%s"
        retlst += (str(self.dopplerRateValues),)
        retstr += ":ImageGenerationParameters"
        return retstr % retlst


class _GeneralProcessingInformation(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.productType = None
        self._processingFacility = None
        self.processingTime = None
        self.softwareVersion = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('productType'):
                self.productType = z.text
            elif z.tag == self.elementName('_processingFacility'):
                self._processingFacility = z.text
            elif z.tag == self.elementName('processingTime'):
                self.processingTime = self.convertToDateTime(z.text)
            elif z.tag == self.elementName('softwareVersion'):
                self.softwareVersion = z.text

    def __str__(self):
        retstr = "GeneralProcessingInformation:"+sep+tab
        retlst = ()
        retstr += "productType=%s"+sep+tab
        retlst += (self.productType,)
        retstr += "_processingFacility=%s"+sep+tab
        retlst += (self._processingFacility,)
        retstr += "processingTime=%s"+sep+tab
        retlst += (self.processingTime,)
        retstr += "softwareVersion=%s"+sep
        retlst += (self.softwareVersion,)
        retstr += ":GeneralProcessingInformation"+sep
        return retstr % retlst

class _SarProcessingInformation(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.lutApplied = None
        self.elevationPatternCorrection = None
        self.rangeSpreadingLossCorrection = None
        self.pulseDependantGainCorrection = None
        self.receiverSettableGain = None
        self.rawDataCorrection = None
        self.rangeReferenceFunctionSource = None
        self.interPolarizationMatricesCorrection = None
        self.zeroDopplerTimeFirstLine = None
        self.zeroDopplerTimeLastLine = None
        self.numberOfLinesProcessed = None
        self.samplingWindowStartTimeFirstRawLine = None
        self.samplingWindowStartTimeLastRawLine = None
        self.numberOfSwstChanges = None
        self.numberOfRangeLooks = None
        self.rangeLookBandwidth = None
        self.totalProcessedRangeBandwidth = None
        self.numberOfAzimuthLooks = None
        self.scalarLookWeights = None
        self.azimuthLookBandwidth = None
        self.totalProcessedAzimuthBandwidth = None
        self.azimuthWindow = _Window('Azimuth')
        self.rangeWindow = _Window('Range')
        self.incidenceAngleNearRange = None
        self.incidenceAngleFarRange = None
        self.slantRangeNearEdge = None
        self._satelliteHeight = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('lutApplied'):
                self.lutApplied = z.text
            elif z.tag == self.elementName('numberOfLinesProcessed'):
                self.numberOfLinesProcessed = int(z.text)
            elif z.tag == self.elementName('azimuthWindow'):
                self.azimuthWindow.set_from_etnode(z)
            elif z.tag == self.elementName('rangeWindow'):
                self.rangeWindow.set_from_etnode(z)
            elif z.tag == self.elementName('incidenceAngleNearRange'):
                self.incidenceAngleNearRange = float(z.text)
            elif z.tag == self.elementName('incidenceAngleFarRange'):
                self.incidenceAngleFarRange = float(z.text)
            elif z.tag == self.elementName('slantRangeNearEdge'):
                self.slantRangeNearEdge = float(z.text)
            elif z.tag == self.elementName('totalProcessedAzimuthBandwidth'):
                self.totalProcessedAzimuthBandwidth = float(z.text)
            elif z.tag == self.elementName('_satelliteHeight'):
                self._satelliteHeight = float(z.text)
            elif z.tag == self.elementName('zeroDopplerTimeFirstLine'):
                self.zeroDopplerTimeFirstLine = self.convertToDateTime(z.text)
            elif z.tag == self.elementName('zeroDopplerTimeLastLine'):
                self.zeroDopplerTimeLastLine = self.convertToDateTime(z.text)

    def __str__(self):
        retstr = "sarProcessingInformation:"+sep+tab
        retlst = ()
        retstr += "lutApplied=%s"+sep+tab
        retlst += (self.lutApplied,)
        retstr += "numberOfLineProcessed=%s"+sep
        retlst += (self.numberOfLinesProcessed,)
        retstr += "%s"+sep
        retlst += (str(self.azimuthWindow),)
        retstr += "%s"+sep
        retlst += (str(self.rangeWindow),)
        retstr += ":sarProcessingInformation"+sep
        return retstr % retlst

class _Window(LT1ABNamespace):
    def __init__(self,type):
        LT1ABNamespace.__init__(self)
        self.type = type
        self.windowName = None
        self.windowCoefficient = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('windowName'):
                self.windowName = z.text
            elif z.tag == self.elementName('windowCoefficient'):
                self.windowCoefficient = float(z.text)

    def __str__(self):
        retstr = "%sWindow:"+sep+tab
        retlst = (self.type,)
        retstr += "windowName=%s"+sep+tab
        retlst += (self.windowName,)
        retstr += "windowCoefficient=%s"+sep
        retlst += (self.windowCoefficient,)
        retstr += ":%sWindow"
        retlst += (self.type,)
        return retstr % retlst

class _DopplerCentroid(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.timeOfDopplerCentroidEstimate = None
        self.dopplerAmbiguity = None
        self.dopplerAmbiguityConfidence= None
        self.dopplerCentroidReferenceTime = None
        self.dopplerCentroidPolynomialPeriod = None
        self.dopplerCentroidCoefficients = []
        self.dopplerCentroidConfidence = None
        
    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('timeOfDopplerCentroidEstimate'):
                self.timeOfDopplerCentroidEstimate = True
            elif z.tag == self.elementName('dopplerAmbiguity'):
                self.dopplerAmbiguity = True
            elif z.tag == self.elementName('referencePoint'):
                self.dopplerCentroidReferenceTime = float(z.text)
        coefficientlist=node.findall('coefficient')
        for i in range(len(coefficientlist)):
            self.dopplerCentroidCoefficients.append(0)
        for i in range(len(coefficientlist)):
            if int(coefficientlist[i].attrib["exponent"])==i:
                self.dopplerCentroidCoefficients[i]=float(coefficientlist[i].text)
        
    def __str__(self):
        retstr = "DopplerCentroid:"+sep+tab
        retlst = ()
        retstr += "dopplerAmbiguity=%s"+sep+tab
        retlst += (self.dopplerAmbiguity,)
        retstr += "dopplerCentroidCoefficients=%s"+sep
        retlst += (self.dopplerCentroidCoefficients,)
        retstr += ":DopplerCentroid"+sep
        return retstr % retlst

class _DopplerRateValues(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.dopplerRateReferenceTime = None
        self.dopplerRateValuesCoefficients = []

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('referencePoint'):
                self.dopplerRateReferenceTime = float(z.text)
        coefficientlist=node.findall('coefficient')
        for i in range(len(coefficientlist)):
            self.dopplerRateValuesCoefficients.append(0)
        for i in range(len(coefficientlist)):
            if int(coefficientlist[i].attrib["exponent"])==i:
                self.dopplerRateValuesCoefficients[i]=float(coefficientlist[i].text)
        



    def __str__(self):
        retstr = "DopplerRateValues:"+sep+tab
        retlst = ()
        retstr += "dopplerRateReferenceTime=%s"+sep+tab
        retlst += (self.dopplerRateReferenceTime,)
        retstr += "dopplerRateValuesCoefficients=%s"+sep+tab
        retlst += (self.dopplerRateValuesCoefficients,)
        retstr += ":DopplerRateValues"
        return retstr % retlst

class _Chirp(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)

class _SlantRangeToGroundRange(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.zeroDopplerAzimuthTime = None
        self.slantRangeTimeToFirstRangeSample = None
        self.groundRangeOrigin = None
        self.groundToSlantRangeCoefficients = []

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('zeroDopplerAzimuthTime'):
                self.zeroDopplerAzimuthTime = self.convertToDateTime(z.text)
            elif z.tag == self.elementName('slantRangeTimeToFirstRangeSample'):
                self.slantRangeTimeToFirstRangeSample = float(z.text)

class _ImageAttributes(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.productFormat = None
        self.outputMediaInterleaving = None
        self.rasterAttributes = _RasterAttributes()
        self.geographicInformation = _GeographicInformation()
        self.fullResolutionImageData = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('productFormat'):
                self.productFormat = z.text
            elif z.tag == self.elementName('outputMediaInterleaving'):
                self.outputMediaInterleaving = z.text
            elif z.tag == self.elementName('rasterAttributes'):
                self.rasterAttributes.set_from_etnode(z)
            elif z.tag == self.elementName('geographicInformation'):
                self.geographicInformation.set_from_etnode(z)
            elif z.tag == self.elementName('fullResolutionImageData'):
                self.fullResolutionImageData = z.text

class _RasterAttributes(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.dataType = None
        self.bitsPerSample = []
        self.numberOfSamplesPerLine = None
        self.numberOfLines = None
        self.sampledPixelSpacing = None
        self.sampledLineSpacing = None
        self.lineTimeOrdering = None
        self.pixelTimeOrdering = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('dataType'):
                self.dataType = z.text
            elif z.tag == self.elementName('bitsPerSample'):
                self.bitsPerSample.append(z.text)  # TODO: Make this a dictionary with keys of 'imaginary' and 'real'
            elif z.tag == self.elementName('numberOfSamplesPerLine'):
                self.numberOfSamplesPerLine = int(z.text)
            elif z.tag == self.elementName('numberOfLines'):
                self.numberOfLines = int(z.text)
            elif z.tag == self.elementName('sampledPixelSpacing'):
                self.sampledPixelSpacing = float(z.text)
            elif z.tag == self.elementName('sampledLineSpacing'):
                self.sampledLineSpacing = float(z.text)
            elif z.tag == self.elementName('lineTimeOrdering'):
                self.lineTimeOrdering = z.text
            elif z.tag == self.elementName('pixelTimeOrdering'):
                self.pixelTimeOrdering = z.text

class _GeographicInformation(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.geolocationGrid = _GeolocationGrid()
        self.rationalFunctions = _RationalFunctions()
        self.referenceEllipsoidParameters = _ReferenceEllipsoidParameters()

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('geolocationGrid'):
                self.geolocationGrid.set_from_etnode(z)

class _GeolocationGrid(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.imageTiePoint = []

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('imageTiePoint'):
                tp = _ImageTiePoint()
                tp.set_from_etnode(z)
                self.imageTiePoint.append(tp)


class _ImageTiePoint(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.imageCoordinates = _ImageCoordinates()
        self.geodeticCoordinates = _GeodeticCoordinates()

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('imageCoordinate'):
                self.imageCoordinates.set_from_etnode(z)
            elif z.tag == self.elementName('geodeticCoordinate'):
                self.geodeticCoordinates.set_from_etnode(z)

    def __str__(self):
        retstr = "ImageTiePoint:"+sep+tab
        retlst = ()
        retstr += "%s"
        retlst += (str(self.imageCoordinates),)
        retstr += "%s"
        retlst += (str(self.geodeticCoordinates),)
        retstr += ":ImageTiePoint"
        return retstr % retlst

class _ImageCoordinates(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.line = None
        self.pixel = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('line'):
                self.line = float(z.text)
            elif z.tag == self.elementName('pixel'):
                self.pixel = float(z.text)

    def __str__(self):
        retstr = "ImageCoordinate:"+sep+tab
        retlst = ()
        retstr += "line=%s"+sep+tab
        retlst += (self.line,)
        retstr += "pixel=%s"+sep+tab
        retlst += (self.pixel,)
        retstr += ":ImageCoordinate"
        return retstr % retlst

class _GeodeticCoordinates(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.latitude = None
        self.longitude = None
        self.height = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('latitude'):
                self.latitude = float(z.text)
            elif z.tag == self.elementName('longitude'):
                self.longitude = float(z.text)
            elif z.tag == self.elementName('height'):
                self.height = float(z.text)

    def __str__(self):
        retstr = "GeodeticCoordinate:"+sep+tab
        retlst = ()
        retstr += "latitude=%s"+sep+tab
        retlst += (self.latitude,)
        retstr += "longitude=%s"+sep+tab
        retlst += (self.longitude,)
        retstr += "height=%s"+sep+tab
        retlst += (self.height,)
        retstr += ":GeodeticCoordinate"
        return retstr % retlst

class _ReferenceEllipsoidParameters(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)
        self.ellipsoidName = None
        self.semiMajorAxis = None
        self.semiMinorAxis = None
        self.geodeticTerrainHeight = None

    def set_from_etnode(self,node):
        for z in node.getchildren():
            if z.tag == self.elementName('ellipsoidName'):
                self.ellipsoidName = z.text
            elif z.tag == self.elementName('semiMajorAxis'):
                self.semiMajorAxis = float(z.text)
            elif z.tag == self.elementName('semiMinorAxis'):
                self.semiMinorAxis = float(z.text)
            elif z.tag == self.elementName('geodeticTerrainHeight'):
                self.geodeticTerrainHeight = float(z.text)

    def __str__(self):
        return ""

class _RationalFunctions(LT1ABNamespace):
    def __init__(self):
        LT1ABNamespace.__init__(self)

    def set_from_etnode(self,node):
        pass

    def __str__(self):
        return ""


def findPreciseOrbit(dirname, fname, year):
    '''
    Find precise orbit file in given folder.
    '''

    import glob

    ###First try root folder itself
    res = glob.glob( os.path.join(dirname, fname.lower()))
    if len(res) == 0:

        res = glob.glob( os.path.join(dirname, "{0}".format(year), fname.lower()))
        if len(res) == 0:
            raise Exception('Orbit Dirname provided but no suitable orbit file found in {0}'.format(dirname))

    
    if len(res) > 1:
        print('More than one matching result found. Using first result.')

    return res[0]

def convertRSTimeToDateTime(instr):
    '''
    Convert RS2 orbit time string to datetime.
    '''

    parts = instr.strip().split('-')
    tparts = parts[-1].split(':')
    secs = float(tparts[2])
    intsecs = int(secs)
    musecs = int((secs - intsecs)*1e6)

    timestamp = datetime.datetime(int(parts[0]),1,1, int(tparts[0]), int(tparts[1]), intsecs, musecs) + datetime.timedelta(days = int(parts[1])-1)

    return timestamp