lox-frames 0.1.0-alpha.16

// SPDX-FileCopyrightText: 2025 Helge Eichhorn <git@helgeeichhorn.de>
//
// SPDX-License-Identifier: MPL-2.0

use std::{fmt::Display, ops::Mul};

use lox_bodies::{TryRotationalElements, UndefinedOriginPropertyError};
use lox_core::glam::{DMat3, DVec3};
use lox_core::{coords::Cartesian, f64::consts::ROTATION_RATE_EARTH};
use lox_test_utils::ApproxEq;
use lox_time::{
    Time,
    julian_dates::JulianDate,
    offsets::{OffsetProvider, TryOffset},
    time_scales::{Tdb, TimeScale, Tt, Ut1},
};
use thiserror::Error;

use crate::{
    Iau, ReferenceFrame,
    iau::icrf_to_iau,
    iers::{
        Corrections, ReferenceSystem,
        cio::CioLocator,
        cip::CipCoords,
        earth_rotation::{EarthRotationAngle, EquationOfTheEquinoxes},
        polar_motion::PoleCoords,
        precession::frame_bias,
    },
};

mod impls;

/// Computes the rotation from one reference frame to another at a given time.
pub trait TryRotation<Origin, Target, T>
where
    Origin: ReferenceFrame,
    Target: ReferenceFrame,
    T: TimeScale,
{
    /// The error type returned when the rotation cannot be computed.
    type Error: std::error::Error + Send + Sync + 'static;

    /// Computes the rotation from `origin` to `target` at the given `time`.
    fn try_rotation(
        &self,
        origin: Origin,
        target: Target,
        time: Time<T>,
    ) -> Result<Rotation, Self::Error>;
}

/// Computes a composed rotation through multiple intermediate frames.
pub trait TryComposedRotation<T, P>
where
    T: TimeScale + Copy,
{
    /// Computes the composed rotation at the given time using the provider.
    fn try_composed_rotation(&self, provider: &P, time: Time<T>)
    -> Result<Rotation, RotationError>;
}

impl<T, P, R1, R2, R3> TryComposedRotation<T, P> for (R1, R2, R3)
where
    T: TimeScale + Copy,
    R1: ReferenceFrame + Copy,
    R2: ReferenceFrame + Copy,
    R3: ReferenceFrame + Copy,
    P: RotationProvider<T>
        + TryRotation<R1, R2, T, Error = RotationError>
        + TryRotation<R2, R3, T, Error = RotationError>,
{
    fn try_composed_rotation(
        &self,
        provider: &P,
        time: Time<T>,
    ) -> Result<Rotation, RotationError> {
        Ok(provider
            .try_rotation(self.0, self.1, time)?
            .compose(provider.try_rotation(self.1, self.2, time)?))
    }
}

impl<T, P, R1, R2, R3, R4> TryComposedRotation<T, P> for (R1, R2, R3, R4)
where
    T: TimeScale + Copy,
    R1: ReferenceFrame + Copy,
    R2: ReferenceFrame + Copy,
    R3: ReferenceFrame + Copy,
    R4: ReferenceFrame + Copy,
    P: RotationProvider<T>
        + TryRotation<R1, R2, T, Error = RotationError>
        + TryRotation<R2, R3, T, Error = RotationError>
        + TryRotation<R3, R4, T, Error = RotationError>,
{
    fn try_composed_rotation(
        &self,
        provider: &P,
        time: Time<T>,
    ) -> Result<Rotation, RotationError> {
        Ok(provider
            .try_rotation(self.0, self.1, time)?
            .compose(provider.try_rotation(self.1, self.2, time)?)
            .compose(provider.try_rotation(self.2, self.3, time)?))
    }
}

impl<T, P, R1, R2, R3, R4, R5> TryComposedRotation<T, P> for (R1, R2, R3, R4, R5)
where
    T: TimeScale + Copy,
    R1: ReferenceFrame + Copy,
    R2: ReferenceFrame + Copy,
    R3: ReferenceFrame + Copy,
    R4: ReferenceFrame + Copy,
    R5: ReferenceFrame + Copy,
    P: RotationProvider<T>
        + TryRotation<R1, R2, T, Error = RotationError>
        + TryRotation<R2, R3, T, Error = RotationError>
        + TryRotation<R3, R4, T, Error = RotationError>
        + TryRotation<R4, R5, T, Error = RotationError>,
{
    fn try_composed_rotation(
        &self,
        provider: &P,
        time: Time<T>,
    ) -> Result<Rotation, RotationError> {
        Ok(provider
            .try_rotation(self.0, self.1, time)?
            .compose(provider.try_rotation(self.1, self.2, time)?)
            .compose(provider.try_rotation(self.2, self.3, time)?)
            .compose(provider.try_rotation(self.3, self.4, time)?))
    }
}

impl<T, P, R1, R2, R3, R4, R5, R6> TryComposedRotation<T, P> for (R1, R2, R3, R4, R5, R6)
where
    T: TimeScale + Copy,
    R1: ReferenceFrame + Copy,
    R2: ReferenceFrame + Copy,
    R3: ReferenceFrame + Copy,
    R4: ReferenceFrame + Copy,
    R5: ReferenceFrame + Copy,
    R6: ReferenceFrame + Copy,
    P: RotationProvider<T>
        + TryRotation<R1, R2, T, Error = RotationError>
        + TryRotation<R2, R3, T, Error = RotationError>
        + TryRotation<R3, R4, T, Error = RotationError>
        + TryRotation<R4, R5, T, Error = RotationError>
        + TryRotation<R5, R6, T, Error = RotationError>,
{
    fn try_composed_rotation(
        &self,
        provider: &P,
        time: Time<T>,
    ) -> Result<Rotation, RotationError> {
        Ok(provider
            .try_rotation(self.0, self.1, time)?
            .compose(provider.try_rotation(self.1, self.2, time)?)
            .compose(provider.try_rotation(self.2, self.3, time)?)
            .compose(provider.try_rotation(self.3, self.4, time)?)
            .compose(provider.try_rotation(self.4, self.5, time)?))
    }
}

/// The source of a rotation error.
#[derive(Debug)]
pub enum RotationErrorKind {
    /// Time scale offset computation failed.
    Offset,
    /// Earth orientation parameter lookup failed.
    Eop,
}

impl Display for RotationErrorKind {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            RotationErrorKind::Offset => "offset error".fmt(f),
            RotationErrorKind::Eop => "EOP error".fmt(f),
        }
    }
}

/// A rotation computation failed due to a time offset or EOP error.
#[derive(Debug, Error)]
#[error("{kind}: {error}")]
pub struct RotationError {
    kind: RotationErrorKind,
    error: Box<dyn std::error::Error + Send + Sync + 'static>,
}

impl RotationError {
    /// Creates a rotation error from a time offset error.
    pub fn offset(err: impl std::error::Error + Send + Sync + 'static) -> Self {
        RotationError {
            kind: RotationErrorKind::Offset,
            error: Box::new(err),
        }
    }

    /// Creates a rotation error from an EOP error.
    pub fn eop(err: impl std::error::Error + Send + Sync + 'static) -> Self {
        RotationError {
            kind: RotationErrorKind::Eop,
            error: Box::new(err),
        }
    }
}

/// Errors from dynamic-dispatch rotation computations.
#[derive(Debug, Error)]
pub enum DynRotationError {
    /// Underlying rotation error (offset or EOP).
    #[error(transparent)]
    Offset(#[from] RotationError),
    /// The origin and target frames use incompatible IERS reference systems.
    #[error("incompatible reference systems")]
    IncompatibleReferenceSystems,
    /// A required body property is undefined.
    #[error(transparent)]
    UndefinedProperty(#[from] UndefinedOriginPropertyError),
}

/// Provides Earth orientation data and frame rotation methods for a given time scale.
pub trait RotationProvider<T: TimeScale>: OffsetProvider {
    /// The error type for EOP lookups.
    type EopError: std::error::Error + Send + Sync + 'static;

    /// Returns nutation/precession corrections for the given reference system.
    fn corrections(
        &self,
        time: Time<T>,
        sys: ReferenceSystem,
    ) -> Result<Corrections, Self::EopError>;
    /// Returns polar motion coordinates at the given time.
    fn pole_coords(&self, time: Time<T>) -> Result<PoleCoords, Self::EopError>;

    /// Rotation from ICRF to an IAU body-fixed frame.
    fn icrf_to_iau<R>(&self, time: Time<T>, frame: Iau<R>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        R: TryRotationalElements,
        Self: TryOffset<T, Tdb>,
    {
        let seconds = time
            .try_to_scale(Tdb, self)
            .map_err(RotationError::offset)?
            .seconds_since_j2000();
        let angles = frame.rotational_elements(seconds);
        let rates = frame.rotational_element_rates(seconds);

        Ok(icrf_to_iau(angles, rates))
    }
    /// Rotation from an IAU body-fixed frame to ICRF.
    fn iau_to_icrf<R>(&self, time: Time<T>, frame: Iau<R>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        R: TryRotationalElements,
        Self: TryOffset<T, Tdb>,
    {
        Ok(self.icrf_to_iau(time, frame)?.transpose())
    }

    /// Rotation from ICRF to ITRF (via CIO-based path).
    fn icrf_to_itrf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb> + TryOffset<T, Tt> + TryOffset<T, Ut1>,
    {
        Ok(self
            .icrf_to_cirf(time)?
            .compose(self.cirf_to_tirf(time)?)
            .compose(self.tirf_to_itrf(time)?))
    }
    /// Rotation from ITRF to ICRF.
    fn itrf_to_icrf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb> + TryOffset<T, Tt> + TryOffset<T, Ut1>,
    {
        Ok(self.icrf_to_itrf(time)?.transpose())
    }

    /// Rotation from ICRF to J2000 (frame bias).
    fn icrf_to_j2000(&self) -> Rotation {
        Rotation::new(frame_bias())
    }

    /// Rotation from J2000 to ICRF (inverse frame bias).
    fn j2000_to_icrf(&self) -> Rotation {
        Rotation::new(frame_bias().transpose())
    }

    /// Rotation from J2000 to Mean of Date.
    fn j2000_to_mod(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        let time = time.try_to_scale(Tt, self).map_err(RotationError::offset)?;
        Ok(sys.precession_matrix(time).into())
    }

    /// Rotation from Mean of Date to J2000.
    fn mod_to_j2000(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        Ok(self.j2000_to_mod(time, sys)?.transpose())
    }

    /// Rotation from ICRF to Mean of Date (bias + precession).
    fn icrf_to_mod(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        let time = time.try_to_scale(Tt, self).map_err(RotationError::offset)?;
        Ok(sys.bias_precession_matrix(time).into())
    }
    /// Rotation from Mean of Date to ICRF.
    fn mod_to_icrf(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        Ok(self.icrf_to_mod(time, sys)?.transpose())
    }

    /// Rotation from Mean of Date to True of Date (nutation).
    fn mod_to_tod(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb>,
    {
        let tdb = time
            .try_to_scale(Tdb, self)
            .map_err(RotationError::offset)?;
        let corr = self.corrections(time, sys).map_err(RotationError::eop)?;
        Ok(sys.nutation_matrix(tdb, corr).into())
    }
    /// Rotation from True of Date to Mean of Date.
    fn tod_to_mod(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb>,
    {
        Ok(self.mod_to_tod(time, sys)?.transpose())
    }

    /// Rotation from True of Date to Pseudo-Earth Fixed (Earth rotation).
    fn tod_to_pef(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt> + TryOffset<T, Ut1>,
    {
        let tt = time.try_to_scale(Tt, self).map_err(RotationError::offset)?;
        let ut1 = time
            .try_to_scale(Ut1, self)
            .map_err(RotationError::offset)?;
        let corr = self.corrections(time, sys).map_err(RotationError::eop)?;
        Ok(
            Rotation::new(sys.earth_rotation(tt, ut1, corr)).with_angular_velocity(DVec3::new(
                0.0,
                0.0,
                ROTATION_RATE_EARTH,
            )),
        )
    }
    /// Rotation from Pseudo-Earth Fixed to True of Date.
    fn pef_to_tod(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt> + TryOffset<T, Ut1>,
    {
        Ok(self.tod_to_pef(time, sys)?.transpose())
    }

    /// Rotation from Pseudo-Earth Fixed to ITRF (polar motion).
    fn pef_to_itrf(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        let tt = time.try_to_scale(Tt, self).map_err(RotationError::offset)?;
        let pole_coords = self.pole_coords(time).map_err(RotationError::eop)?;
        Ok(sys.polar_motion_matrix(tt, pole_coords).into())
    }

    /// Rotation from ITRF to Pseudo-Earth Fixed.
    fn itrf_to_pef(&self, time: Time<T>, sys: ReferenceSystem) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        Ok(self.pef_to_itrf(time, sys)?.transpose())
    }

    /// Rotation from True of Date to TEME.
    fn tod_to_teme(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb>,
    {
        let tdb = time
            .try_to_scale(Tdb, self)
            .map_err(RotationError::offset)?;

        let eoe = EquationOfTheEquinoxes::iau1994(tdb);

        // TOD to TEME is R_z(EoE)
        Ok(Rotation::new(eoe.0.rotation_z()))
    }

    /// Rotation from TEME to True of Date.
    fn teme_to_tod(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb>,
    {
        Ok(self.tod_to_teme(time)?.transpose())
    }

    /// Rotation from ICRF to CIRF (CIP + CIO).
    fn icrf_to_cirf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb>,
    {
        let tdb = time
            .try_to_scale(Tdb, self)
            .map_err(RotationError::offset)?;
        let mut xy = CipCoords::iau2006(tdb);
        let s = CioLocator::iau2006(tdb, xy);

        xy += self
            .corrections(time, ReferenceSystem::Iers2010)
            .unwrap_or_default();

        Ok(Rotation::new(xy.celestial_to_intermediate_matrix(s)))
    }
    /// Rotation from CIRF to ICRF.
    fn cirf_to_icrf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tdb>,
    {
        Ok(self.icrf_to_cirf(time)?.transpose())
    }

    /// Rotation from CIRF to TIRF (Earth rotation angle).
    fn cirf_to_tirf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Ut1>,
    {
        let time = time
            .try_to_scale(Ut1, self)
            .map_err(RotationError::offset)?;
        let era = EarthRotationAngle::iau2000(time);
        Ok(
            Rotation::new(era.0.rotation_z()).with_angular_velocity(DVec3::new(
                0.0,
                0.0,
                ROTATION_RATE_EARTH,
            )),
        )
    }
    /// Rotation from TIRF to CIRF.
    fn tirf_to_cirf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Ut1>,
    {
        Ok(self.cirf_to_tirf(time)?.transpose())
    }

    /// Rotation from TIRF to ITRF (polar motion).
    fn tirf_to_itrf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        let tt = time.try_to_scale(Tt, self).map_err(RotationError::offset)?;
        let pole_coords = self.pole_coords(time).map_err(RotationError::eop)?;
        Ok(ReferenceSystem::Iers2010
            .polar_motion_matrix(tt, pole_coords)
            .into())
    }

    /// Rotation from ITRF to TIRF.
    fn itrf_to_tirf(&self, time: Time<T>) -> Result<Rotation, RotationError>
    where
        T: TimeScale + Copy,
        Self: TryOffset<T, Tt>,
    {
        Ok(self.tirf_to_itrf(time)?.transpose())
    }
}

fn rotation_matrix_derivative(m: DMat3, v: DVec3) -> DMat3 {
    let sx = DVec3::new(0.0, v.z, v.y);
    let sy = DVec3::new(-v.z, 0.0, v.x);
    let sz = DVec3::new(v.y, -v.x, 0.0);
    let s = DMat3::from_cols(sx, sy, sz);
    -s * m
}

/// A rotation matrix with its time derivative, for transforming position and velocity vectors.
#[derive(Debug, Clone, Copy, PartialEq, ApproxEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Rotation {
    /// Rotation matrix.
    pub m: DMat3,
    /// Time derivative of the rotation matrix.
    pub dm: DMat3,
}

impl Rotation {
    /// Identity rotation (no rotation, zero derivative).
    pub const IDENTITY: Self = Self {
        m: DMat3::IDENTITY,
        dm: DMat3::ZERO,
    };

    /// Creates a rotation from a matrix with zero time derivative.
    pub fn new(m: DMat3) -> Self {
        Self { m, dm: DMat3::ZERO }
    }

    /// Sets the time derivative of the rotation matrix.
    pub fn with_derivative(mut self, dm: DMat3) -> Self {
        self.dm = dm;
        self
    }

    /// Sets the time derivative from an angular velocity vector.
    pub fn with_angular_velocity(mut self, v: DVec3) -> Self {
        self.dm = rotation_matrix_derivative(self.m, v);
        self
    }

    /// Returns the position rotation matrix.
    pub fn position_matrix(&self) -> DMat3 {
        self.m
    }

    /// Returns the velocity rotation matrix (time derivative).
    pub fn velocity_matrix(&self) -> DMat3 {
        self.dm
    }

    /// Composes this rotation with another (self then other).
    pub fn compose(self, other: Self) -> Self {
        Self {
            m: other.m * self.m,
            dm: other.dm * self.m + other.m * self.dm,
        }
    }

    /// Returns the transpose (inverse) rotation.
    pub fn transpose(&self) -> Self {
        let m = self.m.transpose();
        let dm = self.dm.transpose();
        Self { m, dm }
    }

    /// Rotates a position vector.
    pub fn rotate_position(&self, pos: DVec3) -> DVec3 {
        self.m * pos
    }

    /// Rotates a velocity vector (accounting for the rotation rate).
    pub fn rotate_velocity(&self, pos: DVec3, vel: DVec3) -> DVec3 {
        self.dm * pos + self.m * vel
    }

    /// Rotates a full state (position and velocity).
    pub fn rotate_state(&self, pos: DVec3, vel: DVec3) -> (DVec3, DVec3) {
        (self.rotate_position(pos), self.rotate_velocity(pos, vel))
    }
}

impl Default for Rotation {
    fn default() -> Self {
        Self {
            m: DMat3::IDENTITY,
            dm: DMat3::ZERO,
        }
    }
}

impl Mul<DVec3> for Rotation {
    type Output = DVec3;

    fn mul(self, rhs: DVec3) -> Self::Output {
        self.m * rhs
    }
}

impl Mul<Cartesian> for Rotation {
    type Output = Cartesian;

    fn mul(self, rhs: Cartesian) -> Self::Output {
        let pos = self.m * rhs.position();
        let vel = self.dm * rhs.position() + self.m * rhs.velocity();
        Cartesian::from_vecs(pos, vel)
    }
}

impl From<DMat3> for Rotation {
    fn from(matrix: DMat3) -> Self {
        Rotation::new(matrix)
    }
}

#[cfg(test)]
mod tests {
    use std::convert::Infallible;

    use lox_test_utils::assert_approx_eq;
    use lox_time::{deltas::TimeDelta, offsets::OffsetProvider};
    use lox_units::AngleUnits;

    use crate::iers::Iau2000Model;

    use super::*;

    #[derive(Debug)]
    struct TestRotationProvider;

    impl OffsetProvider for TestRotationProvider {
        type Error = Infallible;

        fn tai_to_ut1(&self, _delta: TimeDelta) -> Result<TimeDelta, Self::Error> {
            Ok(TimeDelta::from_seconds_f64(-33.072073684954375))
        }

        fn ut1_to_tai(&self, _delta: TimeDelta) -> Result<TimeDelta, Self::Error> {
            unreachable!()
        }
    }

    impl<T> RotationProvider<T> for TestRotationProvider
    where
        T: TimeScale,
    {
        type EopError = Infallible;

        fn corrections(
            &self,
            _time: Time<T>,
            sys: ReferenceSystem,
        ) -> Result<Corrections, Infallible> {
            match sys {
                ReferenceSystem::Iers1996 => {
                    Ok(Corrections(-55.0655e-3.arcsec(), -6.3580e-3.arcsec()))
                }
                ReferenceSystem::Iers2003(_) => {
                    Ok(Corrections(0.1725e-3.arcsec(), -0.2650e-3.arcsec()))
                }
                ReferenceSystem::Iers2010 => {
                    Ok(Corrections(0.1750e-3.arcsec(), -0.2259e-3.arcsec()))
                }
            }
        }

        fn pole_coords(&self, _time: Time<T>) -> Result<PoleCoords, Infallible> {
            Ok(PoleCoords {
                xp: 0.0349282.arcsec(),
                yp: 0.4833163.arcsec(),
            })
        }
    }

    #[test]
    fn test_celestial_to_terrestrial_iers1996() {
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);
        let sys = ReferenceSystem::Iers1996;

        let npb_exp = DMat3::from_cols_array(&[
            0.999998403176203,
            -0.001639032970562,
            -0.000712190961847,
            0.001639000942243,
            0.999998655799521,
            -0.000045552846624,
            0.000712264667137,
            0.000044385492226,
            0.999999745354454,
        ])
        .transpose();
        let c2t_exp = DMat3::from_cols_array(&[
            0.973104317592265,
            0.230363826166883,
            -0.000703332813776,
            -0.230363798723533,
            0.973104570754697,
            0.000120888299841,
            0.000712264667137,
            0.000044385492226,
            0.999999745354454,
        ])
        .transpose();
        let c2t_pm_exp = DMat3::from_cols_array(&[
            0.973104317712772,
            0.230363826174782,
            -0.000703163477127,
            -0.230363800391868,
            0.973104570648022,
            0.000118545116892,
            0.000711560100206,
            0.000046626645796,
            0.999999745754058,
        ])
        .transpose();

        let npb_act = TestRotationProvider
            .j2000_to_mod(tt.with_scale(Tdb), sys)
            .unwrap()
            .compose(
                TestRotationProvider
                    .mod_to_tod(tt.with_scale(Tdb), sys)
                    .unwrap(),
            );
        assert_approx_eq!(npb_act.m, npb_exp, atol <= 1e-12);

        let c2t_act = npb_act.compose(TestRotationProvider.tod_to_pef(tt, sys).unwrap());
        assert_approx_eq!(c2t_act.m, c2t_exp, atol <= 1e-12);

        let c2t_pm_act = c2t_act.compose(TestRotationProvider.pef_to_itrf(tt, sys).unwrap());
        assert_approx_eq!(c2t_pm_act.m, c2t_pm_exp, atol <= 1e-12);
    }

    #[test]
    fn test_celestial_to_terrestrial_iers2003() {
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);
        let sys = ReferenceSystem::Iers2003(Iau2000Model::A);

        let npb_exp = DMat3::from_cols_array(&[
            0.999998402755640,
            -0.001639289519579,
            -0.000712191013215,
            0.001639257491365,
            0.999998655379006,
            -0.000045552787478,
            0.000712264729795,
            0.000044385250265,
            0.999999745354420,
        ])
        .transpose();
        let c2t_exp = DMat3::from_cols_array(&[
            0.973104317573209,
            0.230363826247361,
            -0.000703332818999,
            -0.230363798803834,
            0.973104570735656,
            0.000120888549787,
            0.000712264729795,
            0.000044385250265,
            0.999999745354420,
        ])
        .transpose();
        let c2t_pm_exp = DMat3::from_cols_array(&[
            0.973104317697618,
            0.230363826238780,
            -0.000703163482352,
            -0.230363800455689,
            0.973104570632883,
            0.000118545366826,
            0.000711560162864,
            0.000046626403835,
            0.999999745754024,
        ])
        .transpose();

        let npb_act = TestRotationProvider
            .icrf_to_mod(tt, sys)
            .unwrap()
            .compose(TestRotationProvider.mod_to_tod(tt, sys).unwrap());
        assert_approx_eq!(npb_act.m, npb_exp, atol <= 1e-12);

        let c2t_act = npb_act.compose(TestRotationProvider.tod_to_pef(tt, sys).unwrap());
        assert_approx_eq!(c2t_act.m, c2t_exp, atol <= 1e-12);

        let c2t_pm_act = c2t_act.compose(TestRotationProvider.pef_to_itrf(tt, sys).unwrap());
        assert_approx_eq!(c2t_pm_act.m, c2t_pm_exp, atol <= 1e-12);
    }

    #[test]
    fn test_celestial_to_terrestrial_iau2006() {
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);

        let npb_exp = DMat3::from_cols_array(&[
            0.999999746339445,
            -0.000000005138822,
            -0.000712264730072,
            -0.000000026475227,
            0.999999999014975,
            -0.000044385242827,
            0.000712264729599,
            0.000044385250426,
            0.999999745354420,
        ])
        .transpose();
        let c2t_exp = DMat3::from_cols_array(&[
            0.973104317573127,
            0.230363826247709,
            -0.000703332818845,
            -0.230363798804182,
            0.973104570735574,
            0.000120888549586,
            0.000712264729599,
            0.000044385250426,
            0.999999745354420,
        ])
        .transpose();
        let c2t_pm_exp = DMat3::from_cols_array(&[
            0.973104317697535,
            0.230363826239128,
            -0.000703163482198,
            -0.230363800456037,
            0.973104570632801,
            0.000118545366625,
            0.000711560162668,
            0.000046626403995,
            0.999999745754024,
        ])
        .transpose();

        let npb_act = TestRotationProvider.icrf_to_cirf(tt).unwrap();
        assert_approx_eq!(npb_act.m, npb_exp, atol <= 1e-11);

        let c2t_act = npb_act.compose(TestRotationProvider.cirf_to_tirf(tt).unwrap());
        assert_approx_eq!(c2t_act.m, c2t_exp, atol <= 1e-11);

        let c2t_pm_act = c2t_act.compose(TestRotationProvider.tirf_to_itrf(tt).unwrap());
        assert_approx_eq!(c2t_pm_act.m, c2t_pm_exp, atol <= 1e-11);
    }

    #[test]
    fn test_tod_to_teme() {
        // Use the same time as the EoE test to verify against known reference value
        // EoE at this time = 5.357_758_254_609_257e-5 radians (from test_equation_of_the_equinoxes_iau1994)
        let tdb = Time::from_two_part_julian_date(Tdb, 2400000.5, 41234.0);
        let eoe: f64 = 5.357_758_254_609_257e-5; // radians

        let rotation = TestRotationProvider.tod_to_teme(tdb).unwrap();

        // TOD to TEME is R_z(EoE), so the rotation matrix should be:
        // [cos(EoE)  -sin(EoE)  0]
        // [sin(EoE)   cos(EoE)  0]
        // [0          0         1]
        let (sin_eoe, cos_eoe) = eoe.sin_cos();
        let expected =
            DMat3::from_cols_array(&[cos_eoe, sin_eoe, 0.0, -sin_eoe, cos_eoe, 0.0, 0.0, 0.0, 1.0])
                .transpose();

        assert_approx_eq!(rotation.m, expected, atol <= 1e-15);

        // Verify round-trip
        let roundtrip = rotation.compose(TestRotationProvider.teme_to_tod(tdb).unwrap());
        assert_approx_eq!(roundtrip.m, DMat3::IDENTITY, atol <= 1e-15);
    }

    #[test]
    fn test_teme_icrf_roundtrip() {
        // Test the full TEME <-> ICRF transformation chain
        // Path: ICRF -> MOD -> TOD -> TEME and back
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);
        let sys = ReferenceSystem::Iers1996;

        // Build the full ICRF to TEME transformation
        let icrf_to_mod = TestRotationProvider.icrf_to_mod(tt, sys).unwrap();
        let mod_to_tod = TestRotationProvider.mod_to_tod(tt, sys).unwrap();
        let tod_to_teme = TestRotationProvider.tod_to_teme(tt).unwrap();

        let icrf_to_teme = icrf_to_mod.compose(mod_to_tod).compose(tod_to_teme);

        // Build the reverse transformation
        let teme_to_tod = TestRotationProvider.teme_to_tod(tt).unwrap();
        let tod_to_mod = TestRotationProvider.tod_to_mod(tt, sys).unwrap();
        let mod_to_icrf = TestRotationProvider.mod_to_icrf(tt, sys).unwrap();

        let teme_to_icrf = teme_to_tod.compose(tod_to_mod).compose(mod_to_icrf);

        // Round-trip should give identity
        let roundtrip = icrf_to_teme.compose(teme_to_icrf);
        assert_approx_eq!(roundtrip.m, DMat3::IDENTITY, atol <= 1e-14);
    }

    #[test]
    fn test_icrf_j2000_roundtrip() {
        let fwd =
            <TestRotationProvider as RotationProvider<Tt>>::icrf_to_j2000(&TestRotationProvider);
        let rev =
            <TestRotationProvider as RotationProvider<Tt>>::j2000_to_icrf(&TestRotationProvider);
        let roundtrip = fwd.compose(rev);
        assert_approx_eq!(roundtrip.m, DMat3::IDENTITY, atol <= 1e-15);
    }

    #[test]
    fn test_j2000_mod_equivalence_iers1996() {
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);
        let sys = ReferenceSystem::Iers1996;

        // ICRF -> J2000 -> MOD should equal ICRF -> MOD
        let via_j2000 =
            <TestRotationProvider as RotationProvider<Tt>>::icrf_to_j2000(&TestRotationProvider)
                .compose(TestRotationProvider.j2000_to_mod(tt, sys).unwrap());
        let direct = TestRotationProvider.icrf_to_mod(tt, sys).unwrap();

        assert_approx_eq!(via_j2000.m, direct.m, atol <= 1e-14);
    }

    #[test]
    fn test_j2000_mod_equivalence_iers2003() {
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);
        let sys = ReferenceSystem::Iers2003(Iau2000Model::A);

        let via_j2000 =
            <TestRotationProvider as RotationProvider<Tt>>::icrf_to_j2000(&TestRotationProvider)
                .compose(TestRotationProvider.j2000_to_mod(tt, sys).unwrap());
        let direct = TestRotationProvider.icrf_to_mod(tt, sys).unwrap();

        assert_approx_eq!(via_j2000.m, direct.m, atol <= 1e-14);
    }

    #[test]
    fn test_j2000_mod_equivalence_iers2010() {
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);
        let sys = ReferenceSystem::Iers2010;

        let via_j2000 =
            <TestRotationProvider as RotationProvider<Tt>>::icrf_to_j2000(&TestRotationProvider)
                .compose(TestRotationProvider.j2000_to_mod(tt, sys).unwrap());
        let direct = TestRotationProvider.icrf_to_mod(tt, sys).unwrap();

        assert_approx_eq!(via_j2000.m, direct.m, atol <= 1e-14);
    }

    #[test]
    fn test_j2000_full_chain_equivalence() {
        // ICRF -> J2000 -> MOD -> TOD -> PEF -> ITRF == ICRF -> MOD -> TOD -> PEF -> ITRF
        let tt = Time::from_two_part_julian_date(Tt, 2454195.5, 0.500754444444444);
        let sys = ReferenceSystem::Iers2003(Iau2000Model::A);

        let via_j2000 =
            <TestRotationProvider as RotationProvider<Tt>>::icrf_to_j2000(&TestRotationProvider)
                .compose(TestRotationProvider.j2000_to_mod(tt, sys).unwrap())
                .compose(TestRotationProvider.mod_to_tod(tt, sys).unwrap())
                .compose(TestRotationProvider.tod_to_pef(tt, sys).unwrap())
                .compose(TestRotationProvider.pef_to_itrf(tt, sys).unwrap());

        let direct = TestRotationProvider
            .icrf_to_mod(tt, sys)
            .unwrap()
            .compose(TestRotationProvider.mod_to_tod(tt, sys).unwrap())
            .compose(TestRotationProvider.tod_to_pef(tt, sys).unwrap())
            .compose(TestRotationProvider.pef_to_itrf(tt, sys).unwrap());

        assert_approx_eq!(via_j2000.m, direct.m, atol <= 1e-14);
    }
}