anise 0.9.6

/*
 * ANISE Toolkit
 * Copyright (C) 2021-onward Christopher Rabotin <christopher.rabotin@gmail.com> et al. (cf. AUTHORS.md)
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
 *
 * Documentation: https://nyxspace.com/
 */

use std::f64::consts::{PI, TAU};

use crate::errors::{MathError, PhysicsError};

use super::PhysicsResult;

/// Mean anomaly f64::EPSILON
pub const MA_EPSILON: f64 = 1e-12;

#[macro_export]
macro_rules! f64_eq {
    ($x:expr, $val:expr, $msg:expr) => {
        f64_eq_tol!($x, $val, 1e-10, $msg)
    };
}

#[macro_export]
macro_rules! f64_eq_tol {
    ($x:expr, $val:expr, $tol:expr, $msg:expr) => {
        assert!(
            ($x - $val).abs() < $tol,
            "{}: {:.2e}\tgot: {}\twant: {}",
            $msg,
            ($x - $val).abs(),
            $x,
            $val
        )
    };
}

#[deprecated(since = "0.7.1", note = "use mean_anomaly_to_true_anomaly instead")]
pub fn compute_mean_to_true_anomaly_rad(ma_radians: f64, ecc: f64) -> PhysicsResult<f64> {
    mean_anomaly_to_true_anomaly_rad(ma_radians, ecc)
}

/// Computes the true anomaly from the given mean anomaly for an orbit.
///
/// The computation process varies depending on whether the orbit is elliptical (eccentricity less than or equal to 1)
/// or hyperbolic (eccentricity greater than 1). In each case, the method uses an iterative algorithm to find a
/// sufficiently accurate approximation of the true anomaly.
///
/// # Arguments
///
/// * `ma_radians` - The mean anomaly in radians.
/// * `ecc` - The eccentricity of the orbit.
///
/// # Remarks
///
/// This function uses GTDS MathSpec Equations 3-180, 3-181, and 3-186 for the iterative computation process.
///
/// Source: GMAT source code (`compute_mean_to_true_anomaly`)
pub fn mean_anomaly_to_true_anomaly_rad(ma_radians: f64, ecc: f64) -> PhysicsResult<f64> {
    let rm = ma_radians;
    if ecc <= 1.0 {
        // Elliptical orbit
        let mut e2 = rm + ecc * rm.sin(); // GTDS MathSpec Equation 3-182

        let mut iter = 0;

        loop {
            iter += 1;
            if iter > 1000 {
                return Err(PhysicsError::AppliedMath {
                    source: MathError::MaxIterationsReached {
                        iter,
                        action: "computing the true anomaly from the mean anomaly",
                    },
                });
            }

            // GTDS MathSpec Equation 3-180  Note: a little difference here is that it uses Cos(E) instead of Cos(E-0.5*f)
            let normalized_anomaly = 1.0 - ecc * e2.cos();

            if normalized_anomaly.abs() < MA_EPSILON {
                return Err(PhysicsError::AppliedMath {
                    source: MathError::DomainError {
                        value: normalized_anomaly,
                        msg: "normalized anomaly too small",
                    },
                });
            }

            // GTDS MathSpec Equation 3-181
            let e1 = e2 - (e2 - ecc * e2.sin() - rm) / normalized_anomaly;

            if (e2 - e1).abs() < MA_EPSILON {
                break;
            }

            e2 = e1;
        }

        let mut e = e2;

        if e < 0.0 {
            e += TAU;
        }

        let c = (e - PI).abs();

        let mut ta = if c >= 1.0e-08 {
            let normalized_anomaly = 1.0 - ecc;

            if (normalized_anomaly).abs() < MA_EPSILON {
                return Err(PhysicsError::AppliedMath {
                    source: MathError::DomainError {
                        value: normalized_anomaly,
                        msg: "normalized anomaly too small",
                    },
                });
            }

            let eccentricity_ratio = (1.0 + ecc) / normalized_anomaly; // temp2 = (1+ecc)/(1-ecc)

            if eccentricity_ratio < 0.0 {
                return Err(PhysicsError::AppliedMath {
                    source: MathError::DomainError {
                        value: eccentricity_ratio,
                        msg: "eccentricity ratio too small",
                    },
                });
            }

            let f = eccentricity_ratio.sqrt();
            let g = (e / 2.0).tan();
            // tan(TA/2) = Sqrt[(1+ecc)/(1-ecc)] * tan(E/2)
            2.0 * (f * g).atan()
        } else {
            e
        };

        if ta < 0.0 {
            ta += TAU;
        }
        Ok(ta)
    } else {
        //---------------------------------------------------------
        // hyperbolic orbit
        //---------------------------------------------------------

        // For hyperbolic orbit, anomaly is nolonger to be an angle so we cannot use mod of 2*PI to mean anomaly.
        // We need to keep its original value for calculation.
        //if (rm > PI)                       // incorrect
        //   rm = rm - TWO_PI;               // incorrect

        //f2 = ecc * Sinh(rm) - rm;          // incorrect
        //f2 = rm / 2;                       // incorrect  // GTDS MathSpec Equation 3-186
        let mut f2: f64 = 0.0; // This is the correct initial value for hyperbolic eccentric anomaly.
        let mut iter = 0;

        loop {
            iter += 1;
            if iter > 1000 {
                return Err(PhysicsError::AppliedMath {
                    source: MathError::MaxIterationsReached {
                        iter,
                        action: "computing the true anomaly from the mean anomaly",
                    },
                });
            }

            let normalizer = ecc * f2.cosh() - 1.0;

            if normalizer.abs() < MA_EPSILON {
                return Err(PhysicsError::AppliedMath {
                    source: MathError::DomainError {
                        value: normalizer,
                        msg: "normalized anomaly too small (hyperbolic case)",
                    },
                });
            }

            let f1 = f2 - (ecc * f2.sinh() - f2 - rm) / normalizer; // GTDS MathSpec Equation 3-186
            if (f2 - f1).abs() < MA_EPSILON {
                break;
            }
            f2 = f1;
        }

        let f = f2;
        let normalized_anomaly = ecc - 1.0;

        if normalized_anomaly.abs() < MA_EPSILON {
            return Err(PhysicsError::AppliedMath {
                source: MathError::DomainError {
                    value: normalized_anomaly,
                    msg: "normalized anomaly too small (hyperbolic case)",
                },
            });
        }

        let eccentricity_ratio = (ecc + 1.0) / normalized_anomaly; // temp2 = (ecc+1)/(ecc-1)

        if eccentricity_ratio < 0.0 {
            return Err(PhysicsError::AppliedMath {
                source: MathError::DomainError {
                    value: eccentricity_ratio,
                    msg: "eccentricity ratio too small (hyperbolic case)",
                },
            });
        }

        let e = eccentricity_ratio.sqrt();
        let g = (f / 2.0).tanh();
        let mut ta = 2.0 * (e * g).atan(); // tan(TA/2) = Sqrt[(ecc+1)/(ecc-1)] * Tanh(F/2)    where: F is hyperbolic centric anomaly

        if ta < 0.0 {
            ta += TAU;
        }
        Ok(ta)
    }
}

/// Computes the mean anomaly from the true anomaly.
///
/// # Arguments
///
/// * `nu_rad` - True anomaly in radians.
/// * `ecc` - Eccentricity.
///
/// # Returns
///
/// The mean anomaly in radians (normalized to `[0, 2*PI)`), or a `MathError` if the computation fails.
pub fn true_anomaly_to_mean_anomaly_rad(nu_rad: f64, ecc: f64) -> Result<f64, MathError> {
    // Step 1: Calculate Eccentric Anomaly E_rad
    // The true_anomaly_to_eccentric_anomaly_rad function already checks for ecc < 0.0
    let e_rad = true_anomaly_to_eccentric_anomaly_rad(nu_rad, ecc)?;

    // Step 2: Calculate Mean Anomaly M_rad based on eccentricity
    let m_rad = if ecc < 1.0 {
        // Elliptical or circular orbit: M = E - e * sin(E)
        e_rad - ecc * e_rad.sin()
    } else {
        // Hyperbolic orbit: M = e * sinh(E) - E
        ecc * e_rad.sinh() - e_rad
    };

    // Step 3: Normalize M_rad to be within the range [0, 2*PI)
    // For hyperbolic orbits, mean anomaly is not an angle in the same way,
    // but Kepler's equation for hyperbolic orbits (M = e*sinh(H) - H) is often used with M directly.
    // However, the problem asks to normalize it to [0, 2*PI), which implies it's being treated
    // as an angle for some purpose (perhaps consistency or specific conventions in this library).
    // If this normalization is not desired for hyperbolic cases in a broader context,
    // this part might need reconsideration for those cases.
    // For now, strictly following the requirement.
    let mut m_normalized_rad = m_rad % TAU;
    if m_normalized_rad < 0.0 {
        m_normalized_rad += TAU;
    }

    // Step 4: Return Ok(m_normalized_rad)
    Ok(m_normalized_rad)
}

/// Computes the eccentric anomaly from the true anomaly.
///
/// # Arguments
///
/// * `nu_rad` - True anomaly in radians.
/// * `ecc` - Eccentricity.
///
/// # Returns
///
/// The eccentric anomaly in radians, or a `MathError` if the computation fails.
pub fn true_anomaly_to_eccentric_anomaly_rad(nu_rad: f64, ecc: f64) -> Result<f64, MathError> {
    if ecc < 0.0 {
        return Err(MathError::DomainError {
            value: ecc,
            msg: "eccentricity cannot be negative",
        });
    }

    if ecc < 1.0 {
        // Elliptical or circular orbit
        // E = atan2( sqrt(1 - e^2) * sin(nu), e + cos(nu) )
        let e_num = (1.0 - ecc * ecc).sqrt() * nu_rad.sin();
        let e_den = ecc + nu_rad.cos();
        Ok(e_num.atan2(e_den))
    } else {
        // Hyperbolic orbit
        // E = 2.0 * atanh( sqrt((e - 1.0) / (e + 1.0)) * tan(nu / 2.0) )

        if (ecc + 1.0).abs() < f64::EPSILON {
            return Err(MathError::DivisionByZero {
                action: "computing hyperbolic eccentric anomaly, (e + 1.0) is zero",
            });
        }

        let factor_sqrt = (ecc - 1.0) / (ecc + 1.0);
        if factor_sqrt < 0.0 {
            // This case should ideally not happen if ecc >= 1.0,
            // but floating point inaccuracies might lead to it if ecc is very close to 1.0
            // or if ecc was negative (which is checked above).
            return Err(MathError::DomainError {
                value: factor_sqrt,
                msg: "argument for sqrt in hyperbolic case is negative",
            });
        }

        let tan_nu_half = (nu_rad / 2.0).tan();

        // The argument to atanh must be in (-1, 1).
        // If tan_nu_half is very large (nu_rad/2.0 approaches PI/2),
        // the argument might exceed this range.
        // sqrt((e-1)/(e+1)) is always < 1 for e >= 1.
        // So, the product can only be >= 1 or <= -1 if tan_nu_half is large enough.

        let atanh_arg = factor_sqrt.sqrt() * tan_nu_half;

        if atanh_arg >= 1.0 || atanh_arg <= -1.0 {
            // Check for edge cases like nu_rad = PI for ecc = 1.0 (parabolic)
            // For parabolic orbits (ecc = 1.0), the term sqrt((e-1)/(e+1)) becomes 0.
            // So atanh_arg would be 0 unless tan_nu_half is infinite.
            // If ecc is exactly 1.0, factor_sqrt is 0. So atanh_arg is 0 unless tan_nu_half is Inf/NaN.
            // If nu_rad makes tan(nu_rad/2.0) infinite (e.g. nu_rad = PI),
            // and ecc > 1, then atanh_arg would be Inf * a_number_lt_1 = Inf.
            // The original problem states: "if nu_rad / 2.0 makes tan very large or if ecc is exactly 1.0.
            // Specifically, if ecc == 1.0, this formula is problematic due to division by zero in the sqrt
            // and atanh of infinity."
            // The (e+1.0) division by zero is handled.
            // If ecc == 1.0, then factor_sqrt is 0.0. atanh_arg is 0.0 * tan(nu_rad/2.0).
            // If tan(nu_rad/2.0) is infinite (nu_rad = PI), then 0.0 * Inf = NaN.
            // f64::atanh(NaN) is NaN.
            // If atanh_arg is NaN, it will not satisfy >= 1.0 or <= -1.0.
            // Let's test for NaN explicitly as well.
            if atanh_arg.is_nan() {
                return Err(MathError::DomainError {
                    value: atanh_arg,
                    msg: "atanh argument is NaN in hyperbolic eccentric anomaly calculation",
                });
            }
            // If it's not NaN but out of [-1, 1] range
            return Err(MathError::DomainError {
                value: atanh_arg,
                msg: "atanh argument out of domain (-1, 1) in hyperbolic eccentric anomaly calculation",
            });
        }

        Ok(2.0 * atanh_arg.atanh())
    }
}

/// Solves Kepler's equation for Eccentric Anomaly (E) from Mean Anomaly (M) for elliptical orbits.
pub fn mean_anomaly_to_eccentric_anomaly_rad(
    ma_rad: f64,
    ecc: f64,
    tol: f64,
) -> PhysicsResult<f64> {
    if !(0.0..1.0).contains(&ecc) {
        return Err(PhysicsError::Hyperbolic { ecc });
    }

    // Initial guess
    let mut ea = ma_rad;

    for _ in 0..1000 {
        let f = ea - ecc * ea.sin() - ma_rad;
        let f_prime = 1.0 - ecc * ea.cos();

        if f_prime.abs() < 1.0e-12 {
            return Err(PhysicsError::AppliedMath {
                source: MathError::DomainError {
                    value: f_prime,
                    msg: "eccentricity anomaly iteration too small",
                },
            });
        }

        let delta = f / f_prime;
        ea -= delta;

        if delta.abs() < tol {
            return Ok(ea);
        }
    }

    Err(PhysicsError::AppliedMath {
        source: MathError::MaxIterationsReached {
            iter: 1000,
            action: "computing the eccentricity anomaly from the mean anomaly",
        },
    })
}
#[cfg(test)]
mod ut_utils {
    use super::*;
    use crate::errors::MathError;
    use std::f64::consts::PI;

    const TEST_EPS: f64 = 1e-9;

    // Tests for true_anomaly_to_eccentric_anomaly_rad
    #[test]
    fn test_ta_to_ea_elliptical() {
        // 1. Elliptical (e=0.5)
        let ecc = 0.5;
        // TA=0 -> EA=0
        let res1 = true_anomaly_to_eccentric_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI -> EA=PI
        let res2 = true_anomaly_to_eccentric_anomaly_rad(PI, ecc);
        f64_eq_tol!(res2.unwrap(), PI, TEST_EPS, "");

        // TA=PI/2 -> EA approx PI/3 (atan2(sqrt(1-0.25)*1, 0.5+0) = atan2(sqrt(0.75), 0.5) = atan2(0.8660254, 0.5) = 1.04719755 rad = PI/3)
        let res3 = true_anomaly_to_eccentric_anomaly_rad(PI / 2.0, ecc);
        f64_eq_tol!(res3.unwrap(), PI / 3.0, TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ea_circular() {
        // 2. Circular (e=0.0)
        let ecc = 0.0;
        // TA=0 -> EA=0
        let res1 = true_anomaly_to_eccentric_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI/2 -> EA=PI/2
        let res2 = true_anomaly_to_eccentric_anomaly_rad(PI / 2.0, ecc);
        f64_eq_tol!(res2.unwrap(), PI / 2.0, TEST_EPS, "");

        // TA=PI -> EA=PI
        let res3 = true_anomaly_to_eccentric_anomaly_rad(PI, ecc);
        f64_eq_tol!(res3.unwrap(), PI, TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ea_hyperbolic() {
        // 3. Hyperbolic (e=2.0)
        let ecc = 2.0;
        // TA=0 -> EA=0
        let res1 = true_anomaly_to_eccentric_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI/3 -> EA approx 0.69314718056
        // E = 2 * atanh(sqrt((2-1)/(2+1)) * tan(PI/6)) = 2 * atanh(sqrt(1/3) * 1/sqrt(3)) = 2 * atanh(1/3)
        // 2 * 0.34657359028 = 0.69314718056
        let res2 = true_anomaly_to_eccentric_anomaly_rad(PI / 3.0, ecc);
        f64_eq_tol!(res2.unwrap(), 2.0 * (1.0 / 3.0_f64).atanh(), TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ea_parabolic() {
        // 4. Parabolic (e=1.0)
        let ecc = 1.0;
        // TA=0 -> EA=0
        let res1 = true_anomaly_to_eccentric_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI/2 -> EA=0 (atanh_arg = sqrt(0/2)*tan(PI/4) = 0 * 1 = 0)
        let res2 = true_anomaly_to_eccentric_anomaly_rad(PI / 2.0, ecc);
        f64_eq_tol!(res2.unwrap(), 0.0, TEST_EPS, "");

        // Test TA=PI-0.00001 -> EA approaches 0 for e=1.0
        // tan( (PI-eps)/2 ) = tan(PI/2 - eps/2) -> large positive
        // atanh_arg = sqrt(0/2) * large_positive = 0 * large_positive = 0
        let res3 = true_anomaly_to_eccentric_anomaly_rad(PI - 0.00001, ecc);
        f64_eq_tol!(res3.unwrap(), 0.0, TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ea_hyperbolic_domain_errors() {
        // 5. Hyperbolic atanh domain error
        // Case 1: TA=PI, ecc=1.000000001 (nu/2 = PI/2, tan(PI/2) is Inf)
        // factor_sqrt = (1e-9)/(2.000000001) approx 0.5e-9. sqrt(factor_sqrt) approx 2.23e-5
        // atanh_arg = small_num * Inf = Inf
        let ecc1 = 1.000000001;
        let nu1 = PI;
        let res1 = true_anomaly_to_eccentric_anomaly_rad(nu1, ecc1);
        assert!(res1.is_err());
        matches!(res1.err().unwrap(), MathError::DomainError { .. });

        // Case 2: TA=0.7*PI, ecc=2.0. nu/2 = 0.35*PI. tan(0.35*PI) approx 1.9626105055
        // factor_sqrt = (2-1)/(2+1) = 1/3. sqrt(1/3) approx 0.577350269
        // atanh_arg = 0.577350269 * 1.9626105055 approx 1.13308688
        let ecc2 = 2.0;
        let nu2 = 0.7 * PI;
        let res2 = true_anomaly_to_eccentric_anomaly_rad(nu2, ecc2);
        assert!(res2.is_err());
        matches!(res2.err().unwrap(), MathError::DomainError { .. });
    }

    #[test]
    fn test_ta_to_ea_negative_ecc_error() {
        // 6. Negative eccentricity error
        let ecc = -0.1;
        let nu = PI / 2.0;
        let res = true_anomaly_to_eccentric_anomaly_rad(nu, ecc);
        assert!(res.is_err());
        assert_eq!(
            res.err().unwrap(),
            MathError::DomainError {
                value: -0.1,
                msg: "eccentricity cannot be negative"
            }
        );
    }

    // Tests for true_anomaly_to_mean_anomaly_rad
    #[test]
    fn test_ta_to_ma_elliptical() {
        // 1. Elliptical (e=0.5)
        let ecc = 0.5;
        // TA=0 -> EA=0 -> MA=0-0.5*sin(0)=0
        let res1 = true_anomaly_to_mean_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI -> EA=PI -> MA=PI-0.5*sin(PI)=PI
        let res2 = true_anomaly_to_mean_anomaly_rad(PI, ecc);
        f64_eq_tol!(res2.unwrap(), PI, TEST_EPS, "");

        // TA=PI/2 -> EA=PI/3 -> MA=PI/3 - 0.5*sin(PI/3) = PI/3 - 0.5*sqrt(3)/2 = 1.04719755 - 0.5*0.8660254/2 = 1.04719755 - 0.4330127/2 = 1.04719755 - 0.21650635 =  PI/3 - sqrt(3)/4 approx 0.6141848505
        let ea_for_pi_half = PI / 3.0;
        let expected_ma = ea_for_pi_half - ecc * ea_for_pi_half.sin(); // Approx 0.614184850519009
        let res3 = true_anomaly_to_mean_anomaly_rad(PI / 2.0, ecc);
        f64_eq_tol!(res3.unwrap(), expected_ma, TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ma_circular() {
        // 2. Circular (e=0.0)
        let ecc = 0.0;
        // TA=0 -> EA=0 -> MA=0
        let res1 = true_anomaly_to_mean_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI/2 -> EA=PI/2 -> MA=PI/2
        let res2 = true_anomaly_to_mean_anomaly_rad(PI / 2.0, ecc);
        f64_eq_tol!(res2.unwrap(), PI / 2.0, TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ma_hyperbolic() {
        // 3. Hyperbolic (e=2.0)
        let ecc = 2.0;
        // TA=0 -> EA=0 -> MA=2*sinh(0)-0=0
        let res1 = true_anomaly_to_mean_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI/3 -> EA approx 0.69314718056 (atanh(1/3)*2)
        // MA = 2*sinh(EA) - EA = 2*sinh(0.69314718056) - 0.69314718056
        // sinh(0.69314718056) = (e^0.69314718056 - e^-0.69314718056)/2 = (2 - 0.5)/2 = 1.5/2 = 0.75
        // MA = 2*0.75 - 0.69314718056 = 1.5 - 0.69314718056 = 0.80685281944
        let ea_for_pi_third = 2.0 * (1.0 / 3.0_f64).atanh();
        let expected_ma = ecc * ea_for_pi_third.sinh() - ea_for_pi_third; // Approx 0.8068528194400547
        let res2 = true_anomaly_to_mean_anomaly_rad(PI / 3.0, ecc);
        f64_eq_tol!(res2.unwrap(), expected_ma, TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ma_parabolic() {
        // 4. Parabolic (e=1.0)
        let ecc = 1.0;
        // TA=0 -> EA=0 -> MA = 1.0*sinh(0) - 0 = 0
        let res1 = true_anomaly_to_mean_anomaly_rad(0.0, ecc);
        f64_eq_tol!(res1.unwrap(), 0.0, TEST_EPS, "");

        // TA=PI/2 -> EA=0 -> MA = 1.0*sinh(0) - 0 = 0
        let res2 = true_anomaly_to_mean_anomaly_rad(PI / 2.0, ecc);
        f64_eq_tol!(res2.unwrap(), 0.0, TEST_EPS, "");
    }

    #[test]
    fn test_ta_to_ma_normalization() {
        // 5. Normalization
        let ecc = 0.1;
        let nu = 1.8 * PI; // equivalent to -0.2 * PI

        // For nu = 1.8*PI (324 deg), cos(nu) = cos(-36 deg) = 0.80901699
        // sin(nu) = sin(-36 deg) = -0.58778525
        // E = atan2(sqrt(1-0.01)*sin(nu), 0.1+cos(nu)) = atan2(sqrt(0.99)*(-0.58778525), 0.1+0.80901699)
        // E = atan2(0.994987437*(-0.58778525), 0.90901699) = atan2(-0.58483208, 0.90901699)
        // E approx -0.57056644 rad (which is approx -32.68 deg or 327.31 deg)
        // M = E - e*sin(E) = -0.57056644 - 0.1*sin(-0.57056644) = -0.57056644 - 0.1*(-0.5406722)
        // M = -0.57056644 + 0.05406722 = -0.51649922
        // Expected non-normalized M is -0.5164992201 rad
        // My calculation above was slightly off. Using code:
        let ea_expected = true_anomaly_to_eccentric_anomaly_rad(nu, ecc).unwrap();
        let m_expected_non_normalized = ea_expected - ecc * ea_expected.sin(); // approx -0.5164992200813001

        let res = true_anomaly_to_mean_anomaly_rad(nu, ecc);
        f64_eq_tol!(
            res.unwrap(),
            m_expected_non_normalized.rem_euclid(TAU),
            TEST_EPS,
            ""
        );

        let nu2 = -0.2 * PI; // Test with negative input nu
        let ea2_expected = true_anomaly_to_eccentric_anomaly_rad(nu2, ecc).unwrap();
        let m2_expected_non_normalized = ea2_expected - ecc * ea2_expected.sin();

        let res2 = true_anomaly_to_mean_anomaly_rad(nu2, ecc);
        f64_eq_tol!(
            res2.unwrap(),
            m2_expected_non_normalized.rem_euclid(TAU),
            TEST_EPS,
            ""
        );
        f64_eq_tol!(res.unwrap(), res2.unwrap(), TEST_EPS, ""); // Both should be same due to normalization
    }

    #[test]
    fn test_ta_to_ma_error_propagation() {
        // 6. Error propagation
        let ecc = -0.1;
        let nu = PI / 2.0;
        let res = true_anomaly_to_mean_anomaly_rad(nu, ecc);
        assert!(res.is_err());
        assert_eq!(
            res.err().unwrap(),
            MathError::DomainError {
                value: -0.1,
                msg: "eccentricity cannot be negative"
            }
        );
    }
}