// SPDX-License-Identifier: Apache-2.0
//! Numerical orbit propagator: a configurable force model integrated by the adaptive
//! Runge–Kutta driver, the first **non-analytic** propagator in Kshana (the rest of the
//! orbit stack is the analytic SGP4/SDP4 of [`crate::sgp4`]).
//!
//! It wires the orbital force model of [`crate::forces`] (two-body `−μr/|r|³`, the J2/zonal
//! field, the **epoch-driven Sun/Moon third body**, **solar-radiation pressure**, and
//! **atmospheric drag**) into the step-doubling integrator of [`crate::integrator`], turning
//! `f(t, [r; v]) = [v; a(t, r, v)]` into a state propagator. The third-body and SRP terms are
//! genuinely *time-varying* (the [`crate::ephem`] Sun/Moon ephemerides are sampled at
//! `epoch_jd_tt + t/86400` each RHS evaluation — the Sun once, shared by the Sun third body and
//! SRP) and drag is genuinely *velocity-dependent* (it opposes the velocity relative to the
//! co-rotating atmosphere), so the RHS passes both `r` and `v` to the acceleration. Its
//! correctness is pinned
//! **against analytic truth that is stronger than a numerical cross-tool would be**:
//!
//! * the unperturbed two-body propagation must reproduce the **exact** universal-variable
//!   Kepler solution ([`crate::maneuver::kepler_universal`]) to sub-metre over a 24-hour LEO
//!   orbit — for the two-body problem the closed form *is* the truth, so this is a tighter
//!   gate than the "vs a numerical reference < 10 m" the milestone phrases it as;
//! * specific energy and angular momentum are conserved over the same arc;
//! * the J2-perturbed nodal regression reproduces the closed-form secular rate
//!   [`crate::forces::j2_secular_rates`] to first-order theory accuracy.
//!
//! It also exposes [`solve_kepler_checked`], a Newton solver for Kepler's equation that
//! **returns `Err` instead of a wrong answer** when it fails to converge within a bounded
//! iteration count (e.g. the near-perigee high-eccentricity `e = 0.999` case).
//!
//! ## Scope (honest)
//!
//! The force model spans **two-body + the J2..J6 zonal field + the epoch-driven Sun/Moon third
//! body + solar-radiation pressure + atmospheric drag + the post-Newtonian relativistic
//! correction** (the cannonball SRP model with a conical umbra+penumbra eclipse, quadratic drag
//! against the Vallado piecewise-exponential atmosphere, and the Schwarzschild perigee-advance
//! term). The full high-degree EGM tesseral field (a 200×200 gravity model and its coefficient
//! loader), the NRLMSISE-00 thermospheric density (the < 5 % drag-density clause), solar limb
//! darkening, the Lense–Thirring frame-dragging term — and the cross-validation of a 200×200 EGM
//! 24-hour orbit against an external high-precision propagator (GMAT/Orekit) — remain follow-ons
//! (see `ROADMAP.md`). The third-body/SRP ephemerides are the [`crate::ephem`] *low-precision*
//! analytical series (~0.005° Sun, ~0.3° Moon), not DE/SPK-kernel accuracy, and the drag density
//! is the static exponential model. What is delivered is the integrator + the
//! two-body/J2/zonal/third-body/SRP/drag/relativity force model + the analytic-truth validation
//! harness and the convergence-guarded Kepler solver.

use crate::ephem::{moon_position, sun_position};
use crate::forces::{
    drag_accel, gravity_accel, relativistic_accel, srp_accel, third_body_accel, two_body_accel,
    zonal_accel, EARTH_ZONALS_J2_J6, MU_EARTH, MU_MOON, MU_SUN,
};
use crate::integrator::{integrate, integrate_dopri, rk4_step, Tolerance};
use crate::precession::julian_centuries_tt;
use crate::timescales::SECONDS_PER_DAY;

type Vec3 = [f64; 3];

fn cross(a: Vec3, b: Vec3) -> Vec3 {
    [
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0],
    ]
}

fn dot(a: Vec3, b: Vec3) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}

fn norm(a: Vec3) -> f64 {
    dot(a, a).sqrt()
}

/// Which perturbations the propagated force model includes on top of two-body gravity.
#[derive(Clone, Copy, Debug, Default)]
pub struct ForceModel {
    /// Include the J2 oblateness perturbation.
    pub j2: bool,
    /// Optional zonal-harmonic field (`[J2, J3, …]` from degree 2) integrated via
    /// [`crate::forces::zonal_accel`]. When `Some`, it supersedes the `j2`-only path and the
    /// full supplied zonal set is used on top of two-body gravity.
    pub zonals: Option<&'static [f64]>,
    /// Include the Sun's third-body perturbation, using the [`crate::ephem::sun_position`]
    /// low-precision ephemeris advanced from [`epoch_jd_tt`](Self::epoch_jd_tt).
    pub sun: bool,
    /// Include the Moon's third-body perturbation, using [`crate::ephem::moon_position`].
    pub moon: bool,
    /// Propagation epoch as a Julian Date in TT (the instant of integration time `t = 0`). The
    /// perturber positions are evaluated at `epoch_jd_tt + t/86400` days, so the Sun/Moon
    /// actually advance along their orbits during the integration — this is what makes the
    /// third-body force *time-varying* rather than frozen at the start. Ignored when
    /// [`sun`](Self::sun), [`moon`](Self::moon) and [`srp`](Self::srp) are all `false`.
    pub epoch_jd_tt: f64,
    /// Include solar-radiation pressure ([`crate::forces::srp_accel`], cannonball model with the
    /// cylindrical-shadow eclipse), using the same epoch-driven [`crate::ephem::sun_position`].
    pub srp: bool,
    /// SRP radiation-pressure coefficient `cᵣ` (dimensionless; ≈1 absorptive, →2 specular). Used
    /// only when [`srp`](Self::srp) is `true`.
    pub cr: f64,
    /// SRP cross-section-to-mass ratio `A/m` (m²/kg). Used only when [`srp`](Self::srp) is `true`.
    pub area_over_mass: f64,
    /// Include atmospheric drag ([`crate::forces::drag_accel`], quadratic drag against the
    /// co-rotating atmosphere of [`crate::forces::atmospheric_density`]). Unlike the other terms
    /// this is **velocity-dependent**, so it is applied in [`accel_rv`](Self::accel_rv)/the RHS,
    /// not the position-only [`accel_at`](Self::accel_at).
    pub drag: bool,
    /// Drag ballistic area term `C_D·A/m` (m²/kg). Used only when [`drag`](Self::drag) is `true`.
    pub cd_area_over_mass: f64,
    /// Include the post-Newtonian (Schwarzschild) relativistic correction
    /// ([`crate::forces::relativistic_accel`]). Like atmospheric drag this is
    /// **velocity-dependent**, so it is applied in [`accel_rv`](Self::accel_rv)/the RHS rather
    /// than the position-only [`accel_at`](Self::accel_at).
    pub relativity: bool,
}

impl ForceModel {
    /// Point-mass (two-body) gravity only.
    pub fn two_body() -> Self {
        Self::default()
    }

    /// Two-body plus the J2 oblateness perturbation.
    pub fn with_j2() -> Self {
        Self {
            j2: true,
            ..Self::default()
        }
    }

    /// Two-body plus the full Earth zonal field through degree 6 (`J2..J6`).
    pub fn with_zonals_j2_j6() -> Self {
        Self {
            j2: true,
            zonals: Some(&EARTH_ZONALS_J2_J6),
            ..Self::default()
        }
    }

    /// Add the epoch-driven Sun and/or Moon third-body perturbation to this model. `epoch_jd_tt`
    /// is the Julian Date (TT) at integration time `t = 0`; the perturber positions advance to
    /// `epoch_jd_tt + t/86400` days during the integration, so the force is genuinely
    /// time-varying. Composable with any gravity model, e.g.
    /// `ForceModel::with_zonals_j2_j6().third_body(true, true, epoch)`.
    pub fn third_body(mut self, sun: bool, moon: bool, epoch_jd_tt: f64) -> Self {
        self.sun = sun;
        self.moon = moon;
        self.epoch_jd_tt = epoch_jd_tt;
        self
    }

    /// Add epoch-driven solar-radiation pressure to this model with radiation-pressure
    /// coefficient `cr` (≈1.0–2.0) and cross-section-to-mass ratio `area_over_mass` (m²/kg). SRP
    /// needs the Sun's position, so set the epoch via [`third_body`](Self::third_body) (or rely
    /// on `epoch_jd_tt = 0`); the Sun is sampled at the same advanced epoch as the third body, so
    /// `with_zonals_j2_j6().third_body(true, true, epoch).solar_radiation(1.5, 0.02)` shares one
    /// ephemeris evaluation. Composable with any gravity / third-body configuration.
    pub fn solar_radiation(mut self, cr: f64, area_over_mass: f64) -> Self {
        self.srp = true;
        self.cr = cr;
        self.area_over_mass = area_over_mass;
        self
    }

    /// Add atmospheric drag with ballistic area term `cd_area_over_mass = C_D·A/m` (m²/kg). Drag
    /// is **velocity-dependent**, so it enters via [`accel_rv`](Self::accel_rv) and the integrator
    /// RHS rather than the position-only [`accel_at`](Self::accel_at). Composable with any other
    /// configuration, e.g. `ForceModel::with_zonals_j2_j6().drag(0.02)`.
    pub fn drag(mut self, cd_area_over_mass: f64) -> Self {
        self.drag = true;
        self.cd_area_over_mass = cd_area_over_mass;
        self
    }

    /// Add the post-Newtonian (Schwarzschild) relativistic correction
    /// ([`crate::forces::relativistic_accel`]) to this model. Like drag it is
    /// velocity-dependent, so it enters via [`accel_rv`](Self::accel_rv) and the integrator RHS.
    /// It needs no parameters and composes with any configuration, e.g.
    /// `ForceModel::with_zonals_j2_j6().relativity()`.
    pub fn relativity(mut self) -> Self {
        self.relativity = true;
        self
    }

    /// The time-independent **central** gravity (m/s², ECI) at position `r` (m): two-body plus
    /// the configured J2/zonal field, but *not* the third-body terms (which depend on time
    /// through the ephemeris — see [`accel_at`](Self::accel_at)). For a model without Sun/Moon
    /// this is the full acceleration.
    pub fn accel(&self, r: Vec3) -> Vec3 {
        if let Some(jn) = self.zonals {
            let tb = two_body_accel(r);
            let zo = zonal_accel(r, jn);
            [tb[0] + zo[0], tb[1] + zo[1], tb[2] + zo[2]]
        } else if self.j2 {
            gravity_accel(r)
        } else {
            two_body_accel(r)
        }
    }

    /// The full acceleration (m/s², ECI) at integration time `t` (seconds since the epoch) and
    /// position `r` (m): the central gravity of [`accel`](Self::accel) plus, when enabled, the
    /// Sun/Moon third-body perturbations and solar-radiation pressure, all evaluated at the
    /// *advanced* epoch `epoch_jd_tt + t/86400`. The Sun ephemeris is computed once and shared by
    /// the Sun third body and SRP. When none of those are enabled this is identical to `accel(r)`
    /// for every `t`.
    pub fn accel_at(&self, t: f64, r: Vec3) -> Vec3 {
        let mut a = self.accel(r);
        if self.sun || self.moon || self.srp {
            let jd_tt = self.epoch_jd_tt + t / SECONDS_PER_DAY;
            let tjc = julian_centuries_tt(jd_tt);
            // The Sun ephemeris feeds both the Sun third body and SRP — evaluate it once.
            let sun = if self.sun || self.srp {
                Some(sun_position(tjc))
            } else {
                None
            };
            if self.sun {
                let p = third_body_accel(r, sun.unwrap(), MU_SUN);
                a = [a[0] + p[0], a[1] + p[1], a[2] + p[2]];
            }
            if self.moon {
                let p = third_body_accel(r, moon_position(tjc), MU_MOON);
                a = [a[0] + p[0], a[1] + p[1], a[2] + p[2]];
            }
            if self.srp {
                let p = srp_accel(r, sun.unwrap(), self.cr, self.area_over_mass);
                a = [a[0] + p[0], a[1] + p[1], a[2] + p[2]];
            }
        }
        a
    }

    /// The full acceleration (m/s², ECI) at time `t`, position `r` and velocity `v`: the
    /// position/time-dependent terms of [`accel_at`](Self::accel_at) plus, when enabled, the
    /// **velocity-dependent atmospheric drag** [`crate::forces::drag_accel`]. This is what the
    /// integrator RHS evaluates. With drag disabled it is identical to `accel_at(t, r)`.
    pub fn accel_rv(&self, t: f64, r: Vec3, v: Vec3) -> Vec3 {
        let mut a = self.accel_at(t, r);
        if self.drag {
            let d = drag_accel(r, v, self.cd_area_over_mass);
            a = [a[0] + d[0], a[1] + d[1], a[2] + d[2]];
        }
        if self.relativity {
            let g = relativistic_accel(r, v);
            a = [a[0] + g[0], a[1] + g[1], a[2] + g[2]];
        }
        a
    }

    /// The first-order ODE right-hand side `f(t, [r; v]) = [v; a(t, r, v)]` for the integrator.
    /// The `t`-dependence is real (Sun/Moon/SRP sampled at `epoch_jd_tt + t/86400`) and so is the
    /// velocity dependence (atmospheric drag opposes the velocity relative to the co-rotating
    /// atmosphere) — hence the RHS passes both `r` and `v` to [`accel_rv`](Self::accel_rv).
    fn rhs(&self) -> impl Fn(f64, &[f64]) -> Vec<f64> + '_ {
        move |t: f64, y: &[f64]| {
            let a = self.accel_rv(t, [y[0], y[1], y[2]], [y[3], y[4], y[5]]);
            vec![y[3], y[4], y[5], a[0], a[1], a[2]]
        }
    }
}

/// Numerically propagate the ECI state `(r0, v0)` (m, m/s) forward by `t_end` seconds under
/// `model`, with adaptive step-doubling error control to `tol`. Returns the final `(r, v)`.
pub fn propagate(
    r0: Vec3,
    v0: Vec3,
    t_end: f64,
    model: ForceModel,
    tol: &Tolerance,
) -> (Vec3, Vec3) {
    let f = model.rhs();
    let y0 = vec![r0[0], r0[1], r0[2], v0[0], v0[1], v0[2]];
    // Initial step: a small fraction of the orbital period is a safe, well-scaled guess.
    let h0 = (t_end / 1000.0).max(1.0).min(t_end.max(1e-3));
    let sol = integrate(&f, 0.0, &y0, t_end, h0, tol);
    (
        [sol.y[0], sol.y[1], sol.y[2]],
        [sol.y[3], sol.y[4], sol.y[5]],
    )
}

/// Numerically propagate `(r0, v0)` forward by `t_end` seconds under `model` using the adaptive
/// **Dormand–Prince RK5(4)** driver ([`crate::integrator::integrate_dopri`]) instead of step
/// doubling. Same force model and result as [`propagate`]; the embedded pair reaches the same
/// tolerance in fewer function evaluations (and the two agree to well within tolerance).
pub fn propagate_dopri(
    r0: Vec3,
    v0: Vec3,
    t_end: f64,
    model: ForceModel,
    tol: &Tolerance,
) -> (Vec3, Vec3) {
    let f = model.rhs();
    let y0 = vec![r0[0], r0[1], r0[2], v0[0], v0[1], v0[2]];
    let h0 = (t_end / 1000.0).max(1.0).min(t_end.max(1e-3));
    let sol = integrate_dopri(&f, 0.0, &y0, t_end, h0, tol);
    (
        [sol.y[0], sol.y[1], sol.y[2]],
        [sol.y[3], sol.y[4], sol.y[5]],
    )
}

/// Right ascension of the ascending node `Ω` (rad) of the osculating orbit for state
/// `(r, v)`: the in-plane angle of the node vector `n = ẑ × (r × v)`.
pub fn raan_rad(r: Vec3, v: Vec3) -> f64 {
    let h = cross(r, v);
    // n = ẑ × h = (−h_y, h_x, 0).
    let n = [-h[1], h[0], 0.0];
    n[1].atan2(n[0])
}

/// Two-body specific orbital energy `ε = v²/2 − μ/|r|` (J/kg). Conserved exactly under pure
/// two-body gravity (the integrator's drift check). Under a perturbing potential (J2 or the
/// zonal field) the *total* energy `ε − R(r)` including the disturbing potential
/// [`crate::forces::zonal_potential`] is the conserved quantity, not this bare two-body `ε`.
pub fn specific_energy(r: Vec3, v: Vec3) -> f64 {
    0.5 * dot(v, v) - MU_EARTH / norm(r)
}

/// Trace the orbit at a fixed RK4 step `h` (s) for `t_end` s, returning `(t, Ω(t))` samples.
/// A fixed step (rather than the adaptive driver, which only returns the final state) gives a
/// uniform time series for fitting the secular nodal rate. Small `h` keeps the truncation
/// error far below the secular signal being measured.
pub fn nodal_history(r0: Vec3, v0: Vec3, t_end: f64, h: f64, model: ForceModel) -> Vec<(f64, f64)> {
    let f = model.rhs();
    let mut y = vec![r0[0], r0[1], r0[2], v0[0], v0[1], v0[2]];
    let mut t = 0.0;
    let mut out = vec![(0.0, raan_rad(r0, v0))];
    while t < t_end - 1e-9 {
        y = rk4_step(&f, t, &y, h);
        t += h;
        out.push((t, raan_rad([y[0], y[1], y[2]], [y[3], y[4], y[5]])));
    }
    out
}

/// Least-squares slope of an unwrapped angle time series `(t, θ)` (rad/s) — the secular rate
/// with the periodic (short-period) oscillation averaged out. The angle is unwrapped to
/// remove ±2π jumps before fitting.
pub fn secular_slope(samples: &[(f64, f64)]) -> f64 {
    // Unwrap.
    let mut theta = Vec::with_capacity(samples.len());
    let mut prev = samples[0].1;
    let mut offset = 0.0;
    for &(_, th) in samples {
        let mut d = th - prev;
        while d > std::f64::consts::PI {
            d -= std::f64::consts::TAU;
        }
        while d < -std::f64::consts::PI {
            d += std::f64::consts::TAU;
        }
        offset += d;
        theta.push(samples[0].1 + offset);
        prev = th;
    }
    // Ordinary least-squares slope.
    let n = samples.len() as f64;
    let mean_t = samples.iter().map(|&(t, _)| t).sum::<f64>() / n;
    let mean_y = theta.iter().sum::<f64>() / n;
    let mut num = 0.0;
    let mut den = 0.0;
    for (i, &(t, _)) in samples.iter().enumerate() {
        num += (t - mean_t) * (theta[i] - mean_y);
        den += (t - mean_t) * (t - mean_t);
    }
    num / den
}

/// Non-convergence of [`solve_kepler_checked`]: the residual still exceeded the tolerance
/// after the iteration budget was spent.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct KeplerNonConvergence {
    /// `|E − e·sin E − M|` at the last iterate.
    pub residual: f64,
    /// Iterations spent.
    pub iters: usize,
}

/// Solve Kepler's equation `M = E − e·sin E` for the eccentric anomaly `E` (rad) by
/// Newton–Raphson from the `E₀ = M` start, **returning `Err` rather than a wrong answer**
/// when it does not converge within `max_iter`. The bare `E₀ = M` start is deliberately
/// simple so the guard is meaningful: a near-perigee high-eccentricity case (`e = 0.999`)
/// where `f'(E) = 1 − e·cos E` collapses toward zero overshoots and fails to converge in a
/// bounded budget — exactly the case a silent fixed-iteration solver would return garbage
/// for. (A robust universal starter such as Markley's is a follow-on; this routine's job is
/// the convergence *guard*.)
pub fn solve_kepler_checked(
    mean_anomaly: f64,
    e: f64,
    max_iter: usize,
) -> Result<f64, KeplerNonConvergence> {
    const TOL: f64 = 1e-12;
    if e == 0.0 {
        return Ok(mean_anomaly);
    }
    let mut ea = mean_anomaly;
    for _ in 0..max_iter {
        let residual = ea - e * ea.sin() - mean_anomaly;
        if residual.abs() < TOL {
            return Ok(ea);
        }
        let deriv = 1.0 - e * ea.cos();
        ea -= residual / deriv;
    }
    let final_residual = (ea - e * ea.sin() - mean_anomaly).abs();
    if final_residual < TOL {
        Ok(ea)
    } else {
        Err(KeplerNonConvergence {
            residual: final_residual,
            iters: max_iter,
        })
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::forces::j2_secular_rates;
    use crate::maneuver::kepler_universal;

    /// A representative LEO circular-ish state at a = 7000 km, i = 45°.
    fn leo_state() -> (Vec3, Vec3) {
        let a = 7.0e6;
        let v = (MU_EARTH / a).sqrt(); // circular speed
        let inc = 45.0_f64.to_radians();
        // r along +x; v in the orbit plane inclined 45° about x.
        let r0 = [a, 0.0, 0.0];
        let v0 = [0.0, v * inc.cos(), v * inc.sin()];
        (r0, v0)
    }

    #[test]
    fn two_body_propagation_matches_the_exact_kepler_solution_over_24h() {
        // The unperturbed numerical orbit must land on the exact universal-variable Kepler
        // solution — for two-body that closed form IS the truth, a tighter gate than a
        // numerical reference.
        let (r0, v0) = leo_state();
        let day = 86_400.0;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        let (r_num, _v_num) = propagate(r0, v0, day, ForceModel::two_body(), &tol);
        let (r_exact, _v_exact) = kepler_universal(r0, v0, day, MU_EARTH);
        let err = norm([
            r_num[0] - r_exact[0],
            r_num[1] - r_exact[1],
            r_num[2] - r_exact[2],
        ]);
        assert!(err < 10.0, "24h two-body residual {err} m vs exact Kepler");
        // In fact it is far below the 10 m the milestone asks for.
        assert!(err < 1.0, "should be sub-metre: {err} m");
    }

    #[test]
    fn dopri_propagation_matches_exact_kepler_and_agrees_with_the_rk4_path() {
        // The Dormand–Prince RK5(4) propagator must clear the same analytic-truth gate as the
        // step-doubling one: sub-metre against the exact universal-variable Kepler solution over a
        // 24 h LEO orbit. And the two adaptive drivers must agree with each other to well within
        // tolerance (same force model, same physics — only the error-control scheme differs).
        let (r0, v0) = leo_state();
        let day = 86_400.0;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        let (r_dp, _) = propagate_dopri(r0, v0, day, ForceModel::two_body(), &tol);
        let (r_exact, _) = kepler_universal(r0, v0, day, MU_EARTH);
        let err = norm([
            r_dp[0] - r_exact[0],
            r_dp[1] - r_exact[1],
            r_dp[2] - r_exact[2],
        ]);
        assert!(
            err < 1.0,
            "DP5(4) 24h two-body residual {err} m vs exact Kepler"
        );

        // The two drivers agree on a perturbed (J2..J6) orbit, where there is no closed form.
        let (r_rk4, _) = propagate(r0, v0, day, ForceModel::with_zonals_j2_j6(), &tol);
        let (r_dp2, _) = propagate_dopri(r0, v0, day, ForceModel::with_zonals_j2_j6(), &tol);
        let agree = norm([
            r_rk4[0] - r_dp2[0],
            r_rk4[1] - r_dp2[1],
            r_rk4[2] - r_dp2[2],
        ]);
        assert!(
            agree < 1.0,
            "step-doubling and DP5(4) must agree on the J2..J6 orbit: {agree} m"
        );
    }

    #[test]
    fn two_body_conserves_energy_and_angular_momentum() {
        let (r0, v0) = leo_state();
        let day = 86_400.0;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        let e0 = specific_energy(r0, v0);
        let h0 = norm(cross(r0, v0));
        let (r1, v1) = propagate(r0, v0, day, ForceModel::two_body(), &tol);
        let e1 = specific_energy(r1, v1);
        let h1 = norm(cross(r1, v1));
        assert!(
            (e1 - e0).abs() / e0.abs() < 1e-9,
            "energy drift {}",
            e1 - e0
        );
        assert!(
            (h1 - h0).abs() / h0 < 1e-9,
            "ang-momentum drift {}",
            h1 - h0
        );
    }

    #[test]
    fn j2_nodal_regression_reproduces_the_secular_formula() {
        // Propagate a J2-perturbed ISS-like orbit for a day and fit the secular RAAN rate;
        // it must agree with the closed-form first-order j2_secular_rates. The agreement is
        // to first-order theory (the residual is the O(J2²) higher-order secular term, ~0.1%),
        // not to machine precision — so this is a physics check, validated within 2%.
        let a = 6.778e6; // ~400 km altitude
        let inc = 51.6_f64.to_radians();
        let v = (MU_EARTH / a).sqrt();
        let r0 = [a, 0.0, 0.0];
        let v0 = [0.0, v * inc.cos(), v * inc.sin()];

        let day = 86_400.0;
        let hist = nodal_history(r0, v0, day, 10.0, ForceModel::with_j2());
        let rate_num = secular_slope(&hist);
        let rate_formula = j2_secular_rates(a, 0.0, inc).raan;

        // Both are the westward nodal regression (negative for a prograde orbit).
        assert!(rate_num < 0.0 && rate_formula < 0.0);
        let rel = (rate_num - rate_formula).abs() / rate_formula.abs();
        assert!(
            rel < 0.02,
            "numerical Ω̇ {rate_num} vs formula {rate_formula} (rel {rel})"
        );
        // Sanity: the day's nodal drift is the textbook ~ −5°/day for the ISS.
        let deg_per_day = rate_formula.to_degrees() * day;
        assert!((deg_per_day + 5.0).abs() < 0.6, "Ω̇ {deg_per_day} °/day");
    }

    #[test]
    fn solve_kepler_converges_on_a_well_conditioned_case() {
        // Moderate eccentricity converges and satisfies the equation.
        let m = 1.0;
        let e = 0.3;
        let ea = solve_kepler_checked(m, e, 30).expect("should converge");
        assert!((ea - e * ea.sin() - m).abs() < 1e-12);
        // Circular is exact in one shot.
        assert_eq!(solve_kepler_checked(0.7, 0.0, 30), Ok(0.7));
    }

    #[test]
    fn solve_kepler_returns_err_on_nonconvergence_at_high_eccentricity() {
        // At e = 0.999 the Newton step from the bare E₀ = M start overshoots wildly for M
        // near perigee; M = 0.3 rad diverges and does not converge within 30 iterations, so
        // the solver returns Err rather than a silently wrong eccentric anomaly. (Most other
        // M values do converge — see the well-conditioned test — so this is a real guard, not
        // a broken solver.)
        let result = solve_kepler_checked(0.3, 0.999, 30);
        assert!(
            result.is_err(),
            "expected non-convergence Err, got {result:?}"
        );
        if let Err(nc) = result {
            assert_eq!(nc.iters, 30);
            assert!(nc.residual > 1e-12);
        }
    }

    #[test]
    fn third_body_rhs_samples_the_ephemeris_at_the_advanced_epoch() {
        // The headline of this wave: the third body is wired into the *time-varying* RHS, with
        // the perturber position read at epoch_jd_tt + t/86400. Prove the wiring exactly (to
        // machine precision), not with a tolerance band: the Sun term added by accel_at must be
        // the third-body acceleration evaluated at the ephemeris position for that instant.
        use crate::timescales::JD_J2000;
        let epoch = JD_J2000; // 2000-01-01 12:00 TT → tjc = 0 at t = 0.
        let model = ForceModel::two_body().third_body(true, false, epoch);
        let r = [7.0e6, 1.0e6, -2.0e6];
        let central = model.accel(r);

        // At t = 0 the Sun is sampled at tjc(epoch). Compare against central + the hand-composed
        // third-body term by *bit-identical* equality (accel_at adds the very same vector), which
        // dodges the catastrophic cancellation of subtracting the ~8 m/s² central term.
        let a0 = model.accel_at(0.0, r);
        let sun0 = sun_position(julian_centuries_tt(epoch));
        let expect0 = third_body_accel(r, sun0, MU_SUN);
        for k in 0..3 {
            assert_eq!(
                a0[k],
                central[k] + expect0[k],
                "t=0 Sun term mismatch axis {k}"
            );
        }

        // At t = one day the Sun is sampled exactly one day later (the 86400 s ↔ 1 day, and the
        // 1 day ↔ 1/36525 century, conversions the wiring depends on).
        let a1 = model.accel_at(SECONDS_PER_DAY, r);
        let sun1 = sun_position(julian_centuries_tt(epoch + 1.0));
        let expect1 = third_body_accel(r, sun1, MU_SUN);
        for k in 0..3 {
            assert_eq!(
                a1[k],
                central[k] + expect1[k],
                "t=1day Sun term mismatch axis {k}"
            );
        }
        // And the perturber genuinely moved between the two samples (~1°/day of solar motion),
        // so the RHS is not silently frozen at the start.
        let moved = norm([sun1[0] - sun0[0], sun1[1] - sun0[1], sun1[2] - sun0[2]]);
        assert!(
            moved > 1e9,
            "Sun should advance ~2.6e9 m in a day, moved {moved} m"
        );

        // With no third body enabled, accel_at is time-independent and equals accel — the
        // existing two-body/J2/zonal goldens are untouched.
        let plain = ForceModel::with_j2();
        for &t in &[0.0, 1234.0, SECONDS_PER_DAY] {
            assert_eq!(plain.accel_at(t, r), plain.accel(r));
        }
    }

    #[test]
    fn third_body_perturbs_a_leo_orbit_at_the_textbook_magnitudes() {
        // Two robust, hand-derivable signatures. (1) The *instantaneous* third-body tidal
        // acceleration at LEO sits at the textbook magnitudes — Sun ~5e-7 m/s², Moon ~1.1e-6
        // m/s² (the Moon's geocentric tidal pull is ~2× the Sun's). These follow directly from
        // a_tidal ≈ 2·GM·R_orbit/d³ and need no integration, so they cannot be confounded by the
        // finite-arc geometry. (2) Over a day each body moves the orbit measurably off the
        // two-body path but stays a small, bounded perturbation. NOTE: the day-long *displacement*
        // ordering is NOT simply the accel ratio (the Moon's tidal axis rotates ~13×/day faster,
        // changing the forcing direction), so only the instantaneous accel ordering is asserted.
        use crate::timescales::JD_J2000;
        let (r0, v0) = leo_state();
        let day = 86_400.0;
        let epoch = JD_J2000;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };

        // (1) Instantaneous tidal magnitudes = accel_at minus the central gravity.
        let central = ForceModel::two_body().accel(r0);
        let tidal = |m: ForceModel| {
            let a = m.accel_at(0.0, r0);
            norm([a[0] - central[0], a[1] - central[1], a[2] - central[2]])
        };
        let a_sun = tidal(ForceModel::two_body().third_body(true, false, epoch));
        let a_moon = tidal(ForceModel::two_body().third_body(false, true, epoch));
        assert!(
            (2.0e-7..=8.0e-7).contains(&a_sun),
            "Sun tidal accel {a_sun} m/s² outside ~5e-7 band"
        );
        assert!(
            (5.0e-7..=2.0e-6).contains(&a_moon),
            "Moon tidal accel {a_moon} m/s² outside ~1.1e-6 band"
        );
        assert!(
            a_moon > a_sun,
            "Moon tidal accel ({a_moon}) must exceed the Sun's ({a_sun})"
        );

        // (2) Each body perturbs the day-long trajectory, bounded.
        let (r_tb, _) = propagate(r0, v0, day, ForceModel::two_body(), &tol);
        let sep = |m: ForceModel| {
            let (r, _) = propagate(r0, v0, day, m, &tol);
            norm([r[0] - r_tb[0], r[1] - r_tb[1], r[2] - r_tb[2]])
        };
        for (name, m) in [
            ("Sun", ForceModel::two_body().third_body(true, false, epoch)),
            (
                "Moon",
                ForceModel::two_body().third_body(false, true, epoch),
            ),
        ] {
            let s = sep(m);
            assert!(
                s > 1e-3,
                "{name} third body must perturb the orbit, sep {s} m"
            );
            assert!(
                s < 1e5,
                "{name} third body must stay a small perturbation, sep {s} m"
            );
        }
    }

    #[test]
    fn third_body_propagation_depends_on_the_epoch() {
        // Epoch-driven means the same initial state + arc gives a different trajectory at a
        // different epoch. Choosing epochs a quarter-year apart rotates the Sun (and its tidal
        // axis) by ~90° relative to the inertial orbit — a full-order change in the tidal field
        // (unlike a half-year flip, which leaves the leading tidal tensor nearly invariant). So
        // the two final states must differ, while each stays a bounded small perturbation.
        use crate::timescales::JD_J2000;
        let (r0, v0) = leo_state();
        let arc = 2.0 * 86_400.0;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        let prop = |epoch: f64| {
            propagate(
                r0,
                v0,
                arc,
                ForceModel::two_body().third_body(true, false, epoch),
                &tol,
            )
            .0
        };
        let r_jan = prop(JD_J2000); // Sun near ecliptic longitude ~280°.
        let r_apr = prop(JD_J2000 + 91.31); // ~90° of solar motion later.
        let diff = norm([
            r_jan[0] - r_apr[0],
            r_jan[1] - r_apr[1],
            r_jan[2] - r_apr[2],
        ]);
        assert!(
            diff > 1e-3,
            "epoch must change the trajectory, diff {diff} m"
        );
        assert!(diff < 1e5, "epoch effect must stay bounded, diff {diff} m");
    }

    #[test]
    fn srp_perturbs_a_leo_orbit_and_scales_linearly_with_area_to_mass() {
        // SRP rides the same epoch-driven RHS as the third body. Two hand-derivable signatures:
        // (1) over a day it nudges a LEO orbit off the no-SRP path by a small, bounded amount;
        // (2) the cannonball SRP acceleration is exactly linear in A/m and the cylindrical-shadow
        // factor is position-only (identical across A/m to ~ms timing), so to first order the
        // day-long displacement scales ~linearly — doubling A/m ~doubles the separation.
        use crate::timescales::JD_J2000;
        let (r0, v0) = leo_state();
        let day = 86_400.0;
        let epoch = JD_J2000;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        // Baseline = full gravity + Sun/Moon third body, *no* SRP.
        let base = ForceModel::with_zonals_j2_j6().third_body(true, true, epoch);
        let (r_base, _) = propagate(r0, v0, day, base, &tol);
        let sep = |aom: f64| {
            let (r, _) = propagate(r0, v0, day, base.solar_radiation(1.5, aom), &tol);
            norm([r[0] - r_base[0], r[1] - r_base[1], r[2] - r_base[2]])
        };
        let s1 = sep(0.02);
        let s2 = sep(0.04);
        assert!(s1 > 1e-3, "SRP must perturb the orbit, sep {s1} m");
        assert!(s1 < 1e4, "SRP must stay a small perturbation, sep {s1} m");
        let ratio = s2 / s1;
        assert!(
            (1.8..=2.2).contains(&ratio),
            "SRP displacement should scale ~linearly with A/m, ratio {ratio}"
        );
    }

    #[test]
    fn drag_dissipates_energy_and_decays_the_orbit_monotonically() {
        // Drag is the one dissipative term: unlike the energy-conserving vacuum/zonal/third-body
        // models, a drag-perturbed orbit must LOSE specific energy and semi-major axis over time
        // (the orbit sinks). Use a 300 km orbit (denser atmosphere → measurable decay in a day).
        let alt = 300e3;
        let r0 = [crate::forces::RE_EARTH + alt, 0.0, 0.0];
        let vc = (MU_EARTH / (crate::forces::RE_EARTH + alt)).sqrt();
        let inc = 45.0_f64.to_radians();
        let v0 = [0.0, vc * inc.cos(), vc * inc.sin()];
        let day = 86_400.0;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        let drag_model = ForceModel::two_body().drag(0.02);
        let e0 = specific_energy(r0, v0);

        // The vacuum baseline conserves energy to ~1e-9; drag must strictly lower it.
        let (r_vac, v_vac) = propagate(r0, v0, day, ForceModel::two_body(), &tol);
        assert!(
            (specific_energy(r_vac, v_vac) - e0).abs() / e0.abs() < 1e-9,
            "vacuum orbit must conserve energy"
        );

        // Secular decay: the two-body energy at successive day-fractions strictly decreases (each
        // sample spans several orbits, so the secular trend dominates the per-orbit ripple).
        let mut prev = e0;
        let mut e_day = e0;
        for k in 1..=4 {
            let (r, v) = propagate(r0, v0, day * f64::from(k) / 4.0, drag_model, &tol);
            e_day = specific_energy(r, v);
            assert!(
                e_day < prev,
                "drag energy must decay monotonically: step {k} e {e_day} not < prev {prev}"
            );
            prev = e_day;
        }
        // Semi-major axis a = −μ/(2ε) shrank (ε more negative ⇒ smaller a ⇒ the orbit decayed).
        let a0 = -MU_EARTH / (2.0 * e0);
        let a_day = -MU_EARTH / (2.0 * e_day);
        assert!(
            a_day < a0,
            "drag must shrink the semi-major axis: a_day {a_day} not < a0 {a0}"
        );
        // And the decay is a real, bounded amount over the day (not numerical noise, not a crash).
        let drop = a0 - a_day;
        assert!(
            (1.0..=1e5).contains(&drop),
            "a-decay {drop} m/day outside the physical band for 300 km, C_D·A/m = 0.02"
        );
    }

    #[test]
    fn relativity_perturbs_the_orbit_without_dissipating_it() {
        // The Schwarzschild correction is conservative — it precesses the perigee but, unlike
        // drag, must NOT secularly decay the orbit. Two contrasting signatures over a day:
        // (1) it nudges the trajectory off the two-body path by a small bounded amount, yet
        // (2) it leaves the semi-major axis essentially unchanged (no energy sink) — the
        // opposite of `drag_dissipates_energy_and_decays_the_orbit_monotonically` above.
        let (r0, v0) = leo_state();
        let day = 86_400.0;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        let base = ForceModel::two_body();
        let (r_base, _) = propagate(r0, v0, day, base, &tol);
        let (r_gr, v_gr) = propagate(r0, v0, day, base.relativity(), &tol);
        let sep = norm([
            r_gr[0] - r_base[0],
            r_gr[1] - r_base[1],
            r_gr[2] - r_base[2],
        ]);
        assert!(sep > 1e-4, "relativity must perturb the orbit, sep {sep} m");
        assert!(
            sep < 1e5,
            "relativity must stay a tiny perturbation, sep {sep} m"
        );

        // Non-dissipative: a = −μ/(2ε) holds to well under a metre/day, whereas drag sinks it
        // by ≥1 m/day. This is the structural difference between a conservative and a
        // dissipative perturbation.
        let a0 = -MU_EARTH / (2.0 * specific_energy(r0, v0));
        let a_gr = -MU_EARTH / (2.0 * specific_energy(r_gr, v_gr));
        assert!(
            (a_gr - a0).abs() < 10.0,
            "relativity must not decay the semi-major axis: Δa {} m",
            a_gr - a0
        );
    }

    #[test]
    fn zonal_j2_j6_orbit_conserves_energy_and_perturbs_the_j2_orbit() {
        // The full J2..J6 zonal field is conservative and time-independent, so the TOTAL
        // energy — kinetic plus the central and zonal potential, v²/2 − μ/r − R(r) — is
        // conserved over a day (the bare two-body ε = v²/2 − μ/r is NOT, since the
        // perturbation does work on it). And the J3..J6 terms must actually move the
        // trajectory away from the J2-only orbit (regression against a silently-J2-only force
        // model) while staying a small correction (not a blow-up).
        use crate::forces::zonal_potential;
        let total =
            |r: Vec3, v: Vec3| specific_energy(r, v) - zonal_potential(r, &EARTH_ZONALS_J2_J6);
        let (r0, v0) = leo_state();
        let day = 86_400.0;
        let tol = Tolerance {
            rtol: 1e-12,
            atol: 1e-9,
            ..Tolerance::default()
        };
        let e0 = total(r0, v0);
        let (r_day, v_day) = propagate(r0, v0, day, ForceModel::with_zonals_j2_j6(), &tol);
        let e1 = total(r_day, v_day);
        assert!(
            (e1 - e0).abs() / e0.abs() < 1e-8,
            "zonal field is conservative; total-energy drift {}",
            e1 - e0
        );

        // Over one orbital period the J2..J6 orbit must differ from the J2-only orbit.
        let period = 5400.0;
        let (r_zon, _) = propagate(r0, v0, period, ForceModel::with_zonals_j2_j6(), &tol);
        let (r_j2, _) = propagate(r0, v0, period, ForceModel::with_j2(), &tol);
        let sep = norm([r_zon[0] - r_j2[0], r_zon[1] - r_j2[1], r_zon[2] - r_j2[2]]);
        assert!(
            sep > 1e-3,
            "J3..J6 must perturb the orbit, separation {sep} m"
        );
        assert!(
            sep < 5.0e4,
            "J3..J6 must stay a small correction, separation {sep} m"
        );
    }
}
