use crate::constants::{FLAT, RE_EQ_M};
use std::f64::consts::PI;

/// Convert geodetic latitude to geocentric latitude. [^1][^2]
///
/// Inputs:
/// - `geod`: geodetic latitude in radians; should be within ±π/2.
/// - `h`: ellipsoidal altitude in meters
///
/// Outputs:
/// - geocentric latitude in radians
///
/// # Example
/// ```
/// use navitools::geod2geoc;
/// let test_lat = 45.0_f64.to_radians();
/// let geoc = geod2geoc(test_lat, 0.0);
/// println!("{}", geoc.to_degrees());
/// ```
///
/// [^1]: MATLAB (2026),
/// <https://www.mathworks.com/help/aeroblks/geodetictogeocentriclatitude.html>
/// [^2]: MATLAB (2026),
/// <https://www.mathworks.com/help/aerotbx/ug/geod2geoc.html>
///
pub fn geod2geoc(geod: f64, h: f64) -> f64 {
    if !(-PI / 2.0..=PI / 2.0).contains(&geod) {
        panic!("The input geodetic latitude should be within ±π/2.");
    }

    let f = FLAT;
    let a = RE_EQ_M;
    let mu = geod;

    let e2 = f * (2.0 - f);
    let N = a / (1.0 - e2 * (mu.sin()).powi(2)).sqrt();
    let rho = (N + h) * mu.cos();
    let z = (N * (1.0 - e2) + h) * mu.sin();
    (z / rho).atan()
}

/// Convert geocentric latitude to geodetic latitude. [^1][^2]
///
/// Inputs:
/// - `geoc`: geocentric latitude in radians; should be within ±π/2.
/// - `r`: radius from center of the Earth to the object's CoG, in meters
///
/// Outputs:
/// - geodetic latitude in radians
///
/// # Example
/// ```
/// use navitools::{geoc2geod, constants::RE_EQ_M};
/// let test_lat = 45.0_f64.to_radians();
/// let geod = geoc2geod(test_lat, RE_EQ_M);
/// println!("{}", geod.to_degrees());
/// ```
///
/// [^1]: MATLAB (2026),
/// <https://www.mathworks.com/help/aeroblks/geocentrictogeodeticlatitude.html>
/// [^2]: MATLAB (2026),
/// <https://www.mathworks.com/help/aerotbx/ug/geoc2geod.html>
///
pub fn geoc2geod(geoc: f64, r: f64) -> f64 {
    if !(-PI / 2.0..=PI / 2.0).contains(&geoc) {
        panic!("The input geocentric latitude should be within ±π/2.");
    }

    let f = FLAT;
    let a = RE_EQ_M;
    let lambda = geoc;

    let rho = r * lambda.cos();
    let z = r * lambda.sin();
    let b = a * (1.0 - f);
    let e2 = f * (2.0 - f);
    let ep2 = e2 / (1.0 - e2);

    // Uses the fixed-point iteration of Bowring's formula to calculate μ.
    // This formula typically converges in three iterations.
    let mut mu = 45.0_f64.to_radians();
    let mut beta;
    for _ in 0..3 {
        beta = ((1.0 - f) * mu.sin() / mu.cos()).atan();
        mu = ((z + b * ep2 * beta.sin().powi(3)) / (rho - a * e2 * beta.cos().powi(3))).atan();
    }

    mu
}

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

    #[test]
    fn test_geod2geoc() {
        let tol = 1e-4;

        let test_lat = 45.0_f64.to_radians();
        let ans = 44.8076;
        let geoc = geod2geoc(test_lat, 0.0);
        assert!((ans - geoc.to_degrees()).abs() < tol);

        // special case: 0.0
        let test_lat = 0.0_f64.to_radians();
        let ans = 0.0;
        let geoc = geod2geoc(test_lat, 1000.0);
        assert!((ans - geoc.to_degrees()).abs() < 1e-15);

        // special case: 90.0
        let test_lat = 90.0_f64.to_radians();
        let ans = 90.0;
        let geoc = geod2geoc(test_lat, 2000.0);
        assert!((ans - geoc.to_degrees()).abs() < 1e-15);
    }

    #[test]
    #[should_panic]
    fn test_geod2geoc_panic() {
        let _ = geod2geoc(90.0001_f64.to_radians(), 0.0);
    }

    #[test]
    fn test_geoc2geod() {
        let tol = 1e-4;
        let test_h = 6379136.0;

        let test_lat = 45.0_f64.to_radians();
        let ans = 45.1921;
        let geod = geoc2geod(test_lat, test_h);
        assert!((ans - geod.to_degrees()).abs() < tol);

        // special case: 0.0
        let test_lat = 0.0_f64.to_radians();
        let ans = 0.0;
        let geod = geoc2geod(test_lat, test_h);
        assert!((ans - geod.to_degrees()).abs() < 1e-15);

        // special case: 90.0
        let test_lat = 90.0_f64.to_radians();
        let ans = 90.0;
        let geod = geoc2geod(test_lat, test_h);
        assert!((ans - geod.to_degrees()).abs() < 1e-15);
    }

    #[test]
    #[should_panic]
    fn test_geoc2geod_panic() {
        let _ = geoc2geod(90.0001_f64.to_radians(), 0.0);
    }
}
