use crate::configs::constants::{ECC, RE_EQ_M};
use nalgebra::{Matrix3, Vector3};

/// Represents a curvilinear geodetic position.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Lla {
    /// Latitude in radians
    pub lat: f64,
    /// Longitude in radians
    pub lon: f64,
    /// Height/Altitude above the reference ellipsoid in meters
    pub alt: f64,
}

impl Lla {
    pub fn new(lat: f64, lon: f64, alt: f64) -> Self {
        Self { lat, lon, alt }
    }
}

/// Converts position, velocity, and attitude from ECEF- to NED-frame.
pub fn ecef2ned(
    r_eb_e: &Vector3<f64>,
    v_eb_e: &Vector3<f64>,
    c_b_e: &Matrix3<f64>,
) -> (Lla, Vector3<f64>, Matrix3<f64>) {
    // Convert position using Borkowski closed-form exact solution From (2.113)
    let lambda_b = r_eb_e[1].atan2(r_eb_e[0]);

    // From (C.29) and (C.30)
    let ecc_sq = ECC * ECC;
    let k1 = (1.0 - ecc_sq).sqrt() * r_eb_e[2].abs();
    let k2 = ecc_sq * RE_EQ_M;
    let beta = (r_eb_e[0].powi(2) + r_eb_e[1].powi(2)).sqrt();

    let e_val = (k1 - k2) / beta;
    let f_val = (k1 + k2) / beta;

    // From (C.31)
    let p = (4.0 / 3.0) * (e_val * f_val + 1.0);
    // From (C.32)
    let q = 2.0 * (e_val.powi(2) - f_val.powi(2));
    // From (C.33)
    let d = p.powi(3) + q.powi(2);

    // From (C.34)
    let v = (d.sqrt() - q).cbrt() - (d.sqrt() + q).cbrt();
    // From (C.35)
    let g = 0.5 * ((e_val.powi(2) + v).sqrt() + e_val);
    // From (C.36)
    let t = (g.powi(2) + (f_val - v * g) / (2.0 * g - e_val)).sqrt() - g;

    // From (C.37)
    let z_sign = r_eb_e[2].signum();
    let l_b = z_sign * ((1.0 - t.powi(2)) / (2.0 * t * (1.0 - ecc_sq).sqrt())).atan();
    // From (C.38)
    let h_b = (beta - RE_EQ_M * t) * l_b.cos()
        + (r_eb_e[2] - z_sign * RE_EQ_M * (1.0 - ecc_sq).sqrt()) * l_b.sin();

    let lla = Lla::new(l_b, lambda_b, h_b);

    let cos_lat = l_b.cos();
    let sin_lat = l_b.sin();
    let cos_long = lambda_b.cos();
    let sin_long = lambda_b.sin();

    // Calculate ECEF to NED coordinate transformation matrix using (2.150)
    let c_e_n = Matrix3::new(
        -sin_lat * cos_long,
        -sin_lat * sin_long,
        cos_lat,
        -sin_long,
        cos_long,
        0.0,
        -cos_lat * cos_long,
        -cos_lat * sin_long,
        -sin_lat,
    );

    // Transform velocity using (2.73)
    let v_eb_n = c_e_n * v_eb_e;
    // Transform attitude using (2.15)
    let c_b_n = c_e_n * c_b_e;

    (lla, v_eb_n, c_b_n)
}

/// Converts curvilinear position, velocity, and attitude from NED- to ECEF-frame.
pub fn ned2ecef(
    lla: &Lla,
    v_eb_n: &Vector3<f64>,
    c_b_n: &Matrix3<f64>,
) -> (Vector3<f64>, Vector3<f64>, Matrix3<f64>) {
    let cos_lat = lla.lat.cos();
    let sin_lat = lla.lat.sin();
    let cos_long = lla.lon.cos();
    let sin_long = lla.lon.sin();

    // Calculate transverse radius of curvature using (2.105)
    let ecc_sq = ECC * ECC;
    let r_e = RE_EQ_M / (1.0 - ecc_sq * sin_lat * sin_lat).sqrt();

    let r_eb_e = Vector3::new(
        (r_e + lla.alt) * cos_lat * cos_long,
        (r_e + lla.alt) * cos_lat * sin_long,
        ((1.0 - ecc_sq) * r_e + lla.alt) * sin_lat,
    );

    // Calculate ECEF to NED coordinate transformation matrix using (2.150)
    let c_e_n = Matrix3::new(
        -sin_lat * cos_long,
        -sin_lat * sin_long,
        cos_lat,
        -sin_long,
        cos_long,
        0.0,
        -cos_lat * cos_long,
        -cos_lat * sin_long,
        -sin_lat,
    );

    let c_e_n_t = c_e_n.transpose();
    // Transform velocity using (2.73)
    let v_eb_e = c_e_n_t * v_eb_n;
    // Transform attitude using (2.15)
    let c_b_e = c_e_n_t * c_b_n;

    (r_eb_e, v_eb_e, c_b_e)
}

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

    fn assert_v3_near(actual: &Vector3<f64>, expected: &Vector3<f64>, tol: f64) {
        for i in 0..3 {
            let diff = (actual[i] - expected[i]).abs();
            assert!(
                diff < tol,
                "Vector mismatch at index {}. Actual: {}, Expected: {}, Diff: {}",
                i,
                actual[i],
                expected[i],
                diff
            );
        }
    }

    fn assert_m3_near(actual: &Matrix3<f64>, expected: &Matrix3<f64>, tol: f64) {
        for r in 0..3 {
            for c in 0..3 {
                let diff = (actual[(r, c)] - expected[(r, c)]).abs();
                assert!(
                    diff < tol,
                    "Matrix mismatch at row {}, col {}. Actual: {}, Expected: {}, Diff: {}",
                    r,
                    c,
                    actual[(r, c)],
                    expected[(r, c)],
                    diff
                );
            }
        }
    }

    #[test]
    fn test_ecef2ned_explicit_values() {
        let l_b = 45.0f64.to_radians();
        let lambda_b = 45.0f64.to_radians();
        let h_b = 0.0;
        let lla_input = Lla::new(l_b, lambda_b, h_b);

        let v_eb_n = Vector3::new(10.0, 20.0, -30.0);
        let c_b_n = Matrix3::identity();

        let sin_lat = l_b.sin();
        let cos_lat = l_b.cos();
        let sin_long = lambda_b.sin();
        let cos_long = lambda_b.cos();
        let ecc_sq = ECC * ECC;
        let r_e = RE_EQ_M / (1.0 - ecc_sq * sin_lat * sin_lat).sqrt();

        let expected_r = Vector3::new(
            (r_e + h_b) * cos_lat * cos_long,
            (r_e + h_b) * cos_lat * sin_long,
            ((1.0 - ecc_sq) * r_e + h_b) * sin_lat,
        );

        let (r_eb_e, v_eb_e, c_b_e) = ned2ecef(&lla_input, &v_eb_n, &c_b_n);

        assert_v3_near(&r_eb_e, &expected_r, 1e-6);

        let c_e_n = Matrix3::new(
            -sin_lat * cos_long,
            -sin_lat * sin_long,
            cos_lat,
            -sin_long,
            cos_long,
            0.0,
            -cos_lat * cos_long,
            -cos_lat * sin_long,
            -sin_lat,
        );
        let expected_v = c_e_n.transpose() * v_eb_n;
        assert_v3_near(&v_eb_e, &expected_v, 1e-6);
        assert_m3_near(&c_b_e, &c_e_n.transpose(), 1e-9);
    }

    #[test]
    fn test_ned_ecef_mathematical_roundtrip() {
        let r_eb_e_orig = Vector3::new(3190000.0, 3190000.0, 4440000.0);
        let v_eb_e_orig = Vector3::new(-50.0, 120.5, 15.0);
        let c_b_e_orig = Matrix3::new(
            0.9362, 0.2896, 0.1987, -0.3130, 0.9447, 0.0978, -0.1593, -0.1538, 0.9752,
        );

        let (lla_mid, v_eb_n, c_b_n) = ecef2ned(&r_eb_e_orig, &v_eb_e_orig, &c_b_e_orig);
        let (r_eb_e_final, v_eb_e_final, c_b_e_final) = ned2ecef(&lla_mid, &v_eb_n, &c_b_n);

        assert_v3_near(&r_eb_e_final, &r_eb_e_orig, 1e-5);
        assert_v3_near(&v_eb_e_final, &v_eb_e_orig, 1e-6);
        assert_m3_near(&c_b_e_final, &c_b_e_orig, 1e-6);
    }
}
