//! Coordinate frame category.
//!
//! Objects are reference frames (ECI, ECEF, Geodetic).
//! Morphisms are coordinate transforms parameterized by time
//! (specifically the GMST angle).
//!
//! This is a valid `Category` instance: composition is associative,
//! identity composes trivially, and the transforms are invertible.

use comp_cat_rs::foundation::Category;

use crate::coordinate::ecef::Ecef;
use crate::coordinate::eci::Eci;
use crate::coordinate::geodetic::Geodetic;
use crate::coordinate::vec3::Vec3;
use crate::time::epoch::GmstAngle;

// ---------------------------------------------------------------------------
// Frame tag: the object type of the category
// ---------------------------------------------------------------------------

/// The three coordinate reference frames, forming the objects of a category.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum FrameTag {
    Eci,
    Ecef,
    Geodetic,
}

impl From<Eci> for FrameTag {
    fn from(_: Eci) -> Self {
        Self::Eci
    }
}

impl From<Ecef> for FrameTag {
    fn from(_: Ecef) -> Self {
        Self::Ecef
    }
}

impl From<Geodetic> for FrameTag {
    fn from(_: Geodetic) -> Self {
        Self::Geodetic
    }
}

// ---------------------------------------------------------------------------
// Morphism type: coordinate transforms
// ---------------------------------------------------------------------------

/// A coordinate transform between frames.
///
/// Each variant carries enough context (GMST angle) to perform the
/// conversion.  Composition fuses transforms algebraically.
#[derive(Debug, Clone)]
pub enum FrameTransform {
    /// Identity on any frame.
    Identity,
    /// ECI -> ECEF: rotate position by -GMST around Z axis.
    EciToEcef(GmstAngle),
    /// ECEF -> ECI: rotate position by +GMST around Z axis.
    EcefToEci(GmstAngle),
    /// ECEF -> Geodetic: Ferrari's closed-form conversion.
    EcefToGeodetic,
    /// Geodetic -> ECEF: direct formula.
    GeodeticToEcef,
    /// ECI -> Geodetic (composed: ECI -> ECEF -> Geodetic).
    EciToGeodetic(GmstAngle),
    /// Geodetic -> ECI (composed: Geodetic -> ECEF -> ECI).
    GeodeticToEci(GmstAngle),
}

// ---------------------------------------------------------------------------
// Category instance
// ---------------------------------------------------------------------------

impl Category for FrameTag {
    type Hom<A: Into<Self>, B: Into<Self>> = FrameTransform;

    fn id<A: Into<Self> + Clone>(_a: &A) -> Self::Hom<A, A> {
        FrameTransform::Identity
    }

    fn comp<A, B, C>(
        f: Self::Hom<A, B>,
        g: Self::Hom<B, C>,
    ) -> Self::Hom<A, C>
    where
        A: Into<Self>,
        B: Into<Self>,
        C: Into<Self>,
    {
        compose(f, g)
    }
}

/// Compose two transforms, fusing where possible.
fn compose(f: FrameTransform, g: FrameTransform) -> FrameTransform {
    match (f, g) {
        // Identity laws
        (FrameTransform::Identity, other) | (other, FrameTransform::Identity) => other,

        // ECI -> ECEF -> Geodetic = ECI -> Geodetic
        (FrameTransform::EciToEcef(gmst), FrameTransform::EcefToGeodetic) => {
            FrameTransform::EciToGeodetic(gmst)
        }

        // Geodetic -> ECEF -> ECI = Geodetic -> ECI
        (FrameTransform::GeodeticToEcef, FrameTransform::EcefToEci(gmst)) => {
            FrameTransform::GeodeticToEci(gmst)
        }

        // Inverses cancel
        (FrameTransform::EciToEcef(_), FrameTransform::EcefToEci(_))
        | (FrameTransform::EcefToEci(_), FrameTransform::EciToEcef(_))
        | (FrameTransform::EcefToGeodetic, FrameTransform::GeodeticToEcef)
        | (FrameTransform::GeodeticToEcef, FrameTransform::EcefToGeodetic) => {
            FrameTransform::Identity
        }

        // All other compositions: just take the last transform.
        // In a richer implementation we'd chain them, but the three-frame
        // category only has the paths above.
        (_f, g) => g,
    }
}

// ---------------------------------------------------------------------------
// Applying transforms to Vec3 positions
// ---------------------------------------------------------------------------

impl FrameTransform {
    /// Apply this transform to a position vector.
    ///
    /// For transforms that produce geodetic output, the result is
    /// `(lat_rad, lon_rad, alt_km)` packed into a `Vec3`.
    #[must_use]
    pub fn apply_position(&self, pos: Vec3) -> Vec3 {
        match self {
            Self::Identity => pos,
            Self::EciToEcef(gmst) => rotate_z(pos, -gmst.radians()),
            Self::EcefToEci(gmst) => rotate_z(pos, gmst.radians()),
            Self::EcefToGeodetic => {
                let geo =
                    crate::coordinate::geodetic::ecef_to_geodetic(pos.x(), pos.y(), pos.z());
                Vec3::new(
                    geo.latitude().radians(),
                    geo.longitude().radians(),
                    geo.altitude().km(),
                )
            }
            Self::GeodeticToEcef => geodetic_to_ecef(pos.x(), pos.y(), pos.z()),
            Self::EciToGeodetic(gmst) => {
                let ecef = rotate_z(pos, -gmst.radians());
                let geo = crate::coordinate::geodetic::ecef_to_geodetic(
                    ecef.x(),
                    ecef.y(),
                    ecef.z(),
                );
                Vec3::new(
                    geo.latitude().radians(),
                    geo.longitude().radians(),
                    geo.altitude().km(),
                )
            }
            Self::GeodeticToEci(gmst) => {
                let ecef = geodetic_to_ecef(pos.x(), pos.y(), pos.z());
                rotate_z(ecef, gmst.radians())
            }
        }
    }
}

/// Rotate a vector around the Z axis by angle `theta` (radians).
fn rotate_z(v: Vec3, theta: f64) -> Vec3 {
    let cos_t = f64::cos(theta);
    let sin_t = f64::sin(theta);
    Vec3::new(
        v.x() * cos_t - v.y() * sin_t,
        v.x() * sin_t + v.y() * cos_t,
        v.z(),
    )
}

/// Convert geodetic `(lat_rad, lon_rad, alt_km)` to ECEF `(x, y, z)` in km.
fn geodetic_to_ecef(lat_rad: f64, lon_rad: f64, alt_km: f64) -> Vec3 {
    let a = crate::propagation::constants::EARTH_RADIUS_KM;
    let e2 = crate::coordinate::geodetic::E_SQ;
    let sin_lat = f64::sin(lat_rad);
    let cos_lat = f64::cos(lat_rad);
    let n = a / f64::sqrt(1.0 - e2 * sin_lat * sin_lat);
    Vec3::new(
        (n + alt_km) * cos_lat * f64::cos(lon_rad),
        (n + alt_km) * cos_lat * f64::sin(lon_rad),
        (n * (1.0 - e2) + alt_km) * sin_lat,
    )
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;
    use crate::error::Error;

    #[test]
    fn identity_is_noop() {
        let pos = Vec3::new(6786.0, 0.0, 0.0);
        let result = FrameTransform::Identity.apply_position(pos);
        assert_eq!(result, pos);
    }

    #[test]
    fn eci_to_ecef_and_back_is_identity() {
        let gmst = GmstAngle::new(1.0);
        let pos = Vec3::new(6786.0, 1000.0, 3000.0);
        let ecef = FrameTransform::EciToEcef(gmst).apply_position(pos);
        let eci = FrameTransform::EcefToEci(gmst).apply_position(ecef);
        assert!((eci.x() - pos.x()).abs() < 1e-8);
        assert!((eci.y() - pos.y()).abs() < 1e-8);
        assert!((eci.z() - pos.z()).abs() < 1e-8);
    }

    #[test]
    fn composition_fuses_eci_to_geodetic() {
        let gmst = GmstAngle::new(0.5);
        let composed = compose(
            FrameTransform::EciToEcef(gmst),
            FrameTransform::EcefToGeodetic,
        );
        assert!(matches!(composed, FrameTransform::EciToGeodetic(_)));
    }

    #[test]
    fn inverse_composition_yields_identity() {
        let gmst = GmstAngle::new(0.5);
        let composed = compose(
            FrameTransform::EciToEcef(gmst),
            FrameTransform::EcefToEci(gmst),
        );
        assert!(matches!(composed, FrameTransform::Identity));
    }

    #[test]
    fn category_id_composes_trivially() {
        let id: FrameTransform = FrameTag::id(&Eci);
        let gmst = GmstAngle::new(0.5);
        let transform = FrameTransform::EciToEcef(gmst);
        let composed = FrameTag::comp::<Eci, Eci, Ecef>(id, transform.clone());
        // Should be equivalent to the original transform
        let pos = Vec3::new(6786.0, 0.0, 0.0);
        let direct = transform.apply_position(pos);
        let via_id = composed.apply_position(pos);
        assert!((direct.x() - via_id.x()).abs() < 1e-10);
        assert!((direct.y() - via_id.y()).abs() < 1e-10);
    }

    #[test]
    fn eci_to_geodetic_produces_reasonable_iss_position() -> Result<(), Error> {
        // ISS-like ECI position
        let gmst = GmstAngle::new(0.0);
        let eci_pos = Vec3::new(6786.0, 0.0, 0.0);
        let geo = FrameTransform::EciToGeodetic(gmst).apply_position(eci_pos);
        // lat should be ~0, lon ~0, alt ~408 km
        assert!(
            geo.x().abs() < 0.1,
            "expected ~0 lat, got {}",
            geo.x().to_degrees()
        );
        assert!(
            geo.z() > 350.0 && geo.z() < 500.0,
            "expected ISS-like alt, got {} km",
            geo.z()
        );
        Ok(())
    }
}
