use crate::constants::{J2, J3, J4, J5, J6, MU_M, RE_AVG_M, RE_EQ_M};
use crate::utils::msg_func;
use nalgebra::Vector3;

#[must_use]
/// Gravitation using zonal harmonic model.[^1]
///
/// order: 1 to 6; 1 means "mass point model"
///
/// [^1]: Analytical mechanics of space systems, pp.637-638
///
/// # Panics
/// Will panic if the acquiring position is within the Earth.
pub fn grav_zh(ribi: &Vector3<f64>, mut order: usize, mtlb: bool) -> Vector3<f64> {
    if !(1..7).contains(&order) {
        println!("[warn]: order {order} is out of range [1, 6] and has been set to 6");
        order = 6;
    }

    let mut mu = MU_M;
    let mut r_eq = RE_EQ_M;

    let mut j2 = J2;
    let mut j3 = J3;
    let mut j4 = J4;
    let mut j5 = J5;
    let mut j6 = J6;

    if mtlb {
        // MATLAB params
        mu = 3.986_004_415e14;
        r_eq = 6378.1363e3;
        j2 = 0.001_082_626_9;
        j3 = -0.000_002_532_3;
        j4 = -0.000_001_620_4;
        j5 = 0.0;
        j6 = 0.0;
    }

    let (x, y, z) = (ribi.x, ribi.y, ribi.z);
    let r = ribi.norm();
    //assert!(
    //    r >= RE_AVG_M,
    //    "[err] requested position {ribi:?} is within Earth... ({r} < {RE_AVG_M})"
    //);
    if r < RE_AVG_M {
        msg_func::warn_msg(&format!(
            "[err] requested position {ribi:?} is within Earth... ({r} < {RE_AVG_M})"
        ));
    }
    let mur2 = mu / r / r;
    let reqr = r_eq / r;
    let xr = x / r;
    let yr = y / r;
    let zr = z / r;

    let zr2 = zr * zr;
    let zr3 = zr2 * zr;
    let zr4 = zr2 * zr2;
    let zr5 = zr3 * zr2;
    let zr6 = zr3 * zr3;
    let reqr2 = reqr * reqr;
    let reqr3 = reqr2 * reqr;
    let reqr4 = reqr2 * reqr2;
    let reqr5 = reqr3 * reqr2;
    let reqr6 = reqr3 * reqr3;

    let aj2 = -3.0 / 2.0
        * j2
        * mur2
        * reqr2
        * Vector3::new(
            (1.0 - 5.0 * zr2) * xr,
            (1.0 - 5.0 * zr2) * yr,
            (3.0 - 5.0 * zr2) * zr,
        );

    let aj3 = 1.0 / 2.0
        * j3
        * mur2
        * reqr3
        * Vector3::new(
            5.0 * (7.0 * zr3 - 3.0 * zr) * xr,
            5.0 * (7.0 * zr3 - 3.0 * zr) * yr,
            3.0 * (1.0 - 10.0 * zr2 + 35.0 / 3.0 * zr4),
        );

    let aj4 = 5.0 / 8.0
        * j4
        * mur2
        * reqr4
        * Vector3::new(
            (3.0 - 42.0 * zr2 + 63.0 * zr4) * xr,
            (3.0 - 42.0 * zr2 + 63.0 * zr4) * yr,
            (15.0 - 70.0 * zr2 + 63.0 * zr4) * zr,
        );

    let aj5 = 1.0 / 8.0
        * j5
        * mur2
        * reqr5
        * Vector3::new(
            3.0 * (35.0 * zr - 210.0 * zr3 + 231.0 * zr5) * xr,
            3.0 * (35.0 * zr - 210.0 * zr3 + 231.0 * zr5) * yr,
            693.0 * zr6 - 945.0 * zr4 + 315.0 * zr2 - 15.0,
        );

    let aj6 = -1.0 / 16.0
        * j6
        * mur2
        * reqr6
        * Vector3::new(
            (35.0 - 945.0 * zr2 + 3465.0 * zr4 - 3003.0 * zr6) * xr,
            (35.0 - 945.0 * zr2 + 3465.0 * zr4 - 3003.0 * zr6) * yr,
            (245.0 - 2205.0 * zr2 + 4851.0 * zr4 - 3003.0 * zr6) * zr,
        );
    let aj1 = Vector3::zeros();
    let ajs = [aj1, aj2, aj3, aj4, aj5, aj6];
    let gamma: Vector3<f64> = -mur2 * ribi / r
        + ajs
            .iter()
            .take(order)
            .copied()
            .fold(aj1, |acc, vec| acc + vec);
    gamma
}

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

    //    #[test]
    //    #[should_panic]
    //    fn test_grav_zh_panic() {
    //        let R0 = RE_AVG_M - 1.0;
    //        // position within Earth
    //        let eci_loc = Vector3::new(R0, 0.0, 0.0);
    //        let _gamma = grav_zh(&eci_loc, 1, false);
    //    }
    #[test]
    fn test_grav_zh() {
        let R0 = RE_EQ_M;
        let eci_loc = Vector3::new(R0, R0 * 0.5, R0 * 1.5);

        // order 1 test
        let gamma_ans = Vector3::new(
            -1.4964011118743263,
            -0.7482005559371632,
            -2.2446016678114895,
        );
        let gamma = grav_zh(&eci_loc, 1, false);

        // order 6 test
        assert_eq!(gamma, gamma_ans);
        let gamma_ans = Vector3::new(
            -1.4948657637670792,
            -0.7474328818835396,
            -2.2443768065803353,
        );
        let gamma = grav_zh(&eci_loc, 6, false);
        assert_eq!(gamma, gamma_ans);

        // mtlb option test
        let gamma_ans = Vector3::new(
            -1.4948657141452983,
            -0.7474328570726492,
            -2.2443767751826527,
        );
        for order in 4..7 {
            let gamma = grav_zh(&eci_loc, order, true);
            assert_eq!(gamma, gamma_ans);
        }
    }
}
