open-standards/map/map-rs/crates/map-orbital/src/geodetic.rs

//! ECEF to geodetic (latitude, longitude, altitude) conversions.
//!
//! Uses Bowring's iterative method for ECEF to geodetic conversion,
//! which converges in 2-3 iterations for typical accuracy requirements.

use serde::{Deserialize, Serialize};

use crate::constants::*;
use crate::ecef::EcefPosition;

/// Geodetic position (latitude, longitude, altitude above WGS-84 ellipsoid).
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
pub struct GeodeticPosition {
    /// Geodetic latitude (degrees, -90 to +90).
    pub lat_deg: f64,
    /// Geodetic longitude (degrees, -180 to +180).
    pub lon_deg: f64,
    /// Altitude above the WGS-84 ellipsoid (km).
    pub alt_km: f64,
}

/// Convert ECEF position to geodetic coordinates using Bowring's iterative method.
///
/// The algorithm:
/// 1. Initial estimate: beta = atan2(z, (1-f)*p) where p = sqrt(x^2 + y^2)
/// 2. Iterate:
///    - lat = atan2(z + e'^2 * b * sin^3(beta), p - e^2 * a * cos^3(beta))
///    - beta = atan2((1-f)*sin(lat), cos(lat))
/// 3. lon = atan2(y, x)
/// 4. N = a / sqrt(1 - e^2*sin^2(lat))
/// 5. alt = p/cos(lat) - N
///
/// Convergence is typically achieved in 2-3 iterations.
pub fn ecef_to_geodetic(ecef: &EcefPosition) -> GeodeticPosition {
    let a = EARTH_RADIUS_WGS84; // semi-major axis
    let b = EARTH_POLAR_RADIUS; // semi-minor axis
    let e2 = EARTH_ECCENTRICITY_SQ; // first eccentricity squared
    let ep2 = EARTH_ECCENTRICITY_SQ2; // second eccentricity squared
    let f = FLATTENING;

    let x = ecef.x;
    let y = ecef.y;
    let z = ecef.z;

    let p = (x * x + y * y).sqrt();

    // Handle pole cases
    if p < 1e-10 {
        let lat_deg = if z >= 0.0 { 90.0 } else { -90.0 };
        let lon_deg = 0.0;
        let alt_km = z.abs() - b;
        return GeodeticPosition {
            lat_deg,
            lon_deg,
            alt_km,
        };
    }

    // Longitude is straightforward
    let lon = y.atan2(x);

    // Bowring's iterative method for latitude
    let mut beta = (z / ((1.0 - f) * p)).atan();

    for _ in 0..5 {
        let sin_beta = beta.sin();
        let cos_beta = beta.cos();

        let lat = (z + ep2 * b * sin_beta * sin_beta * sin_beta)
            .atan2(p - e2 * a * cos_beta * cos_beta * cos_beta);

        let new_beta = ((1.0 - f) * lat.sin()).atan2(lat.cos());

        if (new_beta - beta).abs() < 1e-12 {
            beta = new_beta;
            break;
        }
        beta = new_beta;
    }

    // Final latitude computation
    let sin_beta = beta.sin();
    let cos_beta = beta.cos();
    let lat = (z + ep2 * b * sin_beta * sin_beta * sin_beta)
        .atan2(p - e2 * a * cos_beta * cos_beta * cos_beta);

    // Altitude
    let sin_lat = lat.sin();
    let cos_lat = lat.cos();
    let n = a / (1.0 - e2 * sin_lat * sin_lat).sqrt();

    let alt = if cos_lat.abs() > 1e-10 {
        p / cos_lat - n
    } else {
        z.abs() / sin_lat.abs() - n * (1.0 - e2)
    };

    GeodeticPosition {
        lat_deg: lat * RAD_TO_DEG,
        lon_deg: lon * RAD_TO_DEG,
        alt_km: alt,
    }
}

/// Convert geodetic coordinates to ECEF position.
pub fn geodetic_to_ecef(geo: &GeodeticPosition) -> EcefPosition {
    let a = EARTH_RADIUS_WGS84;
    let e2 = EARTH_ECCENTRICITY_SQ;

    let lat = geo.lat_deg * DEG_TO_RAD;
    let lon = geo.lon_deg * DEG_TO_RAD;
    let alt = geo.alt_km;

    let sin_lat = lat.sin();
    let cos_lat = lat.cos();
    let sin_lon = lon.sin();
    let cos_lon = lon.cos();

    // Radius of curvature in the prime vertical
    let n = a / (1.0 - e2 * sin_lat * sin_lat).sqrt();

    EcefPosition {
        x: (n + alt) * cos_lat * cos_lon,
        y: (n + alt) * cos_lat * sin_lon,
        z: (n * (1.0 - e2) + alt) * sin_lat,
    }
}

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

    #[test]
    fn test_roundtrip_equator() {
        let geo = GeodeticPosition {
            lat_deg: 0.0,
            lon_deg: 0.0,
            alt_km: 0.0,
        };
        let ecef = geodetic_to_ecef(&geo);
        let geo2 = ecef_to_geodetic(&ecef);

        assert!((geo.lat_deg - geo2.lat_deg).abs() < 1e-8, "lat: {} vs {}", geo.lat_deg, geo2.lat_deg);
        assert!((geo.lon_deg - geo2.lon_deg).abs() < 1e-8, "lon: {} vs {}", geo.lon_deg, geo2.lon_deg);
        assert!((geo.alt_km - geo2.alt_km).abs() < 1e-6, "alt: {} vs {}", geo.alt_km, geo2.alt_km);
    }

    #[test]
    fn test_roundtrip_arbitrary() {
        let geo = GeodeticPosition {
            lat_deg: 40.7128,
            lon_deg: -74.0060,
            alt_km: 0.3,
        };
        let ecef = geodetic_to_ecef(&geo);
        let geo2 = ecef_to_geodetic(&ecef);

        assert!((geo.lat_deg - geo2.lat_deg).abs() < 1e-6, "lat: {} vs {}", geo.lat_deg, geo2.lat_deg);
        assert!((geo.lon_deg - geo2.lon_deg).abs() < 1e-6, "lon: {} vs {}", geo.lon_deg, geo2.lon_deg);
        assert!((geo.alt_km - geo2.alt_km).abs() < 1e-4, "alt: {} vs {}", geo.alt_km, geo2.alt_km);
    }

    #[test]
    fn test_roundtrip_satellite_altitude() {
        let geo = GeodeticPosition {
            lat_deg: 51.6416,
            lon_deg: 120.0,
            alt_km: 408.0,
        };
        let ecef = geodetic_to_ecef(&geo);
        let geo2 = ecef_to_geodetic(&ecef);

        assert!((geo.lat_deg - geo2.lat_deg).abs() < 1e-6, "lat: {} vs {}", geo.lat_deg, geo2.lat_deg);
        assert!((geo.lon_deg - geo2.lon_deg).abs() < 1e-6, "lon: {} vs {}", geo.lon_deg, geo2.lon_deg);
        assert!((geo.alt_km - geo2.alt_km).abs() < 1e-4, "alt: {} vs {}", geo.alt_km, geo2.alt_km);
    }

    #[test]
    fn test_roundtrip_pole() {
        let geo = GeodeticPosition {
            lat_deg: 90.0,
            lon_deg: 0.0,
            alt_km: 0.0,
        };
        let ecef = geodetic_to_ecef(&geo);
        let geo2 = ecef_to_geodetic(&ecef);

        assert!((geo.lat_deg - geo2.lat_deg).abs() < 1e-6, "lat: {} vs {}", geo.lat_deg, geo2.lat_deg);
        assert!((geo.alt_km - geo2.alt_km).abs() < 1e-4, "alt: {} vs {}", geo.alt_km, geo2.alt_km);
    }

    #[test]
    fn test_roundtrip_negative_lat() {
        let geo = GeodeticPosition {
            lat_deg: -33.8688,
            lon_deg: 151.2093,
            alt_km: 0.05,
        };
        let ecef = geodetic_to_ecef(&geo);
        let geo2 = ecef_to_geodetic(&ecef);

        assert!((geo.lat_deg - geo2.lat_deg).abs() < 1e-6);
        assert!((geo.lon_deg - geo2.lon_deg).abs() < 1e-6);
        assert!((geo.alt_km - geo2.alt_km).abs() < 1e-4);
    }

    #[test]
    fn test_equator_x_axis() {
        // On the equator, prime meridian, at sea level
        let ecef = geodetic_to_ecef(&GeodeticPosition {
            lat_deg: 0.0,
            lon_deg: 0.0,
            alt_km: 0.0,
        });
        // Should be on the X axis at the equatorial radius
        assert!((ecef.x - EARTH_RADIUS_WGS84).abs() < 1e-6);
        assert!(ecef.y.abs() < 1e-6);
        assert!(ecef.z.abs() < 1e-6);
    }

    #[test]
    fn test_geo_orbit_altitude() {
        // GEO altitude is ~35786 km
        let geo = GeodeticPosition {
            lat_deg: 0.0,
            lon_deg: 0.0,
            alt_km: 35786.0,
        };
        let ecef = geodetic_to_ecef(&geo);
        let geo2 = ecef_to_geodetic(&ecef);
        assert!((geo.alt_km - geo2.alt_km).abs() < 0.01);
    }
}