//! Geodetic coordinate frame (latitude, longitude, altitude).
//!
//! Conversion from ECEF uses Ferrari's closed-form solution to
//! avoid iterative methods (and thus loops).

use crate::error::Error;
use crate::propagation::constants::EARTH_RADIUS_KM;

/// Witness type for the Geodetic frame.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Geodetic;

/// Latitude in radians, range [-pi/2, pi/2].
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
pub struct Latitude(f64);

impl Latitude {
    /// Create a latitude from radians.
    ///
    /// # Errors
    ///
    /// Returns an error if `rad` is outside [-pi/2, pi/2].
    pub fn new(rad: f64) -> Result<Self, Error> {
        let half_pi = core::f64::consts::FRAC_PI_2;
        if (-half_pi..=half_pi).contains(&rad) {
            Ok(Self(rad))
        } else {
            Err(Error::PropagationDiverged {
                context: "latitude out of range",
            })
        }
    }

    /// Create a latitude from radians without range checking.
    /// Used internally where the value is known to be valid.
    #[must_use]
    pub(crate) fn from_radians_unchecked(rad: f64) -> Self {
        Self(rad)
    }

    #[must_use]
    pub fn radians(self) -> f64 {
        self.0
    }

    #[must_use]
    pub fn degrees(self) -> f64 {
        self.0.to_degrees()
    }
}

/// Longitude in radians, range [-pi, pi].
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
pub struct Longitude(f64);

impl Longitude {
    /// Create a longitude from radians.
    ///
    /// # Errors
    ///
    /// Returns an error if `rad` is outside [-pi, pi].
    pub fn new(rad: f64) -> Result<Self, Error> {
        let pi = core::f64::consts::PI;
        if (-pi..=pi).contains(&rad) {
            Ok(Self(rad))
        } else {
            Err(Error::PropagationDiverged {
                context: "longitude out of range",
            })
        }
    }

    #[must_use]
    pub(crate) fn from_radians_unchecked(rad: f64) -> Self {
        Self(rad)
    }

    #[must_use]
    pub fn radians(self) -> f64 {
        self.0
    }

    #[must_use]
    pub fn degrees(self) -> f64 {
        self.0.to_degrees()
    }
}

/// Altitude above the WGS-84 ellipsoid in kilometers.
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
pub struct Altitude(f64);

impl Altitude {
    #[must_use]
    pub fn new(km: f64) -> Self {
        Self(km)
    }

    #[must_use]
    pub fn km(self) -> f64 {
        self.0
    }
}

/// A geodetic position: latitude, longitude, altitude.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct GeodeticPosition {
    latitude: Latitude,
    longitude: Longitude,
    altitude: Altitude,
}

impl GeodeticPosition {
    #[must_use]
    pub fn new(latitude: Latitude, longitude: Longitude, altitude: Altitude) -> Self {
        Self {
            latitude,
            longitude,
            altitude,
        }
    }

    #[must_use]
    pub fn latitude(&self) -> Latitude {
        self.latitude
    }

    #[must_use]
    pub fn longitude(&self) -> Longitude {
        self.longitude
    }

    #[must_use]
    pub fn altitude(&self) -> Altitude {
        self.altitude
    }
}

// ---------------------------------------------------------------------------
// ECEF -> Geodetic conversion (Ferrari's closed-form method)
// ---------------------------------------------------------------------------

/// WGS-84 flattening.
const FLATTENING: f64 = 1.0 / 298.257_223_563;

/// WGS-84 semi-major axis (km).  Use the SGP4 value for consistency.
const SEMI_MAJOR: f64 = EARTH_RADIUS_KM;

/// WGS-84 first eccentricity squared.
pub(crate) const E_SQ: f64 = FLATTENING * (2.0 - FLATTENING);

/// Convert ECEF `(x, y, z)` in km to geodetic `(lat, lon, alt)`.
///
/// Uses Ferrari's exact closed-form solution (no iteration, no loops).
#[must_use]
#[allow(clippy::many_single_char_names)]
pub fn ecef_to_geodetic(x: f64, y: f64, z: f64) -> GeodeticPosition {
    let a = SEMI_MAJOR;
    let e2 = E_SQ;
    let b = a * f64::sqrt(1.0 - e2);
    let ep2 = (a * a - b * b) / (b * b); // second eccentricity squared

    let p = f64::sqrt(x * x + y * y);
    let theta = f64::atan2(z * a, p * b);

    let sin_theta = f64::sin(theta);
    let cos_theta = f64::cos(theta);

    let lat = f64::atan2(
        z + ep2 * b * sin_theta * sin_theta * sin_theta,
        p - e2 * a * cos_theta * cos_theta * cos_theta,
    );

    let lon = f64::atan2(y, x);

    let sin_lat = f64::sin(lat);
    let n = a / f64::sqrt(1.0 - e2 * sin_lat * sin_lat);
    let alt = p / f64::cos(lat) - n;

    GeodeticPosition {
        latitude: Latitude::from_radians_unchecked(lat),
        longitude: Longitude::from_radians_unchecked(lon),
        altitude: Altitude::new(alt),
    }
}

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

    #[test]
    fn north_pole_geodetic() {
        // Point on the Z axis at the north pole
        let geo = ecef_to_geodetic(0.0, 0.0, SEMI_MAJOR * (1.0 - FLATTENING));
        assert!(
            (geo.latitude().degrees() - 90.0).abs() < 0.1,
            "expected ~90 deg, got {}",
            geo.latitude().degrees()
        );
    }

    #[test]
    fn equator_prime_meridian() {
        // Point on the equator at the prime meridian
        let geo = ecef_to_geodetic(SEMI_MAJOR, 0.0, 0.0);
        assert!(
            geo.latitude().degrees().abs() < 0.1,
            "expected ~0 lat, got {}",
            geo.latitude().degrees()
        );
        assert!(
            geo.longitude().degrees().abs() < 0.1,
            "expected ~0 lon, got {}",
            geo.longitude().degrees()
        );
        assert!(
            geo.altitude().km().abs() < 1.0,
            "expected ~0 alt, got {}",
            geo.altitude().km()
        );
    }

    #[test]
    fn iss_altitude_orbit_range() {
        // An ISS-like point ~6786 km from center at 51.6 deg latitude
        let lat_rad = 51.6_f64.to_radians();
        let r = 6786.0;
        let x = r * f64::cos(lat_rad);
        let z = r * f64::sin(lat_rad);
        let geo = ecef_to_geodetic(x, 0.0, z);
        // Altitude should be roughly 400-420 km
        assert!(
            geo.altitude().km() > 350.0 && geo.altitude().km() < 500.0,
            "ISS-like alt {} km out of range",
            geo.altitude().km()
        );
    }
}
