use crate::constants::MU_KM;
use nalgebra::Vector3;

/// The naming here follows MATLAB's `ijk2keplerian()` function.
/// - a: Semi-major axis
/// - ecc: Orbit eccentricity
/// - incl: Inclination
/// - RAAN: Right ascension of ascending node
/// - argp: Angle between vehicle ascending node and periapsis
/// - nu: Angle between periapsis and current position of vehicle
/// - truelon: Angle between x-axis and vehicle position vector
/// - arglat: Angle between ascending node and vehicle position vector
/// - lonper: Angle between x-axis and eccentricity vector
#[derive(Debug)]
pub struct OrbitalElements {
    pub a: f64,
    pub ecc: f64,
    pub incl: f64,
    pub RAAN: f64,
    pub argp: f64,
    pub nu: f64,
    pub truelon: f64,
    pub arglat: f64,
    pub lonper: f64,
}

/// Convert position and velocity from ECI to orbit elements.[^1] [^2]
///
/// # Example
/// TODO...
///
/// NOTE:
/// The distances used in this function are in kilometers, while the inputs are in meters.
///
/// [^1]: Vallador (2013), Fundamentals of Astrodynamics and Applications, 4th ed., alg. 9.
/// [^2]: MATLAB (2025), <https://www.mathworks.com/help/aerotbx/ug/ijk2keplerian.html>
///
pub fn ijk2keplerian(r_in: &Vector3<f64>, v_in: &Vector3<f64>) -> OrbitalElements {
    // GM, Earth gravitational constant (km^3 s^-2)
    let mu = MU_KM;

    // m -> km
    let r = r_in / 1e3;
    let v = v_in / 1e3;

    let h = r.cross(&v);
    let h_norm = h.norm();

    let K = Vector3::new(0.0, 0.0, 1.0);
    let n = K.cross(&h);
    let n_norm = n.norm();

    let r_norm = r.norm();
    let v_norm = v.norm();
    let v2 = v_norm * v_norm;
    // eccentricity, ecc
    let e = ((v2 - mu / r_norm) * r - r.dot(&v) * v) / mu;
    let e_norm = e.norm();
    let xi = v2 / 2.0 - mu / r_norm;

    // semimajor axis, a
    // semiparameter, p; not used here
    //let a;
    //let mut p;
    let a = if (e_norm - 1.0).abs() > f64::EPSILON {
        -mu / (2.0 * xi)
        //p = a * (1.0 - e_norm * e_norm);
    } else {
        f64::INFINITY
        //p = h_norm * h_norm / mu;
    };

    // inclination, incl
    let i = ((h[2] / h_norm).acos()).to_degrees();
    // right ascension of the ascending node, RAAN
    let mut Omega = ((n[0] / n_norm).acos()).to_degrees();
    if n[1] < 0.0 {
        Omega = 360.0 - Omega;
    }
    // argument of perigee, argp
    let mut omega = ((n.dot(&e) / (n_norm * e_norm)).acos()).to_degrees();
    if e[2] < 0.0 {
        omega = 360.0 - omega;
    }
    // true anomaly, nu
    let mut nu = ((e.dot(&r) / (e_norm * r_norm)).acos()).to_degrees();
    if r.dot(&v) < 0.0 {
        nu = 360.0 - nu;
    }

    // Special Cases
    // true longitude of perigee, lonper (not used?)
    //let mut omega_true = ((e[0] / e_norm).acos()).to_degrees();
    //if e[1] < 0.0 {
    //    omega_true = 360.0 - omega_true;
    //}
    // longitude of perigee, lonper
    let omega_per = Omega + omega;
    // argument of lat, arglat
    let mut u = ((n.dot(&r) / (n_norm * r_norm)).acos()).to_degrees();
    if r[2] < 0.0 {
        u = 360.0 - u;
    }
    // true longitude, truelon
    let mut lambda_true = ((r[0] / r_norm).acos()).to_degrees();
    if r[1] < 0.0 {
        lambda_true = 360.0 - lambda_true;
    }

    OrbitalElements {
        a,
        ecc: e_norm,
        incl: i,
        RAAN: Omega,
        argp: omega,
        nu,
        truelon: lambda_true,
        arglat: u,
        lonper: omega_per,
    }
}

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

    #[test]
    fn test_ijk2keplerian() {
        let r_ijk = Vector3::new(-2981784.0, 5207055.0, 3161595.0);
        let v_ijk = Vector3::new(-3384.0, -4887.0, 4843.0);

        let oe = ijk2keplerian(&r_ijk, &v_ijk);
        let rel_tol = 1e-15;
        // oe.a unit: km
        assert!(((oe.a - 6.784480024503917e+03) / oe.a).abs() < rel_tol);
        assert!(((oe.ecc - 9.195020068497780e-04) / oe.ecc).abs() < rel_tol);
        assert!(((oe.incl - 51.752785685220600) / oe.incl).abs() < rel_tol);
        assert!(((oe.RAAN - 95.257032532242630) / oe.RAAN).abs() < rel_tol);
        assert!(((oe.argp - 1.064005206706784e+02) / oe.argp).abs() < rel_tol);
        assert!(((oe.nu - 2.900095663885400e+02) / oe.nu).abs() < rel_tol);
    }
}
