#!/usr/bin/env python3
import numpy as np
from datetime import datetime, timedelta
import orekit_jpype as orekit
orekit.initVM()
from orekit_jpype.pyhelpers import setup_orekit_data
from java.util import ArrayList # type: ignore
from org.hipparchus.geometry.euclidean.threed import Vector3D # type: ignore
from org.orekit.data import DataContext, DataSource # type: ignore
from org.orekit.files.ccsds.ndm import ParserBuilder # type: ignore
from org.orekit.propagation import SpacecraftState # type: ignore
from org.orekit.propagation.analytical import Ephemeris # type: ignore
from org.orekit.propagation.analytical.tle import TLE, TLEPropagator # type: ignore
from org.orekit.utils import IERSConventions # type:ignore


orekit.pyhelpers.setup_orekit_data('orekit-data', False)
dc = DataContext.getDefault()
EME2000 = dc.getFrames().getEME2000()
ITRF = dc.getFrames().getITRF(IERSConventions.IERS_1996, True)
UTC = dc.getTimeScales().getUTC()

orekit_t0 = datetime.fromisoformat("2025-05-01T00:00:00") # from OCM
orekit_t_grid = np.linspace(0, 3600*6, 361)

#tleLine1 = "1 39453U 13067C   25192.21673508  .00005034  00000-0  11431-3 0  9997"
#tleLine2 = "2 39453  87.3412 180.4819 0003176  90.8504 269.3117 15.42753470653556"
tleLine1 = "1 39453U 13067C   25121.75162400  .00012848  00000-0  30392-3 0  9990"
tleLine2 = "2 39453  87.3442 206.2042 0002682 103.1251 257.0305 15.41673805642690"
orekitTle = TLE(tleLine1, tleLine2)
sgp4Propagator = TLEPropagator.selectExtrapolator(orekitTle)
sgp4_positions = []
for orekit_epoch in orekit_t_grid:
        tlePV_ECI = sgp4Propagator.getPVCoordinates(orekit_t0 + timedelta(seconds=orekit_epoch), ITRF)
        sgp4_positions.append(tlePV_ECI.getPosition().toArray())
sgp4_positions = np.array(sgp4_positions) / 1000
print(sgp4_positions[0])

ocm = ParserBuilder().buildOcmParser().parse(DataSource("39453_2025-07-10_CPF.ocm"))
eph = ocm.getSatellites().get("SWARM C")
prop = eph.getPropagator()
cpf_positions = []
orekit_t_grid = np.linspace(0, 3600*10, 361)
for orekit_epoch in orekit_t_grid:
        cpf_ECI    = prop.getPVCoordinates(orekit_t0 + timedelta(seconds=orekit_epoch), ITRF)
        cpf_positions.append(cpf_ECI.getPosition().toArray())
cpf_positions = np.array(cpf_positions) / 1000
print(cpf_positions[0])

from matplotlib import pyplot as plt
# Plotting
fig = plt.figure(figsize=(10, 7))
ax = fig.add_subplot(111, projection='3d')

# Plot CPF positions
ax.plot(
    cpf_positions[:, 0],
    cpf_positions[:, 1],
    cpf_positions[:, 2],
    label='CPF Ephemeris',
    color='blue',
    linewidth=2
)

# Plot SGP4 positions
ax.plot(
    sgp4_positions[:, 0],
    sgp4_positions[:, 1],
    sgp4_positions[:, 2],
    label='SGP4 Propagation',
    color='red',
    linestyle='--',
    linewidth=2
)

# Labels and title
ax.set_xlabel("X [km]")
ax.set_ylabel("Y [km]")
ax.set_zlabel("Z [km]")
ax.set_title("CPF vs SGP4 Trajectories in ECI Frame")
ax.legend()

plt.tight_layout()
plt.show()