/*
    Nyx, blazing fast astrodynamics
    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as published
    by the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
*/

use super::TrajError;
use super::{ExportCfg, Traj};
use crate::State;
use crate::cosmic::{GuidanceMode, Spacecraft};
use crate::dynamics::guidance::{ThrustDirectionReplay, Thruster};
use crate::errors::{FromAlmanacSnafu, NyxError};
use crate::io::{InputOutputError, MissingDataSnafu, ParquetSnafu, StdIOSnafu};
use crate::md::prelude::{Interpolatable, StateParameter};
use crate::time::{Duration, Epoch, TimeUnits};
use anise::analysis::prelude::OrbitalElement;
use anise::astro::Aberration;
use anise::ephemerides::EphemerisError;
use anise::ephemerides::ephemeris::Ephemeris;
use anise::errors::AlmanacError;
use anise::prelude::{Almanac, Frame};
use arrow::array::RecordBatchReader;
use arrow::array::{Array, Float64Array, StringArray};
use hifitime::TimeSeries;
use log::info;
use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder;
use snafu::{ResultExt, ensure};
use std::collections::HashMap;
use std::error::Error;
use std::fs::File;
use std::path::{Path, PathBuf};
use std::sync::Arc;
#[cfg(not(target_arch = "wasm32"))]
use std::time::Instant;

impl Traj<Spacecraft> {
    pub fn to_thrust_direction_replay(&self) -> Arc<ThrustDirectionReplay> {
        ThrustDirectionReplay::from_trajectory(self.clone())
    }

    /// Builds a new trajectory built from the SPICE BSP (SPK) file loaded in the provided Almanac, provided the start and stop epochs.
    ///
    /// If the start and stop epochs are not provided, then the full domain of the trajectory will be used.
    pub fn from_bsp(
        target_frame: Frame,
        observer_frame: Frame,
        almanac: &Almanac,
        sc_template: Spacecraft,
        step: Duration,
        start_epoch: Option<Epoch>,
        end_epoch: Option<Epoch>,
        ab_corr: Option<Aberration>,
        name: Option<String>,
    ) -> Result<Self, AlmanacError> {
        let (domain_start, domain_end) =
            almanac
                .spk_domain(target_frame.ephemeris_id)
                .map_err(|e| AlmanacError::Ephemeris {
                    action: "could not fetch domain",
                    source: Box::new(e),
                })?;

        let start_epoch = start_epoch.unwrap_or(domain_start);
        let end_epoch = end_epoch.unwrap_or(domain_end);

        let time_series = TimeSeries::inclusive(start_epoch, end_epoch, step);
        let mut states = Vec::with_capacity(time_series.len());
        for epoch in time_series {
            let orbit = almanac.transform(target_frame, observer_frame, epoch, ab_corr)?;

            states.push(sc_template.with_orbit(orbit));
        }

        Ok(Self { name, states })
    }
    /// Allows converting the source trajectory into the (almost) equivalent trajectory in another frame
    #[allow(clippy::map_clone)]
    pub fn to_frame(&self, new_frame: Frame, almanac: &Almanac) -> Result<Self, NyxError> {
        if self.states.is_empty() {
            return Err(NyxError::Trajectory {
                source: TrajError::CreationError {
                    msg: "No trajectory to convert".to_string(),
                },
            });
        }

        #[cfg(not(target_arch = "wasm32"))]
        let start_instant = Instant::now();
        let mut traj = Self::new();
        for state in &self.states {
            let new_orbit =
                almanac
                    .transform_to(state.orbit, new_frame, None)
                    .context(FromAlmanacSnafu {
                        action: "transforming trajectory into new frame",
                    })?;
            traj.states.push(state.with_orbit(new_orbit));
        }
        traj.finalize();

        #[cfg(not(target_arch = "wasm32"))]
        info!(
            "Converted trajectory from {} to {} in {} ms: {traj}",
            self.first().orbit.frame,
            new_frame,
            (Instant::now() - start_instant).as_millis()
        );

        #[cfg(target_arch = "wasm32")]
        info!(
            "Converted trajectory from {} to {}: {traj}",
            self.first().orbit.frame,
            new_frame,
        );

        Ok(traj)
    }

    /// Exports this trajectory to the provided filename in parquet format with only the epoch, the geodetic latitude, longitude, and height at one state per minute.
    /// Must provide a body fixed frame to correctly compute the latitude and longitude.
    #[allow(clippy::identity_op)]
    pub fn to_groundtrack_parquet<P: AsRef<Path>>(
        &self,
        path: P,
        body_fixed_frame: Frame,
        metadata: Option<HashMap<String, String>>,
        almanac: &Almanac,
    ) -> Result<PathBuf, Box<dyn Error>> {
        let traj = self.to_frame(body_fixed_frame, almanac)?;

        let mut cfg = ExportCfg::builder()
            .step(1.minutes())
            .fields(vec![
                StateParameter::Element(OrbitalElement::Latitude),
                StateParameter::Element(OrbitalElement::Longitude),
                StateParameter::Element(OrbitalElement::Height),
                StateParameter::Element(OrbitalElement::Rmag),
            ])
            .build();
        cfg.metadata = metadata;

        traj.to_parquet(path, cfg)
    }

    /// Export this spacecraft trajectory estimate to an ANISE Ephemeris
    pub fn to_ephemeris(&self, object_id: String, cfg: ExportCfg) -> Ephemeris {
        let mut ephem = Ephemeris::new(object_id);

        // Build the states iterator -- this does require copying the current states but I can't either get a reference or a copy of all the states.
        let states = if cfg.start_epoch.is_some() || cfg.end_epoch.is_some() || cfg.step.is_some() {
            // Must interpolate the data!
            let start = cfg.start_epoch.unwrap_or_else(|| self.first().epoch());
            let end = cfg.end_epoch.unwrap_or_else(|| self.last().epoch());
            let step = cfg.step.unwrap_or_else(|| 1.minutes());
            self.every_between(step, start, end).collect()
        } else {
            self.states.to_vec()
        };

        for sc_state in &states {
            ephem.insert_orbit(sc_state.orbit());
        }

        ephem
    }

    /// Initialize a new spacecraft trajectory from the path to a CCSDS OEM file.
    ///
    /// CCSDS OEM only contains the orbit information but Nyx builds spacecraft trajectories.
    /// If not spacecraft template is provided, then a default massless spacecraft will be built.
    pub fn from_oem_file<P: AsRef<Path>>(
        path: P,
        tpl_option: Option<Spacecraft>,
    ) -> Result<Self, EphemerisError> {
        // Read the ephemeris
        let ephem = Ephemeris::from_ccsds_oem_file(path)?;
        // Rebuild a trajectory by applying the template
        let template = tpl_option.unwrap_or_default();
        let mut traj = Self::default();
        for record in &ephem {
            traj.states.push(template.with_orbit(record.orbit));
        }
        traj.name = Some(ephem.object_id);

        Ok(traj)
    }

    pub fn to_oem_file<P: AsRef<Path>>(
        &self,
        path: P,
        object_id: String,
        originator: Option<String>,
        object_name: Option<String>,
        cfg: ExportCfg,
    ) -> Result<(), EphemerisError> {
        let ephem = self.to_ephemeris(object_id, cfg);
        ephem.write_ccsds_oem(path, originator, object_name)
    }

    pub fn from_parquet<P: AsRef<Path>>(path: P) -> Result<Self, InputOutputError> {
        let file = File::open(&path).context(StdIOSnafu {
            action: "opening trajectory file",
        })?;

        let builder = ParquetRecordBatchReaderBuilder::try_new(file).unwrap();

        let mut metadata = HashMap::new();
        // Build the custom metadata
        if let Some(file_metadata) = builder.metadata().file_metadata().key_value_metadata() {
            for key_value in file_metadata {
                if !key_value.key.starts_with("ARROW:") {
                    metadata.insert(
                        key_value.key.clone(),
                        key_value.value.clone().unwrap_or("[unset]".to_string()),
                    );
                }
            }
        }

        // Check the schema
        let mut has_epoch = false; // Required
        let mut frame = None;
        let mut has_guidance_mode = false;

        let mut found_fields = vec![
            (StateParameter::Element(OrbitalElement::X), false),
            (StateParameter::Element(OrbitalElement::Y), false),
            (StateParameter::Element(OrbitalElement::Z), false),
            (StateParameter::Element(OrbitalElement::VX), false),
            (StateParameter::Element(OrbitalElement::VY), false),
            (StateParameter::Element(OrbitalElement::VZ), false),
            (StateParameter::DryMass(), false),
            (StateParameter::Isp(), false),
            (StateParameter::PropMass(), false),
            (StateParameter::Thrust(), false),
            (StateParameter::ThrustX(), false),
            (StateParameter::ThrustY(), false),
            (StateParameter::ThrustZ(), false),
        ];

        let reader = builder.build().context(ParquetSnafu {
            action: "building output trajectory file",
        })?;

        for field in &reader.schema().fields {
            if field.name().as_str() == "Epoch (UTC)" {
                has_epoch = true;
            } else if field.name().as_str() == "guidance_mode" {
                has_guidance_mode = true;
            } else {
                for potential_field in &mut found_fields {
                    if field.name() == potential_field.0.to_field(None).name() {
                        potential_field.1 = true;
                        if potential_field.0 != StateParameter::PropMass()
                            && let Some(frame_info) = field.metadata().get("Frame")
                        {
                            // Frame is expected to be serialized as Dhall.
                            match serde_dhall::from_str(frame_info).parse::<Frame>() {
                                Err(e) => {
                                    return Err(InputOutputError::ParseDhall {
                                        data: frame_info.to_string(),
                                        err: format!("{e}"),
                                    });
                                }
                                Ok(deser_frame) => frame = Some(deser_frame),
                            };
                        }
                        break;
                    }
                }
            }
        }

        ensure!(
            has_epoch,
            MissingDataSnafu {
                which: "Epoch (UTC)"
            }
        );

        ensure!(
            frame.is_some(),
            MissingDataSnafu {
                which: "Frame in metadata"
            }
        );

        for (field, exists) in found_fields.iter().take(6) {
            ensure!(
                exists,
                MissingDataSnafu {
                    which: format!("Missing `{}` field", field.to_field(None).name())
                }
            );
        }

        let sc_compat = found_fields
            .iter()
            .find(|(field, _)| *field == StateParameter::PropMass())
            .map(|(_, exists)| *exists)
            .unwrap_or(false);

        let expected_type = std::any::type_name::<Spacecraft>()
            .split("::")
            .last()
            .unwrap();

        if expected_type == "Spacecraft" {
            ensure!(
                sc_compat,
                MissingDataSnafu {
                    which: format!(
                        "Missing `{}` field",
                        found_fields.last().unwrap().0.to_field(None).name()
                    )
                }
            );
        } else if sc_compat {
            // Not a spacecraft, remove the prop mass
            for found_field in &mut found_fields {
                if found_field.0 == StateParameter::PropMass() && found_field.1 {
                    found_field.1 = false;
                    break;
                }
            }
        }

        // At this stage, we know that the measurement is valid and the conversion is supported.
        let mut traj = Traj::default();

        // Now convert each batch on the fly
        for maybe_batch in reader {
            let batch = maybe_batch.unwrap();

            let epochs = batch
                .column_by_name("Epoch (UTC)")
                .unwrap()
                .as_any()
                .downcast_ref::<StringArray>()
                .unwrap();

            let mut shared_data = vec![];
            let guidance_mode_data = if has_guidance_mode {
                Some(
                    batch
                        .column_by_name(StateParameter::GuidanceMode().to_field(None).name())
                        .unwrap()
                        .as_any()
                        .downcast_ref::<StringArray>()
                        .unwrap(),
                )
            } else {
                None
            };

            for (field, exists) in &found_fields {
                if *exists {
                    shared_data.push((
                        *field,
                        batch
                            .column_by_name(field.to_field(None).name())
                            .unwrap()
                            .as_any()
                            .downcast_ref::<Float64Array>()
                            .unwrap(),
                    ));
                }
            }

            // Grab the frame -- it should have been serialized with all of the data so we don't need to reload it.

            // Build the states
            for i in 0..batch.num_rows() {
                let mut state = Spacecraft::zeros();
                state.set_epoch(Epoch::from_gregorian_str(epochs.value(i)).map_err(|e| {
                    InputOutputError::Inconsistency {
                        msg: format!("{e} when parsing epoch"),
                    }
                })?);
                state.set_frame(frame.unwrap()); // We checked it was set above with an ensure! call
                state.unset_stm(); // We don't have any STM data, so let's unset this.
                if found_fields.iter().any(|(field, exists)| {
                    *exists && matches!(field, StateParameter::Isp() | StateParameter::Thrust())
                }) {
                    state.thruster = Some(Thruster {
                        thrust_N: 0.0,
                        isp_s: 0.0,
                    });
                }
                if let Some(guidance_mode_data) = guidance_mode_data {
                    state.mut_mode(match guidance_mode_data.value(i) {
                        "Thrust" => GuidanceMode::Thrust,
                        "Inhibit" => GuidanceMode::Inhibit,
                        _ => GuidanceMode::Coast,
                    });
                }

                for (param, data) in &shared_data {
                    if data.is_valid(i) {
                        state.set_value(*param, data.value(i)).unwrap();
                    }
                }

                traj.states.push(state);
            }
        }

        // Remove any duplicates that may exist in the imported trajectory.
        traj.finalize();

        Ok(traj)
    }
}

#[cfg(test)]
mod ut_ccsds_oem {

    use crate::Spacecraft;
    use crate::State;
    use crate::cosmic::GuidanceMode;
    use crate::dynamics::guidance::{LocalFrame, Objective, Ruggiero, Thruster};
    use crate::md::StateParameter;
    use crate::md::prelude::{OrbitalDynamics, Propagator, SpacecraftDynamics};
    use crate::time::{Epoch, TimeUnits};
    use crate::{Orbit, io::ExportCfg, md::prelude::Traj};
    use anise::almanac::Almanac;
    use anise::analysis::prelude::OrbitalElement;
    use anise::constants::frames::MOON_J2000;
    use arrow::array::{Array, Float64Array};
    use parquet::arrow::arrow_reader::ParquetRecordBatchReaderBuilder;
    use pretty_env_logger;
    use std::env;
    use std::fs::File;
    use std::str::FromStr;
    use std::sync::Arc;
    use std::{collections::HashMap, path::PathBuf};

    #[test]
    fn test_load_oem_leo() {
        // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/
        let path: PathBuf = [
            env!("CARGO_MANIFEST_DIR"),
            "../data",
            "03_tests",
            "ccsds",
            "oem",
            "LEO_10s.oem",
        ]
        .iter()
        .collect();

        let _ = pretty_env_logger::try_init();

        let traj: Traj<Spacecraft> = Traj::from_oem_file(path, None).unwrap();

        // This trajectory has two duplicate epochs, which should be removed by the call to finalize()
        assert_eq!(traj.states.len(), 361);
        assert_eq!(traj.name.unwrap(), "0000-000A".to_string());
    }

    #[test]
    fn test_load_oem_meo() {
        // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/
        let path: PathBuf = [
            env!("CARGO_MANIFEST_DIR"),
            "../data",
            "03_tests",
            "ccsds",
            "oem",
            "MEO_60s.oem",
        ]
        .iter()
        .collect();

        let _ = pretty_env_logger::try_init();

        let traj: Traj<Spacecraft> = Traj::from_oem_file(path, None).unwrap();

        assert_eq!(traj.states.len(), 61);
        assert_eq!(traj.name.unwrap(), "0000-000A".to_string());
    }

    #[test]
    fn test_load_oem_geo() {
        use pretty_env_logger;
        use std::env;

        // All three samples were taken from https://github.com/bradsease/oem/blob/main/tests/samples/real/
        let path: PathBuf = [
            env!("CARGO_MANIFEST_DIR"),
            "../data",
            "03_tests",
            "ccsds",
            "oem",
            "GEO_20s.oem",
        ]
        .iter()
        .collect();

        let _ = pretty_env_logger::try_init();

        let traj: Traj<Spacecraft> = Traj::from_oem_file(path, None).unwrap();

        assert_eq!(traj.states.len(), 181);
        assert_eq!(traj.name.as_ref().unwrap(), &"0000-000A".to_string());

        // Reexport this to CCSDS.
        let cfg = ExportCfg::builder()
            .timestamp(true)
            .metadata(HashMap::from([
                ("originator".to_string(), "Test suite".to_string()),
                ("object_name".to_string(), "TEST_OBJ".to_string()),
            ]))
            .build();

        let path: PathBuf = [
            env!("CARGO_MANIFEST_DIR"),
            "../data",
            "04_output",
            "GEO_20s_rebuilt.oem",
        ]
        .iter()
        .collect();

        traj.to_oem_file(
            &path,
            "0000-000A".to_string(),
            Some("Test Suite".to_string()),
            Some("TEST_OBJ".to_string()),
            cfg,
        )
        .unwrap();
        // And reload, make sure we have the same data.
        let traj_reloaded: Traj<Spacecraft> = Traj::from_oem_file(&path, None).unwrap();

        assert_eq!(traj_reloaded, traj);

        // Now export after trimming one state on either end
        let cfg = ExportCfg::builder()
            .timestamp(true)
            .metadata(HashMap::from([
                ("originator".to_string(), "Test suite".to_string()),
                ("object_name".to_string(), "TEST_OBJ".to_string()),
            ]))
            .step(20.seconds())
            .start_epoch(traj.first().orbit.epoch + 1.seconds())
            .end_epoch(traj.last().orbit.epoch - 1.seconds())
            .build();
        traj.to_oem_file(
            &path,
            "TEST-OBJ-ID".to_string(),
            Some("Test Suite".to_string()),
            Some("TEST_OBJ".to_string()),
            cfg,
        )
        .unwrap();
        // And reload, make sure we have the same data.
        let traj_reloaded: Traj<Spacecraft> = Traj::from_oem_file(path, None).unwrap();

        // Note that the number of states has changed because we interpolated with a step similar to the original one but
        // we started with a different time.
        assert_eq!(traj_reloaded.states.len(), traj.states.len() - 1);
        assert_eq!(
            traj_reloaded.first().orbit.epoch,
            traj.first().orbit.epoch + 1.seconds()
        );
        // Note: because we used a fixed step, the last epoch is actually an offset of step size - end offset
        // from the original trajectory
        assert_eq!(
            traj_reloaded.last().orbit.epoch,
            traj.last().orbit.epoch - 19.seconds()
        );
    }

    #[test]
    fn test_moon_frame_long_prop() {
        use std::path::PathBuf;

        let manifest_dir = PathBuf::from(env!("CARGO_MANIFEST_DIR"));

        let almanac = Almanac::new(
            &manifest_dir
                .clone()
                .join("../data/01_planetary/pck08.pca")
                .to_string_lossy(),
        )
        .unwrap()
        .load(
            &manifest_dir
                .join("../data/01_planetary/de440s.bsp")
                .to_string_lossy(),
        )
        .unwrap();

        let epoch = Epoch::from_str("2022-06-13T12:00:00").unwrap();
        let orbit = Orbit::try_keplerian_altitude(
            350.0,
            0.02,
            30.0,
            45.0,
            85.0,
            0.0,
            epoch,
            almanac.frame_info(MOON_J2000).unwrap(),
        )
        .unwrap();

        let mut traj =
            Propagator::default_dp78(SpacecraftDynamics::new(OrbitalDynamics::two_body()))
                .with(orbit.into(), Arc::new(almanac))
                .for_duration_with_traj(45.days())
                .unwrap()
                .1;
        // Set the name of this object
        traj.name = Some("TEST_MOON_OBJ".to_string());

        // Export CCSDS OEM file
        let path: PathBuf = [
            env!("CARGO_MANIFEST_DIR"),
            "../data",
            "04_output",
            "moon_45days.oem",
        ]
        .iter()
        .collect();

        traj.to_oem_file(
            &path,
            "TEST_MOON_OBJ".to_string(),
            Some("Test Suite".to_string()),
            Some("TEST_MOON_OBJ".to_string()),
            ExportCfg::default(),
        )
        .unwrap();

        // And reload
        let traj_reloaded: Traj<Spacecraft> = Traj::from_oem_file(path, None).unwrap();

        assert_eq!(traj, traj_reloaded);
    }

    #[test]
    fn test_parquet_exports_thrust_angles() {
        let _ = pretty_env_logger::try_init();

        let manifest_dir = PathBuf::from(env!("CARGO_MANIFEST_DIR"));

        let almanac = Almanac::new(
            &manifest_dir
                .clone()
                .join("../data/01_planetary/pck08.pca")
                .to_string_lossy(),
        )
        .unwrap()
        .load(
            &manifest_dir
                .join("../data/01_planetary/de440s.bsp")
                .to_string_lossy(),
        )
        .unwrap();

        let almanac = Arc::new(almanac);

        let eme2k = almanac
            .frame_info(anise::constants::frames::EARTH_J2000)
            .unwrap();
        let epoch = Epoch::from_gregorian_utc_at_noon(2021, 1, 1);
        let orbit = Orbit::try_keplerian_altitude(900.0, 5e-5, 5e-3, 0.0, 178.0, 0.0, epoch, eme2k)
            .unwrap();

        let objectives = &[Objective::within_tolerance(
            StateParameter::Element(OrbitalElement::AoP),
            183.0,
            5e-3,
        )];

        let guidance = Ruggiero::simple(objectives, orbit.into()).unwrap();
        let spacecraft = Spacecraft::from_thruster(
            orbit,
            300.0,
            67.0,
            Thruster {
                thrust_N: 89e-3,
                isp_s: 1650.0,
            },
            GuidanceMode::Thrust,
        );

        let (_, traj) = Propagator::default(SpacecraftDynamics::from_guidance_law(
            OrbitalDynamics::two_body(),
            guidance,
        ))
        .with(spacecraft, almanac.clone())
        .for_duration_with_traj(5.minutes())
        .unwrap();

        let path = manifest_dir
            .join("../data/04_output")
            .join("thrust_axes_export_test.parquet");

        traj.to_parquet(&path, ExportCfg::default()).unwrap();

        // Reload the trajectory and replay
        let reloaded = Traj::<Spacecraft>::from_parquet(&path).unwrap();
        assert_eq!(reloaded.first().mode(), GuidanceMode::Thrust);
        assert!(
            reloaded
                .states
                .iter()
                .skip(1)
                .any(|state| state.thrust_direction().is_some())
        );

        let reader = ParquetRecordBatchReaderBuilder::try_new(File::open(&path).unwrap())
            .unwrap()
            .build()
            .unwrap();

        let in_plane_name = StateParameter::ThrustInPlane(LocalFrame::RCN)
            .to_field(None)
            .name()
            .to_string();
        let out_of_plane_name = StateParameter::ThrustOutOfPlane(LocalFrame::RCN)
            .to_field(None)
            .name()
            .to_string();

        let batch = reader.into_iter().next().unwrap().unwrap();
        let in_plane = batch
            .column_by_name(&in_plane_name)
            .unwrap()
            .as_any()
            .downcast_ref::<Float64Array>()
            .unwrap();
        let out_of_plane = batch
            .column_by_name(&out_of_plane_name)
            .unwrap()
            .as_any()
            .downcast_ref::<Float64Array>()
            .unwrap();

        assert!(in_plane.is_null(0));
        assert!(out_of_plane.is_null(0));
        assert!((1..batch.num_rows()).any(|idx| in_plane.is_valid(idx)));
        assert!((1..batch.num_rows()).any(|idx| out_of_plane.is_valid(idx)));

        let replay_guidance = reloaded.to_thrust_direction_replay();
        let replay_initial_state = reloaded.first().to_owned();
        let replay_duration = reloaded.last().epoch() - reloaded.first().epoch();
        let replay_dynamics =
            SpacecraftDynamics::from_guidance_law(OrbitalDynamics::two_body(), replay_guidance);
        let (replayed_end, _) = Propagator::default(replay_dynamics)
            .with(replay_initial_state, almanac)
            .for_duration_with_traj(replay_duration)
            .unwrap();

        let truth_end = traj.last();
        let pos_err_km = (replayed_end.orbit.radius_km - truth_end.orbit.radius_km).norm();
        let vel_err_km_s =
            (replayed_end.orbit.velocity_km_s - truth_end.orbit.velocity_km_s).norm();
        let prop_err_kg = (replayed_end.mass.prop_mass_kg - truth_end.mass.prop_mass_kg).abs();

        assert!(
            dbg!(pos_err_km) < 1e-3,
            "replay position error too large: {pos_err_km} km"
        );
        assert!(
            dbg!(vel_err_km_s) < 1e-5,
            "replay velocity error too large: {vel_err_km_s} km/s"
        );
        assert!(
            dbg!(prop_err_kg) < 1e-8,
            "replay prop mass error too large: {prop_err_kg} kg"
        );
    }
}
