#!/usr/bin/env python
# coding: utf-8

# # Orbit determination example
# This notebook does the following:
# * Download an orbit first guess from SpaceTrack
# * Download laser ranging data
# * Feed the data to Orekit
# * Estimate the orbit
# * Propagate and compare the orbit to the first guess

# Two types of laser ranging data can be chosen (see below):
# 
# * Normal point data: https://ilrs.cddis.eosdis.nasa.gov/data_and_products/data/npt/index.html
# * Full rate data: https://ilrs.cddis.eosdis.nasa.gov/data_and_products/data/frt/index.html
#     * This will improve the orbit estimation
#     * Caution, this format involves large quantities of data
#     * Caution 2, this data is unfiltered, therefore there can be a superposition of two range curves if two retro-reflectors on the satellite are visible by the station at the same time

# ## OD parameters
# First, some parameters need to be defined for the orbit determination:
# * Satellite ID in NORAD and COSPAR format
# * Spacecraft mass: important for the drag term
# * Measurement weights: used to weight certain measurements more than others during the orbit estimation. Here, we only have range measurements and we do not know the confidence associated to these measurements, so all weights are identical
# * OD date: date at which the orbit will be estimated. 
# * Data collection duration: for example, if equals 2 days, the laser data from the 2 days before the OD date will be used to estimate the orbit. This value is an important trade-off for the quality of the orbit determination:
#     * The longer the duration, the more ranging data is available, which can increase the quality of the estimation
#     * The longer the duration, the longer the orbit must be propagated, and the higher the covariance because of the orbit perturbations such as the gravity field, drag, Sun, Moon, etc.

# Satellite parameters

# In[1]:


sat_list = {    
    'envisat': {
        'norad_id': 27386,
        'cospar_id': '0200901',
        'sic_id': '6179',
        'mass': 8000.0, # kg; TODO: compute proper value
        'cross_section': 100.0, # m2; TODO: compute proper value
        'cd': 2.0, # TODO: compute proper value
        'cr': 1.0  # TODO: compute proper value
    },    
    'lageos2': {
        'norad_id': 22195,
        'cospar_id': '9207002',
        'sic_id': '5986',
        'mass': 405.0, # kg
        'cross_section': 0.2827, # m2
        'cd': 2.0, # TODO: compute proper value
        'cr': 1.0  # TODO: compute proper value
    },
    'technosat': {
        'norad_id': 42829,
        'cospar_id': '1704205',
        'sic_id': '6203',
        'mass': 20.0, # kg
        'cross_section': 0.10, # m2,
        'cd': 2.0, # TODO: compute proper value
        'cr': 1.0  # TODO: compute proper value
    }
}

sc_name = 'lageos2'  # Change the name to select a different satellite in the dict


# In[2]:


# Constants
c = 299792458 # m/s

# Parameters
range_weight = 1.0 # Will be normalized later (i.e divided by the number of observations)
range_sigma = 1.0 # Estimated covariance of the range measurements, in meters

from datetime import datetime
odDate = datetime(2018, 9, 22) # Beginning of the orbit determination
collectionDuration = 2 # days
from datetime import timedelta
startCollectionDate = odDate + timedelta(days=-collectionDuration)

# Orbit propagator parameters
prop_min_step = 0.001 # s
prop_max_step = 300.0 # s
prop_position_error = 10.0 # m

# Estimator parameters
estimator_position_scale = 1.0 # m
estimator_convergence_thres = 1e-3
estimator_max_iterations = 25
estimator_max_evaluations = 35


# ## API credentials
# The following sets up accounts for SpaceTrack (for orbit data) and the EDC API (for laser ranging data).
# * A SpaceTrack account is required, it can be created for free at: https://www.space-track.org/auth/createAccount
# * An EDC account is required, it can be created for free at: https://edc.dgfi.tum.de/en/register/

# In[3]:


# Space-Track
identity_st = input('Enter SpaceTrack username')
import getpass
password_st = getpass.getpass(prompt='Enter SpaceTrack password for account {}'.format(identity_st))
import spacetrack.operators as op
from spacetrack import SpaceTrackClient
st = SpaceTrackClient(identity=identity_st, password=password_st)

# EDC API
username_edc = input('Enter EDC API username')
password_edc = getpass.getpass(prompt='Enter EDC API password for account {}'.format(username_edc)) # You will get prompted for your password
url = 'https://edc.dgfi.tum.de/api/v1/'


# ## Setting up models
# Initializing Orekit and JVM

# In[4]:


import orekit
orekit.initVM()

# Modified from https://gitlab.orekit.org/orekit-labs/python-wrapper/blob/master/python_files/pyhelpers.py
from java.io import File
from org.orekit.data import DataProvidersManager, DirectoryCrawler
from orekit import JArray

orekit_filename = 'orekit-data'
DM = DataProvidersManager.getInstance()
datafile = File(orekit_filename)
if not datafile.exists():
    print('Directory :', datafile.absolutePath, ' not found')

crawler = DirectoryCrawler(datafile)
DM.clearProviders()
DM.addProvider(crawler)


# Import station data from file

# In[5]:


stationFile = 'SLRF2014_POS+VEL_2030.0_180504.snx'
stationEccFile = 'ecc_xyz.snx'
import slrDataUtils
stationData = slrDataUtils.parseStationData(stationFile, stationEccFile, startCollectionDate)


# The orbit determination needs a first guess. For this, we use Two-Line Elements. Retrieving the latest TLE prior to the beginning of the orbit determination. It is important to have a "fresh" TLE, because the newer the TLE, the better the orbit estimation.

# In[6]:


rawTle = st.tle(norad_cat_id=sat_list[sc_name]['norad_id'], epoch='<{}'.format(odDate), orderby='epoch desc', limit=1, format='tle')
tleLine1 = rawTle.split('\n')[0]
tleLine2 = rawTle.split('\n')[1]
print(tleLine1)
print(tleLine2)


# Setting up Orekit frames and models

# In[39]:


from org.orekit.utils import Constants as orekit_constants

from org.orekit.frames import FramesFactory, ITRFVersion
from org.orekit.utils import IERSConventions
tod = FramesFactory.getTOD(IERSConventions.IERS_2010, False) # Taking tidal effects into account when interpolating EOP parameters
gcrf = FramesFactory.getGCRF()
itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, False)
#itrf = FramesFactory.getITRF(ITRFVersion.ITRF_2014, IERSConventions.IERS_2010, False)
# Selecting frames to use for OD
eci = gcrf
ecef = itrf

from org.orekit.models.earth import ReferenceEllipsoid
wgs84Ellipsoid = ReferenceEllipsoid.getWgs84(ecef)
from org.orekit.bodies import CelestialBodyFactory
moon = CelestialBodyFactory.getMoon()
sun = CelestialBodyFactory.getSun()

from org.orekit.time import AbsoluteDate, TimeScalesFactory
utc = TimeScalesFactory.getUTC()
mjd_utc_epoch = AbsoluteDate(1858, 11, 17, 0, 0, 0.0, utc)


# Setting up the propagator from the initial TLEs

# In[8]:


from org.orekit.propagation.analytical.tle import TLE
orekitTle = TLE(tleLine1, tleLine2)

from org.orekit.attitudes import NadirPointing
nadirPointing = NadirPointing(eci, wgs84Ellipsoid)

from org.orekit.propagation.analytical.tle import SGP4
sgp4Propagator = SGP4(orekitTle, nadirPointing, sat_list[sc_name]['mass'])

tleInitialState = sgp4Propagator.getInitialState()
tleEpoch = tleInitialState.getDate()
tleOrbit_TEME = tleInitialState.getOrbit()
tlePV_ECI = tleOrbit_TEME.getPVCoordinates(eci)

from org.orekit.orbits import CartesianOrbit
tleOrbit_ECI = CartesianOrbit(tlePV_ECI, eci, wgs84Ellipsoid.getGM())

from org.orekit.propagation.conversion import DormandPrince853IntegratorBuilder
integratorBuilder = DormandPrince853IntegratorBuilder(prop_min_step, prop_max_step, prop_position_error)

from org.orekit.propagation.conversion import NumericalPropagatorBuilder
from org.orekit.orbits import PositionAngle
propagatorBuilder = NumericalPropagatorBuilder(tleOrbit_ECI,
                                               integratorBuilder, PositionAngle.MEAN, estimator_position_scale)
propagatorBuilder.setMass(sat_list[sc_name]['mass'])
propagatorBuilder.setAttitudeProvider(nadirPointing)


# Adding perturbation forces to the propagator

# In[9]:


# Earth gravity field with degree 64 and order 64
from org.orekit.forces.gravity.potential import GravityFieldFactory
gravityProvider = GravityFieldFactory.getConstantNormalizedProvider(64, 64)
from org.orekit.forces.gravity import HolmesFeatherstoneAttractionModel
gravityAttractionModel = HolmesFeatherstoneAttractionModel(ecef, gravityProvider)
propagatorBuilder.addForceModel(gravityAttractionModel)

# Moon and Sun perturbations
from org.orekit.forces.gravity import ThirdBodyAttraction
moon_3dbodyattraction = ThirdBodyAttraction(moon)
propagatorBuilder.addForceModel(moon_3dbodyattraction)
sun_3dbodyattraction = ThirdBodyAttraction(sun)
propagatorBuilder.addForceModel(sun_3dbodyattraction)

# Solar radiation pressure
from org.orekit.forces.radiation import IsotropicRadiationSingleCoefficient
isotropicRadiationSingleCoeff = IsotropicRadiationSingleCoefficient(sat_list[sc_name]['cross_section'], sat_list[sc_name]['cr']);
from org.orekit.forces.radiation import SolarRadiationPressure
solarRadiationPressure = SolarRadiationPressure(sun, wgs84Ellipsoid.getEquatorialRadius(),
                                                isotropicRadiationSingleCoeff)
propagatorBuilder.addForceModel(solarRadiationPressure)

# Atmospheric drag
from org.orekit.forces.drag.atmosphere.data import MarshallSolarActivityFutureEstimation
msafe = MarshallSolarActivityFutureEstimation(
    '(?:Jan|Feb|Mar|Apr|May|Jun|Jul|Aug|Sep|Oct|Nov|Dec)\p{Digit}\p{Digit}\p{Digit}\p{Digit}F10\.(?:txt|TXT)',
    MarshallSolarActivityFutureEstimation.StrengthLevel.AVERAGE)
DM.feed(msafe.getSupportedNames(), msafe) # Feeding the F10.7 bulletins to Orekit's data manager

from org.orekit.forces.drag.atmosphere import NRLMSISE00
atmosphere = NRLMSISE00(msafe, sun, wgs84Ellipsoid)
#from org.orekit.forces.drag.atmosphere import DTM2000
#atmosphere = DTM2000(msafe, sun, wgs84Ellipsoid)
from org.orekit.forces.drag import IsotropicDrag
isotropicDrag = IsotropicDrag(sat_list[sc_name]['cross_section'], sat_list[sc_name]['cd'])
from org.orekit.forces.drag import DragForce
dragForce = DragForce(atmosphere, isotropicDrag)
propagatorBuilder.addForceModel(dragForce)

# Relativity
#from org.orekit.forces.gravity import Relativity
#relativity = Relativity(orekit_constants.EIGEN5C_EARTH_MU)
#propagatorBuilder.addForceModel(relativity)


# Setting up the estimator

# In[10]:


from org.hipparchus.linear import QRDecomposer
matrixDecomposer = QRDecomposer(1e-11)
from org.hipparchus.optim.nonlinear.vector.leastsquares import GaussNewtonOptimizer
optimizer = GaussNewtonOptimizer(matrixDecomposer, False)

from org.orekit.estimation.leastsquares import BatchLSEstimator
estimator = BatchLSEstimator(optimizer, propagatorBuilder)
estimator.setParametersConvergenceThreshold(estimator_convergence_thres)
estimator.setMaxIterations(estimator_max_iterations)
estimator.setMaxEvaluations(estimator_max_evaluations)


# ## Fetching range data
# Looking for laser ranging data prior to the OD date.
# 
# The API only allows to look for data using the date formats 2018-07-1%, 2018-07-14% or 2018-07-14 0% for example. As a consequence, the search must be split into several days. The results are then sorted, and the observations which are outside of the date range are deleted.

# In[11]:


import slrDataUtils
nptDatasetList = slrDataUtils.querySlrData(username_edc, password_edc, url, 'NPT',
                                         sat_list[sc_name]['cospar_id'], startCollectionDate, odDate)
frdDatasetList = slrDataUtils.querySlrData(username_edc, password_edc, url, 'FRD',
                                         sat_list[sc_name]['cospar_id'], startCollectionDate, odDate)


# Downloading the list of observations.

# In[12]:


from orekit.pyhelpers import *
from org.orekit.estimation.measurements import Range
import slrDataUtils
nptDataFrame = slrDataUtils.dlAndParseSlrData(username_edc, password_edc, url, 'NPT', nptDatasetList)
# Comment out to download Full-Rate data
# frdDataFrame = slrDataUtils.dlAndParseSlrData(username_edc, password_edc, url, 'FRD', frdDatasetList)


# Adding the measurements to the estimator

# In[13]:


slrDataFrame = nptDataFrame
# Comment out to use Full-Rate data
# slrDataFrame = frdDataFrame
for receiveTime, slrData in slrDataFrame.iterrows():
    orekitRange = Range(stationData.loc[slrData['station-id'], 'OrekitGroundStation'], 
                        datetime_to_absolutedate(receiveTime),
                        slrData['range'],
                        range_sigma,
                        range_weight
                       ) # Uses date of signal reception; https://www.orekit.org/static/apidocs/org/orekit/estimation/measurements/Range.html
    estimator.addMeasurement(orekitRange)


# ## Performing the OD
# Estimate the orbit. This step can take a long time.

# In[14]:


estimatedPropagatorArray = estimator.estimate()


# Getting the estimated propagator and orbit.

# In[15]:


estimatedPropagator = estimatedPropagatorArray[0]
estimatedInitialState = estimatedPropagator.getInitialState()
actualOdDate = estimatedInitialState.getDate()
estimatedOrbit_init = estimatedInitialState.getOrbit()


# ## Covariance analysis
# Creating the LVLH frame, computing the covariance matrix in both TOD and LVLH frames

# In[16]:


# Creating the LVLH frame 
from org.orekit.frames import LocalOrbitalFrame
from org.orekit.frames import LOFType
lvlh = LocalOrbitalFrame(eci, LOFType.LVLH, estimatedPropagator, 'LVLH')

# Getting covariance matrix in ECI frame
covMat_eci_java = estimator.getPhysicalCovariances(1.0e-10)

# Converting matrix to LVLH frame
# Getting an inertial frame aligned with the LVLH frame at this instant
# The LVLH is normally not inertial, but this should not affect results too much
# Reference: David Vallado, Covariance Transformations for Satellite Flight Dynamics Operations, 2003
eci2lvlh_frozen = eci.getTransformTo(lvlh, actualOdDate).freeze() 

# Computing Jacobian
from org.orekit.utils import CartesianDerivativesFilter
from orekit.pyhelpers import JArray_double2D
jacobianDoubleArray = JArray_double2D(6, 6)
eci2lvlh_frozen.getJacobian(CartesianDerivativesFilter.USE_PV, jacobianDoubleArray)
from org.hipparchus.linear import Array2DRowRealMatrix
jacobian = Array2DRowRealMatrix(jacobianDoubleArray)
# Applying Jacobian to convert matrix to lvlh
covMat_lvlh_java = jacobian.multiply(
    covMat_eci_java.multiply(covMat_eci_java.transpose()))

# Converting the Java matrices to numpy
import numpy as np
covarianceMat_eci = np.matrix([covMat_eci_java.getRow(iRow) 
                              for iRow in range(0, covMat_eci_java.getRowDimension())])
covarianceMat_lvlh = np.matrix([covMat_lvlh_java.getRow(iRow) 
                              for iRow in range(0, covMat_lvlh_java.getRowDimension())])


# Computing the position and velocity standard deviation 

# In[17]:


pos_std_crossTrack = np.sqrt(max(0.0, covarianceMat_lvlh[0,0]))
pos_std_alongTrack = np.sqrt(max(0.0, covarianceMat_lvlh[1,1]))
pos_std_outOfPlane = np.sqrt(max(0.0, covarianceMat_lvlh[2,2]))
vel_std_crossTrack = np.sqrt(max(0.0, covarianceMat_lvlh[3,3])) # In case the value is negative...
vel_std_alongTrack = np.sqrt(max(0.0, covarianceMat_lvlh[4,4]))
vel_std_outOfPlane = np.sqrt(max(0.0, covarianceMat_lvlh[5,5]))


# ## Analyzing residuals
# Getting the estimated and measured ranges.

# In[18]:


propagatorParameters   = estimator.getPropagatorParametersDrivers(True)
measurementsParameters = estimator.getMeasurementsParametersDrivers(True)

lastEstimations = estimator.getLastEstimations()
valueSet = lastEstimations.values()
estimatedMeasurements = valueSet.toArray()
keySet = lastEstimations.keySet()
realMeasurements = keySet.toArray()

from org.orekit.estimation.measurements import EstimatedMeasurement
from org.orekit.estimation.measurements import Range

import pandas as pd
observedRangeSeries = pd.Series()
estimatedRangeSeries = pd.Series()

for estMeas in estimatedMeasurements:
    estMeas = EstimatedMeasurement.cast_(estMeas)
    observedValue = estMeas.getObservedValue()
    estimatedValue = estMeas.getEstimatedValue()
    pyDateTime = absolutedate_to_datetime(estMeas.date)
    observedRangeSeries[pyDateTime] = observedValue[0]
    estimatedRangeSeries[pyDateTime] = estimatedValue[0]


# Setting up Plotly for offline mode

# In[19]:


import plotly.io as pio
pio.renderers.default = 'jupyterlab+notebook'


# Plotting residuals

# In[20]:


import plotly.graph_objs as go

trace = go.Scattergl(
    x=observedRangeSeries.index, y=observedRangeSeries-estimatedRangeSeries,
    mode='markers'
)

data = [trace]

layout = go.Layout(
    title = 'Range residuals',
    xaxis = dict(
        title = 'Datetime UTC'
    ),
    yaxis = dict(
        title = 'Range residual (m)'
    )
)

fig = dict(data=data, layout=layout)

pio.show(fig)


# ## Propagating the solution 
# Propagating the solution and saving the PV coordinates from both the estimated propagator and the initial TLE guess.

# In[21]:


dt = 300.0
date_start = datetime_to_absolutedate(startCollectionDate)
date_start = date_start.shiftedBy(-86400.0)
date_end = datetime_to_absolutedate(odDate)
date_end = date_end.shiftedBy(86400.0) # Stopping 1 day after OD date


# In[22]:


# First propagating in ephemeris mode
estimatedPropagator.resetInitialState(estimatedInitialState)
estimatedPropagator.setEphemerisMode()
# Propagating from 1 day before data collection
# To 1 week after orbit determination (for CPF generation)
estimatedPropagator.propagate(date_start, datetime_to_absolutedate(odDate).shiftedBy(7 * 86400.0))
bounded_propagator = estimatedPropagator.getGeneratedEphemeris()

# Propagating the bounded propagator to retrieve the intermediate states

from slrDataUtils import orekitPV2dataframe

PV_eci_df = pd.DataFrame()
PV_ecef_df = pd.DataFrame()
PV_tle_eci_df = pd.DataFrame()
deltaPV_lvlh_df = pd.DataFrame() # SGP4 PV from SLROD origin

# Saving all intermediate
from java.util import ArrayList
states_list = ArrayList()

date_current = date_start
while date_current.compareTo(date_end) <= 0:
    datetime_current = absolutedate_to_datetime(date_current)    
    spacecraftState = bounded_propagator.propagate(date_current)
    
    states_list.add(spacecraftState)
    
    PV_eci = spacecraftState.getPVCoordinates(eci)
    PV_eci_df = PV_eci_df.append(orekitPV2dataframe(PV_eci, datetime_current))
    
    PV_ecef = spacecraftState.getPVCoordinates(ecef)
    PV_ecef_df = PV_ecef_df.append(orekitPV2dataframe(PV_ecef, datetime_current))
    
    PV_tle_eci = sgp4Propagator.getPVCoordinates(date_current, eci)
    PV_tle_eci_df = PV_tle_eci_df.append(orekitPV2dataframe(PV_tle_eci, datetime_current))
    
    deltaPV_lvlh = sgp4Propagator.getPVCoordinates(date_current, lvlh)
    deltaPV_lvlh_df = deltaPV_lvlh_df.append(orekitPV2dataframe(deltaPV_lvlh, datetime_current))
    
    date_current = date_current.shiftedBy(dt)    


# ## Writing CPF file

# A CPF file usually contains 7 days of orbit prediction in ECEF frame with a sample time of 5 minutes, to allow the laser stations to track the satellite.
# 
# Therefore we have to propagate for 7 days.

# In[23]:


# Function to compute MJD days and seconds of day
def datetime_to_mjd_days_seconds(le_datetime):
    apparent_clock_offset_s = datetime_to_absolutedate(le_datetime).offsetFrom(
        mjd_utc_epoch, utc)
    days_since_mjd_epoch = int(np.floor(apparent_clock_offset_s / 86400.0))
    seconds_of_day = apparent_clock_offset_s - days_since_mjd_epoch * 86400.0
    return days_since_mjd_epoch, seconds_of_day


# In[24]:


date_end_cpf = datetime_to_absolutedate(odDate).shiftedBy(7 * 86400.0)

from slrDataUtils import orekitPV2dataframe

PV_ecef_cpf_df = pd.DataFrame()

dt = 300.0
date_current = datetime_to_absolutedate(odDate)
while date_current.compareTo(date_end_cpf) <= 0:
    datetime_current = absolutedate_to_datetime(date_current)    
    spacecraftState = bounded_propagator.propagate(date_current)
        
    PV_ecef_cpf = spacecraftState.getPVCoordinates(ecef)
    PV_ecef_cpf_df = PV_ecef_cpf_df.append(orekitPV2dataframe(PV_ecef_cpf, datetime_current))
        
    date_current = date_current.shiftedBy(dt)   
    
PV_ecef_cpf_df['mjd_days'], PV_ecef_cpf_df['seconds_of_day'] = zip(*PV_ecef_cpf_df['DateTimeUTC'].apply(lambda x: 
                                                                         datetime_to_mjd_days_seconds(x)))


# In[25]:


from slrDataUtils import write_cpf
write_cpf(PV_ecef_cpf_df, 
          'cpf_out.ass', 
          'PYT', 
          odDate, 
          4242, 
          sc_name, 
          sat_list[sc_name]['cospar_id'],
          sat_list[sc_name]['sic_id'], 
          str(sat_list[sc_name]['norad_id']), 
          odDate, 
          absolutedate_to_datetime(date_end_cpf), 
          int(dt))


# ## Comparison with TLE
# Plotting the position difference between the initial TLE and the estimated orbit. The grey area represents the time window where range measurements were used to perform the orbit determination.

# In[26]:


# Rectangles to visualise time window for orbit determination.

od_window_rectangle =  {
    'type': 'rect',
    # x-reference is assigned to the x-values
    'xref': 'x',
    # y-reference is assigned to the plot paper [0,1]
    'yref': 'paper',
    'x0': startCollectionDate,
    'y0': 0,
    'x1': odDate,
    'y1': 1,
    'fillcolor': '#d3d3d3',
    'opacity': 0.3,
    'line': {
        'width': 0,
    }
}


# In[27]:


# Computing norm of position difference
import numpy as np
deltaPos_norm = pd.Series(data=np.linalg.norm(
    PV_tle_eci_df[['x','y','z']] - PV_eci_df[['x','y','z']], 
    axis=1),
                         index=PV_tle_eci_df.index)

import plotly.graph_objs as go

trace = go.Scattergl(
    x = deltaPos_norm.index,
    y = deltaPos_norm,
    mode='lines'
)

data = [trace]

layout = go.Layout(
    title = 'Norm of position difference between TLE and estimation',
    xaxis = dict(
        title = 'Datetime UTC'
    ),
    yaxis = dict(
        title = 'Norm of position delta (m)'
    ),
    shapes=[od_window_rectangle]
)

fig = dict(data=data, layout=layout)

pio.show(fig)


# Plotting the components of the position different between the TLE and the estimation, in LVLH frame. The grey area represents the time window where range measurements were used to perform the orbit determination.

# In[28]:


import plotly.graph_objs as go

traceX = go.Scattergl(
    x = deltaPV_lvlh_df['x'].index,
    y = deltaPV_lvlh_df['x'],
    mode='lines',
    name='Cross-Track'
)

traceY = go.Scattergl(
    x = deltaPV_lvlh_df['y'].index,
    y = deltaPV_lvlh_df['y'],
    mode='lines',
    name='Along-Track'
)

traceZ = go.Scattergl(
    x = deltaPV_lvlh_df['z'].index,
    y = deltaPV_lvlh_df['z'],
    mode='lines',
    name='Out-Of-Plane'
)

data = [traceX, traceY, traceZ]

layout = go.Layout(
    title = 'Delta position between TLE and estimation in LVLH frame',
    xaxis = dict(
        title = 'Datetime UTC'
    ),
    yaxis = dict(
        title = 'Position difference (m)'
    ),
    shapes=[od_window_rectangle]
)

fig = dict(data=data, layout=layout)

pio.show(fig)


# ## Fitting an "enhanced" TLE
# Let's fit a TLE to the estimated propagator. This requires the original TLE. If no TLE is available, then a first guess can be built by:
# 
# * Computing the Keplerian orbital elements from the propagator, for example using RV2COE at one instant.
# * Writing these elements to the TLE. Although these elements are not mean elements, they will be fitted within a certain range.
# * Write the BSTAR coefficient equal to zero. It is a free parameter in the fitting

# ### Fitting
# Fitting the TLE, based on great example by RomaricH on the Orekit forum: https://forum.orekit.org/t/generation-of-tle/265/4

# In[29]:


from org.orekit.propagation.conversion import TLEPropagatorBuilder, FiniteDifferencePropagatorConverter
from org.orekit.propagation.analytical.tle import TLEPropagator
threshold = 1.0  # "absolute threshold for optimization algorithm", but no idea about its impact
tle_builder = TLEPropagatorBuilder(orekitTle, PositionAngle.MEAN, 1.0)
fitter = FiniteDifferencePropagatorConverter(tle_builder, threshold, 1000)
fitter.convert(states_list, False, 'BSTAR')  # Setting BSTAR as free parameter
tle_propagator = TLEPropagator.cast_(fitter.getAdaptedPropagator())
tle_fitted = tle_propagator.getTLE()


# Let's compare both the original and the "enhanced" TLE:

# In[30]:


print(orekitTle)
print('')
print(tle_fitted)


# Let us propagate again to save the PV from this new TLE

# In[31]:


# Setting up yet another SGP4 propagator
sgp4Propagator_fitted = SGP4(tle_fitted, nadirPointing, sat_list[sc_name]['mass'])


# In[32]:


PV_tle_fitted_eci_df = pd.DataFrame()
deltaPV_tle_fitted_lvlh_df = pd.DataFrame() # SGP4 PV from SLROD origin

date_current = date_start
while date_current.compareTo(date_end) <= 0:    
    datetime_current = absolutedate_to_datetime(date_current)
    
    PV_tle_fitted_eci = sgp4Propagator_fitted.getPVCoordinates(date_current, eci)
    PV_tle_fitted_eci_df = PV_tle_fitted_eci_df.append(orekitPV2dataframe(PV_tle_fitted_eci, datetime_current))
    
    deltaPV_tle_fitted_lvlh = sgp4Propagator_fitted.getPVCoordinates(date_current, lvlh)
    deltaPV_tle_fitted_lvlh_df = deltaPV_tle_fitted_lvlh_df.append(orekitPV2dataframe(deltaPV_tle_fitted_lvlh, datetime_current))
    
    date_current = date_current.shiftedBy(dt)    


# ### Comparing with estimated propagator

# In some cases, the fitted TLE is much better, in some cases not.
# 
# The figure below shows the norm of the position difference with the estimated propagator.

# In[33]:


# Computing norm of position difference
import numpy as np
deltaPos_tle_fitted_norm = pd.Series(data=np.linalg.norm(
    PV_tle_fitted_eci_df[['x','y','z']] - PV_eci_df[['x','y','z']], 
    axis=1),
                         index=PV_tle_fitted_eci_df.index)

import plotly.graph_objs as go

trace = go.Scattergl(
    x = deltaPos_tle_fitted_norm.index,
    y = deltaPos_tle_fitted_norm,
    mode='lines'
)

data = [trace]

layout = go.Layout(
    title = 'Norm of position difference between fitted TLE and estimation',
    xaxis = dict(
        title = 'Datetime UTC'
    ),
    yaxis = dict(
        title = 'Norm of position delta (m)'
    )
)

fig = dict(data=data, layout=layout)

pio.show(fig)


# The individual components in LVLH frame are now more centered around zero. The fitting helped.

# In[34]:


import plotly.graph_objs as go

traceX = go.Scattergl(
    x = deltaPV_tle_fitted_lvlh_df['x'].index,
    y = deltaPV_tle_fitted_lvlh_df['x'],
    mode='lines',
    name='Cross-Track'
)

traceY = go.Scattergl(
    x = deltaPV_tle_fitted_lvlh_df['y'].index,
    y = deltaPV_tle_fitted_lvlh_df['y'],
    mode='lines',
    name='Along-Track'
)

traceZ = go.Scattergl(
    x = deltaPV_tle_fitted_lvlh_df['z'].index,
    y = deltaPV_tle_fitted_lvlh_df['z'],
    mode='lines',
    name='Out-Of-Plane'
)

data = [traceX, traceY, traceZ]

layout = go.Layout(
    title = 'Delta position between fitted TLE and estimation in LVLH frame',
    xaxis = dict(
        title = 'Datetime UTC'
    ),
    yaxis = dict(
        title = 'Position difference (m)'
    )
)

fig = dict(data=data, layout=layout)

pio.show(fig)


# ## Comparison with CPF
# The EDC API also provides Consolidated Prediction Files, which contain spacecraft position/velocity in ITRF frame as generated by their orbit determination system. Let us compare the results

# Requesting CPF data

# In[35]:


from slrDataUtils import queryCpfData, dlAndParseCpfData
cpfList = queryCpfData(username_edc, password_edc, url, 
                       sat_list[sc_name]['cospar_id'], startCollectionDate - timedelta(days=1))


# Downloading and parsing CPF data

# In[36]:


cpfDataFrame = dlAndParseCpfData(username_edc, password_edc, url, 
                                 cpfList, startCollectionDate - timedelta(days=1),
                                 odDate + timedelta(days=1))


# Plotting position difference. The grey area represents the time window where range measurements were used to perform the orbit determination.

# In[37]:


import plotly.graph_objs as go

delta_x_cpf = (PV_ecef_df['x'] - cpfDataFrame['x']).dropna()
delta_y_cpf = (PV_ecef_df['y'] - cpfDataFrame['y']).dropna()
delta_z_cpf = (PV_ecef_df['z'] - cpfDataFrame['z']).dropna()

traceX = go.Scattergl(
    x = delta_x_cpf.index,
    y = delta_x_cpf,
    mode='lines',
    name='X ECEF'
)

traceY = go.Scattergl(
    x = delta_y_cpf.index,
    y = delta_y_cpf,
    mode='lines',
    name='Y ECEF'
)

traceZ = go.Scattergl(
    x = delta_z_cpf.index,
    y = delta_z_cpf,
    mode='lines',
    name='Z ECEF'
)

data = [traceX, traceY, traceZ]

layout = go.Layout(
    title = 'Delta position between CPF and estimation in ECEF frame',
    xaxis = dict(
        title = 'Datetime UTC'
    ),
    yaxis = dict(
        title = 'Position difference (m)'
    ),
    shapes=[od_window_rectangle]
)

fig = dict(data=data, layout=layout)

pio.show(fig)


# ## 3D plot

# In[38]:


import plotly.graph_objs as go

phi = np.linspace(0, 2*np.pi, num=20)
theta = np.linspace(-np.pi/2, np.pi/2, num=20)
phi, theta = np.meshgrid(phi, theta)

x = 1e-3 * np.cos(theta) * np.sin(phi) * orekit_constants.WGS84_EARTH_EQUATORIAL_RADIUS
y = 1e-3 * np.cos(theta) * np.cos(phi) * orekit_constants.WGS84_EARTH_EQUATORIAL_RADIUS
z = 1e-3 * np.sin(theta) * orekit_constants.WGS84_EARTH_EQUATORIAL_RADIUS

trace_earth = go.Mesh3d(
    x=x.flatten(),
    y=y.flatten(),
    z=z.flatten(),                   
    alphahull=0,                   
    opacity=1.0,
    color='#00AAFF',
    name='Earth',
    lighting=dict(ambient=1))

trace = go.Scatter3d(
    x = 1e-3 * PV_eci_df['x'],
    y = 1e-3 * PV_eci_df['y'],
    z = 1e-3 * PV_eci_df['z'],
    mode='lines',
    line=dict(
        width=3
    ),
    name='Orbit'
)

data = [trace_earth, trace]

layout = go.Layout(
    title = '3D trajectory plot in ECI frame',
    scene=dict(
        aspectmode='data'
    )
)

fig = dict(data=data, layout=layout)

pio.show(fig)


# In[ ]: