# %%
# Measurement generator

'''
This program is a tool to generate measurements of satellites as observed from a configured ground station.
Range , Azimuth , Elevation with appropriately configured random noise will be generated.
'''



import orekit
print('================= Orekit Initialization ================')
vm = orekit.initVM()
print ('Java version:',vm.java_version)
print ('Orekit version:', orekit.VERSION)
print('========================================================')

#setup orekit 
from orekit.pyhelpers import setup_orekit_curdir
# orekit directory path
orekit_dir = "C:\\Users\\radhakrishna\\Documents\\orekit-files\\orekit-data-main"
setup_orekit_curdir(orekit_dir)



# %%
tle='''0 SENTINEL-1C                
1 62261U 24235A   25218.58972421 -.00001515  00000-0 -31181-3 0  9990
2 62261  98.1819 225.4143 0001316  85.5830 274.5520 14.59197729 35531'''

from org.orekit.propagation.analytical.tle import TLE
from org.orekit.propagation.analytical.tle import TLEPropagator
from org.orekit.time import AbsoluteDate, TimeScalesFactory
from org.orekit.frames import FramesFactory, TopocentricFrame
from org.orekit.orbits import CartesianOrbit


tle=TLE(tle.splitlines()[1], tle.splitlines()[2])
propagator = TLEPropagator.selectExtrapolator(tle)
epoch=tle.getDate()
satelliteState=propagator.propagate(epoch)
#the orbit has frame inbuilt so need not worry about the frame here, but yeah, TLE calcs are done in TEME frame


initial_orbit=satelliteState.getOrbit()
initial_frame = initial_orbit.getFrame()

gcrf = FramesFactory.getGCRF()
transform = initial_frame.getTransformTo(gcrf, epoch)

pvIngcrf = transform.transformPVCoordinates(initial_orbit.getPVCoordinates())

mu=initial_orbit.getMu()
initial_orbit = CartesianOrbit(pvIngcrf, gcrf, epoch, mu)
print('Initial Orbit at epoch:', initial_orbit)

#ground station

from org.orekit.frames import FramesFactory
from org.orekit.utils import IERSConventions
from org.orekit.estimation.measurements import GroundStation
from org.orekit.bodies import OneAxisEllipsoid, GeodeticPoint
from org.orekit.frames import TopocentricFrame
from math import radians,degrees
# Define ITRF and Earth model
itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, True)
earth = OneAxisEllipsoid(6378137.0, 1.0 / 298.257223563, itrf)

# Define a geodetic point for the ground station

latitude_deg = 17.92 # radians
longitude_deg = 78.75 # radians
altitude = 500.0 # meters
point = GeodeticPoint(radians(latitude_deg), radians(longitude_deg), altitude)

# Create a TopocentricFrame for the ground station
station_frame = TopocentricFrame(earth, point, "radar")

# Create the GroundStation object
ground_station = GroundStation(station_frame)
print(ground_station)





# %%
from orekit import JArray_double
from orekit.pyhelpers import JArray_double2D
from org.orekit.propagation.numerical import NumericalPropagator
from org.hipparchus.ode.nonstiff import DormandPrince853Integrator
from org.orekit.orbits import OrbitType
from org.orekit.propagation import SpacecraftState
from org.orekit.utils import Constants
from org.orekit.forces.gravity import HolmesFeatherstoneAttractionModel
from org.orekit.forces.gravity.potential import GravityFieldFactory
from org.orekit.forces.radiation import SolarRadiationPressure, IsotropicRadiationSingleCoefficient
from org.orekit.forces.drag import DragForce, IsotropicDrag
from org.orekit.models.earth.atmosphere import DTM2000
from org.orekit.models.earth.atmosphere.data import CssiSpaceWeatherData
from org.orekit.forces.gravity import ThirdBodyAttraction
from org.orekit.bodies import CelestialBodyFactory

def GetNumericalPropagator(initial_orbit):
    min_step, max_step, init_step, pos_tol = 0.001, 1000.0, 60.0, 0.01
    # eme2000 = FramesFactory.getEME2000()
    # initial_state_eme2000 = satelliteState.toFrame(eme2000)
    init_orbit = initial_orbit
    tolerances = NumericalPropagator.tolerances(pos_tol, initial_orbit, initial_orbit.getType())
    # Integrator for Az/El
    integrator= DormandPrince853Integrator(
        min_step, max_step, JArray_double.cast_(tolerances[0]), JArray_double.cast_(tolerances[1])
    )
    integrator.setInitialStepSize(init_step)
    
    # Propagators
    propagator = NumericalPropagator(integrator)
    propagator.setOrbitType(OrbitType.CARTESIAN)
    propagator.setInitialState(SpacecraftState(initial_orbit))

    # Define force models
    itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, True)
    earth = OneAxisEllipsoid(
        Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, itrf
    )
    gravity_provider = GravityFieldFactory.getNormalizedProvider(120, 120)
    force_models = [
        HolmesFeatherstoneAttractionModel(itrf, gravity_provider),
        # ThirdBodyAttraction(CelestialBodyFactory.getMoon()),
        # ThirdBodyAttraction(CelestialBodyFactory.getSun()),
        # SolarRadiationPressure(
        #     CelestialBodyFactory.getSun(), earth, IsotropicRadiationSingleCoefficient(10.0, 1.2)
        # ),
        # DragForce(
        #     DTM2000(CssiSpaceWeatherData("SpaceWeather-All-v1.2.txt"), 
        #             CelestialBodyFactory.getSun(), earth),
        #     IsotropicDrag(10.0, 1.2)
        # )
    ]
    
    # Add force models to both propagators
    for fm in force_models:
        propagator.addForceModel(fm)
        

    print(f'NumericalPropagator setup complete with {len(force_models)} force models.')
    return propagator

numerical_propagator = GetNumericalPropagator(initial_orbit)

# %%



# --- 5. Configure Measurement Generation with Noise ---
from org.hipparchus.linear import ArrayRealVector,Array2DRowRealMatrix
from org.hipparchus.random import CorrelatedRandomVectorGenerator
from org.hipparchus.random import GaussianRandomGenerator
from org.hipparchus.random import Well19937a
from orekit import JArray_double, JArray
from orekit.pyhelpers import JArray_double2D
from org.orekit.estimation.measurements.generation import Generator, RangeBuilder,AngularAzElBuilder, EventBasedScheduler, SignSemantic, GatheringSubscriber
from org.orekit.estimation.measurements import GroundStation, ObservedMeasurement , ObservableSatellite


# Noise Standard Deviations (Sigma values)
# Azimuth and Elevation sigma in radians
az_el_sigma_deg = 1 #degrees
az_el_mean=JArray_double([0.0, 0.0])
az_el_sigma_rad = radians(az_el_sigma_deg)
range_mean=JArray_double([0.0])
range_sigma = 1000.0   # Range sigma in meters



# Use a generator to produce measurements
generator = Generator()
# Add the satellite to be observed
sat = generator.addPropagator(numerical_propagator)
observable_satellite = ObservableSatellite(0)
# Set up noise for Az/El
# Covariance matrix: diagonal matrix with sigma^2. No correlation

def get_angle_noise_generator(mean,sigma):
     # mean format => JArray_double([0.0, 0.0])

    sigma_az=sigma
    sigma_el=sigma

    var_az=sigma_az*sigma_az
    var_el=sigma_el*sigma_el
    cov_azel=0.0#var_az/7
    cov_elaz=0.0#var_az/7
    matrix=JArray_double2D(2,2)
    matrix[0]=JArray_double([var_az, cov_azel])
    matrix[1]=JArray_double([cov_elaz, var_el])

    cvm=Array2DRowRealMatrix(matrix)

    #Diagonal elements threshold under which column are considered to be dependent on previous ones and are discarded.
    small = 1e-10
    n_r_g = GaussianRandomGenerator(Well19937a())
    c_r_v_g = CorrelatedRandomVectorGenerator(mean,cvm,small,n_r_g)

    return c_r_v_g

def get_range_noise_generator(mean,sigma):

    
    cov=sigma*sigma
    
    matrix=JArray_double2D(1,1)
    matrix[0]=JArray_double([cov])
    cvm=Array2DRowRealMatrix(matrix)

    #Diagonal elements threshold under which column are considered to be dependent on previous ones and are discarded.
    small = 1e-10
    n_r_g = GaussianRandomGenerator(Well19937a())
    c_r_v_g_nonzero_cov = CorrelatedRandomVectorGenerator(mean,cvm,small,n_r_g)


    return c_r_v_g_nonzero_cov

angle_noise=get_angle_noise_generator(az_el_mean,az_el_sigma_rad)
range_noise=get_range_noise_generator(range_mean,range_sigma)

# Set up measurement builders

# create a builder
ang_azel_builder=AngularAzElBuilder(angle_noise,
    ground_station,
    [az_el_sigma_rad, az_el_sigma_rad], 
    [1.0, 1.0],  # Base weight
    observable_satellite
)

range_builder = RangeBuilder(range_noise,
        ground_station,
        False, # twoway
        range_sigma,  # sigma
        1.0, # weight
        observable_satellite
)







# %%
from org.orekit.estimation.measurements.generation import Generator, RangeBuilder,AngularAzElBuilder, EventBasedScheduler, SignSemantic, GatheringSubscriber
from org.orekit.propagation.events import ElevationDetector
from org.orekit.propagation.events.handlers import ContinueOnEvent
from org.orekit.time import FixedStepSelector


 #---
timescale=TimeScalesFactory.getUTC()
az_el_step_selector = FixedStepSelector(60.0,timescale)
range_step_selector = FixedStepSelector(60.0,timescale)

#---

azel_scheduler=EventBasedScheduler(
    ang_azel_builder,
    az_el_step_selector,
    numerical_propagator, 
    ElevationDetector(station_frame).withHandler(ContinueOnEvent()), 
    SignSemantic.FEASIBLE_MEASUREMENT_WHEN_POSITIVE
)

range_scheduler=EventBasedScheduler(
    range_builder,
    range_step_selector,
    numerical_propagator, 
    ElevationDetector(station_frame).withHandler(ContinueOnEvent()), 
    SignSemantic.FEASIBLE_MEASUREMENT_WHEN_POSITIVE
)

generator.addScheduler(azel_scheduler)
generator.addScheduler(range_scheduler)

start_date=epoch
end_date=epoch.shiftedBy(3600.0*24*4) # 4 days



measurements_collector = GatheringSubscriber()
generator.addSubscriber(measurements_collector)
generator.generate(start_date, end_date)
measurements = measurements_collector.getGeneratedMeasurements().toArray()

# %%
print(len(measurements))

# %%
from org.orekit.estimation.measurements import EstimatedMeasurementBase
from orekit.pyhelpers import absolutedate_to_datetime,datetime_to_absolutedate
import pandas as pd

meas_dict={
    "Range":[],
    "Azimuth_deg":[],
    "Elevation_deg":[],
    "Time":[],
    "Lat":[],
    "Lon":[],
    "Alt":[],
    "Az_sigma_deg":[],
    "El_sigma_deg":[],
    "Range_sigma":[]
}
for m in measurements:
    obs_meas=EstimatedMeasurementBase.cast_(m).getObservedMeasurement()
    val=obs_meas.getObservedValue()
    time=obs_meas.getDate()
    #convert to python datetime
    python_time=absolutedate_to_datetime(time)
    #CONVERT TO PROPER FORMAT STRING
    observation_time=python_time.strftime('%Y-%m-%dT%H:%M:%S.%f')+'Z'
    
    meas_type=obs_meas.getMeasurementType()
    if meas_type=="Range":
        meas_dict['Range'].append(val[0])
        meas_dict['Time'].append(observation_time)
        meas_dict['Lat'].append(latitude_deg)
        meas_dict['Lon'].append(longitude_deg)
        meas_dict['Alt'].append(altitude)
        meas_dict["Range_sigma"].append(range_sigma)
    elif meas_type=="AngularAzEl":
        meas_dict["Azimuth_deg"].append(degrees(val[0]))
        meas_dict["Elevation_deg"].append(degrees(val[1]))
        meas_dict["Az_sigma_deg"].append(az_el_sigma_deg)
        meas_dict["El_sigma_deg"].append(az_el_sigma_deg)

#convert to dataframe
measurement_df=pd.DataFrame(meas_dict)
print(measurement_df)
        
        
    

# %%
measurement_df.plot(x="Time",y=["Elevation_deg"])
import datetime
now_time=datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
path="C:\\Users\\radhakrishna\\Documents\\projects\\lrde_modelling\\simulated_measurements"
measurement_df.to_csv(f'{path}\\generated_OREKIT_RADAR_angsig_{az_el_sigma_deg}deg_rangesig_{range_sigma}m_TIME_{now_time}.csv')

# %%


# %%


# %%


# %%