// Test for interlink

extern crate nyx_space as nyx;
extern crate pretty_env_logger;

use anise::constants::celestial_objects::{EARTH, SUN};
use anise::constants::frames::IAU_MOON_FRAME;
use indexmap::IndexSet;
use nyx::Spacecraft;
use nyx::cosmic::Orbit;
use nyx::dynamics::SpacecraftDynamics;
use nyx::dynamics::orbital::OrbitalDynamics;
use nyx::md::prelude::*;
use nyx::od::prelude::*;
use nyx::propagators::Propagator;
use nyx::time::{Epoch, TimeUnits};

use anise::prelude::Almanac;
use rstest::*;
use std::collections::BTreeMap;
use std::sync::Arc;

#[fixture]
fn almanac() -> Arc<Almanac> {
    use crate::test_almanac_arcd;
    test_almanac_arcd()
}

/// Ensures that a propagation in a non-J2000 frame is strictly identical to the pure predictor propagation
#[rstest]
fn val_pure_predictor(almanac: Arc<Almanac>) {
    let _ = pretty_env_logger::try_init();

    let moon_iau = almanac.frame_info(IAU_MOON_FRAME).unwrap();

    let prop_time = 1.hours();

    let epoch = Epoch::from_gregorian_tai_at_midnight(2025, 1, 1);

    let bodies = vec![EARTH, SUN];
    let dynamics = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies));

    let setup = Propagator::rk89(
        dynamics,
        IntegratorOptions::builder().max_step(1.minutes()).build(),
    );

    let llo_orbit =
        Orbit::try_keplerian_altitude(110.0, 1e-4, 90.0, 0.0, 0.0, 0.0, epoch, moon_iau).unwrap();

    let llo_sc = Spacecraft::builder().orbit(llo_orbit).build();

    let mut this_prop = setup.with(llo_sc, almanac.clone());

    let (_, llo_traj) = this_prop.for_duration_with_traj(prop_time).unwrap();

    /* == Setup the OD process == */

    let mut measurement_types = IndexSet::new();
    measurement_types.insert(MeasurementType::Range);
    measurement_types.insert(MeasurementType::Doppler);

    // Devices are the transmitter, which is our NRHO vehicle.
    let mut devices = BTreeMap::new();
    devices.insert(
        "Canberra".to_string(),
        GroundStation::dss34_canberra(
            0.0,
            StochasticNoise::default_range_km(),
            StochasticNoise::default_doppler_km_s(),
        ),
    );

    let mut configs = BTreeMap::new();
    configs.insert(
        "NRHO Tx SC".to_string(),
        TrkConfig::builder()
            .strands(vec![Strand {
                start: epoch,
                end: epoch + prop_time,
            }])
            .build(),
    );

    // Run a truth OD where we estimate the LLO position
    let llo_uncertainty = SpacecraftUncertainty::builder()
        .nominal(llo_sc)
        .x_km(1.0)
        .y_km(1.0)
        .z_km(1.0)
        .vx_km_s(1e-3)
        .vy_km_s(1e-3)
        .vz_km_s(1e-3)
        .build();

    let initial_estimate = llo_uncertainty.to_estimate().unwrap();

    let odp = SpacecraftKalmanOD::new(
        setup,
        KalmanVariant::DeviationTracking,
        None,
        devices,
        almanac,
    );

    let pred_truth = llo_traj.last();

    let pp = odp
        .predict_until(initial_estimate, pred_truth.epoch())
        .unwrap();

    let final_est = pp.estimates.last().unwrap().orbital_state();
    let err = final_est.ric_difference(&pred_truth.orbit).unwrap();
    assert!(err.rmag_km() < f64::EPSILON);
    assert!(err.vmag_km_s() < f64::EPSILON);
}
