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

/// Converts position, velocity, and attitude from ECEF- to ECI-frame.
///
/// # Arguments
/// * `t` - Time (s)
/// * `r_eb_e` - Cartesian position of body frame w.r.t. ECEF frame, resolved along ECEF axes (m)
/// * `v_eb_e` - Velocity of body frame w.r.t. ECEF frame, resolved along ECEF axes (m/s)
/// * `c_b_e` - Body-to-ECEF coordinate transformation matrix
///
/// # Returns
/// A tuple containing:
/// * `r_ib_i` - Cartesian position of body frame w.r.t. ECI frame, resolved along ECI axes (m)
/// * `v_ib_i` - Velocity of body frame w.r.t. ECI frame, resolved along ECI axes (m/s)
/// * `c_b_i` - Body-to-ECI coordinate transformation matrix
pub fn ecef2eci(
    t: f64,
    r_eb_e: &Vector3<f64>,
    v_eb_e: &Vector3<f64>,
    c_b_e: &Matrix3<f64>,
) -> (Vector3<f64>, Vector3<f64>, Matrix3<f64>) {
    let theta = OMEGA_E * t;
    let cos_t = theta.cos();
    let sin_t = theta.sin();

    // ECEF to ECI coordinate transformation matrix
    let c_e_i = Matrix3::new(cos_t, -sin_t, 0.0, sin_t, cos_t, 0.0, 0.0, 0.0, 1.0);

    // Transform position
    let r_ib_i = c_e_i * r_eb_e;

    // Calculate Coriolis/transport effect component
    let transport_term = Vector3::new(-r_eb_e[1], r_eb_e[0], 0.0);

    // Transform velocity
    let v_ib_i = c_e_i * (v_eb_e + OMEGA_E * transport_term);

    // Transform attitude
    let c_b_i = c_e_i * c_b_e;

    (r_ib_i, v_ib_i, c_b_i)
}

/// Converts position, velocity, and attitude from ECI- to ECEF-frame.
pub fn eci2ecef(
    t: f64,
    r_ib_i: &Vector3<f64>,
    v_ib_i: &Vector3<f64>,
    c_b_i: &Matrix3<f64>,
) -> (Vector3<f64>, Vector3<f64>, Matrix3<f64>) {
    let theta = OMEGA_E * t;
    let cos_t = theta.cos();
    let sin_t = theta.sin();

    // ECI to ECEF coordinate transformation matrix
    let c_i_e = Matrix3::new(cos_t, sin_t, 0.0, -sin_t, cos_t, 0.0, 0.0, 0.0, 1.0);

    // Transform position
    let r_eb_e = c_i_e * r_ib_i;

    // Calculate Coriolis/transport effect component
    // MATLAB 1-based indexing: r_ib_i(2) is y (index 1), r_ib_i(1) is x (index 0)
    let transport_term = Vector3::new(-r_ib_i[1], r_ib_i[0], 0.0);

    // Transform velocity
    let v_eb_e = c_i_e * (v_ib_i - OMEGA_E * transport_term);

    // Transform attitude
    let c_b_e = c_i_e * c_b_i;

    (r_eb_e, v_eb_e, c_b_e)
}

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

    // Custom zero-dependency assertions to output clear error messages on failure
    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_ecef2eci_explicit_values() {
        // Setup simple, predictable values to verify step-by-step math
        // Let t = 10.0 seconds.
        // theta = OMEGA_E * 10 = 7.292115e-5 * 10 = 0.0007292115 radians
        let t = 10.0;
        let theta = OMEGA_E * t; // 0.0007292115

        let cos_t = theta.cos(); // 0.99999973413
        let sin_t = theta.sin(); // 0.00072921148

        let r_eb_e = Vector3::new(6000000.0, 3000000.0, 0.0);
        let v_eb_e = Vector3::new(100.0, 200.0, 0.0);
        let c_b_e = Matrix3::identity();

        // 1. Manually calculate expected C_e_i matrix
        let expected_c_e_i = Matrix3::new(cos_t, -sin_t, 0.0, sin_t, cos_t, 0.0, 0.0, 0.0, 1.0);

        // 2. Manually calculate expected Position: r_ib_i = C_e_i * r_eb_e
        let expected_r_ib_i = Vector3::new(
            cos_t * 6000000.0 - sin_t * 3000000.0,
            sin_t * 6000000.0 + cos_t * 3000000.0,
            0.0,
        );

        // 3. Manually calculate expected Velocity: v_ib_i = C_e_i * (v_eb_e + omega x r)
        // transport_term = [-y; x; 0] = [-3000000.0; 6000000.0; 0.0]
        // OMEGA_E * transport_term = [-218.76345; 437.5269; 0.0]
        // v_inside = v_eb_e + omega_r = [100.0 - 218.76345; 200.0 + 437.5269; 0.0] = [-118.76345; 637.5269; 0.0]
        let v_inside = Vector3::new(
            100.0 + (OMEGA_E * -3000000.0),
            200.0 + (OMEGA_E * 6000000.0),
            0.0,
        );
        let expected_v_ib_i = expected_c_e_i * v_inside;

        // Run actual function execution
        let (r_ib_i, v_ib_i, c_b_i) = ecef2eci(t, &r_eb_e, &v_eb_e, &c_b_e);

        // Verify matches exact manual outputs
        assert_v3_near(&r_ib_i, &expected_r_ib_i, 1e-6);
        assert_v3_near(&v_ib_i, &expected_v_ib_i, 1e-6);
        assert_m3_near(&c_b_i, &expected_c_e_i, 1e-9); // c_b_e is Identity, so c_b_i equals c_e_i
    }

    #[test]
    fn test_eci2ecef_explicit_values() {
        let t = 10.0;
        let theta = OMEGA_E * t;

        let cos_t = theta.cos();
        let sin_t = theta.sin();

        let r_ib_i = Vector3::new(6000000.0, 3000000.0, 0.0);
        let v_ib_i = Vector3::new(100.0, 200.0, 0.0);
        let c_b_i = Matrix3::identity();

        // 1. Manually calculate expected C_i_e matrix
        let expected_c_i_e = Matrix3::new(cos_t, sin_t, 0.0, -sin_t, cos_t, 0.0, 0.0, 0.0, 1.0);

        // 2. Manually calculate expected Position: r_eb_e = C_i_e * r_ib_i
        let expected_r_eb_e = Vector3::new(
            cos_t * 6000000.0 + sin_t * 3000000.0,
            -sin_t * 6000000.0 + cos_t * 3000000.0,
            0.0,
        );

        // 3. Manually calculate expected Velocity: v_eb_e = C_i_e * (v_ib_i - omega x r)
        // transport_term = [-y; x; 0] = [-3000000.0; 6000000.0; 0.0]
        // v_inside = v_ib_i - omega_r = [100.0 - (-218.76345); 200.0 - 437.5269; 0.0]
        let v_inside = Vector3::new(
            100.0 - (OMEGA_E * -3000000.0),
            200.0 - (OMEGA_E * 6000000.0),
            0.0,
        );
        let expected_v_eb_e = expected_c_i_e * v_inside;

        // Run actual function execution
        let (r_eb_e, v_eb_e, c_b_e) = eci2ecef(t, &r_ib_i, &v_ib_i, &c_b_i);

        // Verify matches exact manual outputs
        assert_v3_near(&r_eb_e, &expected_r_eb_e, 1e-6);
        assert_v3_near(&v_eb_e, &expected_v_eb_e, 1e-6);
        assert_m3_near(&c_b_e, &expected_c_i_e, 1e-9);
    }

    #[test]
    fn test_mathematical_roundtrip() {
        // Verifies reciprocal conversion accuracy with arbitrary values
        let t = 500.25;
        let r_start = Vector3::new(6378137.0, -12500.5, 45000.3);
        let v_start = Vector3::new(-150.0, 720.5, 12.0);
        let c_start = Matrix3::identity();

        let (r_eci, v_eci, c_eci) = ecef2eci(t, &r_start, &v_start, &c_start);
        let (r_back, v_back, c_back) = eci2ecef(t, &r_eci, &v_eci, &c_eci);

        assert_v3_near(&r_start, &r_back, 1e-6);
        assert_v3_near(&v_start, &v_back, 1e-6);
        assert_m3_near(&c_start, &c_back, 1e-9);
    }
}
