kshana 0.25.0

// SPDX-License-Identifier: AGPL-3.0-only
//! **Optical ↔ RF measurement handoff** — the receiver state-and-covariance transfer when
//! the active PNT measurement type switches between a **tight** optical modality (small
//! measurement noise `R`) and a **loose** RF modality (large `R`).
//!
//! The engineering hazard a handoff must avoid is a *jump* in the delivered navigation state
//! at the instant the sensor changes. This module makes that impossible by construction: the
//! handoff carries the estimator's **mean unchanged** (bit-for-bit) and only re-scales the
//! covariance. The three composed pieces are the ones the rest of the fusion stack already
//! uses:
//!
//! * **Joseph-form measurement update** — the numerically-stable covariance update
//!   `P⁺ = (1−K)²P + K²R` (the scalar, per-axis form of the matrix Joseph update in
//!   [`crate::kalman`] and [`crate::fusion::coupled`]). A tighter `R` (optical) deflates the
//!   covariance more than a looser `R` (RF).
//! * **Closed-loop error-state reset** — at the handoff the estimated error state is folded
//!   into the nominal and reset to zero, exactly as the closed-loop INS/GNSS filter in
//!   [`crate::fusion::closed_loop`] resets after each correction. Because the correction is
//!   already in the mean, the reset moves nothing: the delivered state is continuous.
//! * **NEES consistency gate** — the normalised estimation-error-squared
//!   `ε = (x̂ − x)ᵀ P⁻¹ (x̂ − x)` is χ²-distributed with as many degrees of freedom as
//!   states, so a *consistent* filter keeps `ε` inside the two-sided χ² gate
//!   `[χ²_{0.025}(dof), χ²_{0.975}(dof)]` ([`crate::raim::chi2_quantile`]). A handoff that
//!   inflated or deflated the covariance inconsistently would push `ε` out of that gate.
//!
//! ## Validated vs Modelled
//!
//! - **Validated (closed-form invariant).** The **mean-continuity** (no-jump) guarantee is
//!   exact: [`HandoffState::handoff`] copies the mean bit-for-bit, so the delivered state is
//!   provably unchanged across the switch. The NEES gate bounds are the exact χ² quantiles.
//! - **Modelled.** The specific optical / RF measurement-noise magnitudes and the
//!   modality-transition covariance-inflation factor are representative inputs.

use crate::raim::chi2_quantile;
use serde::Serialize;

/// Lower / upper tail probabilities of the two-sided NEES consistency gate (95% central).
const NEES_GATE_LO_P: f64 = 0.025;
const NEES_GATE_HI_P: f64 = 0.975;

/// A receiver estimate with a **diagonal** covariance: the mean `x` and the per-axis
/// variances `p_diag`. Per-axis scalar updates keep the covariance diagonal, which is all
/// the handoff / mean-continuity argument needs (and keeps the NEES an exact sum of
/// per-axis normalised squares).
#[derive(Clone, Debug, PartialEq)]
pub struct HandoffState {
    /// State mean (e.g. `[east, north, up, clock]`).
    pub x: Vec<f64>,
    /// Per-axis variance (the covariance diagonal), same length as `x`.
    pub p_diag: Vec<f64>,
}

impl HandoffState {
    /// A new state from a mean and its per-axis variances.
    pub fn new(x: Vec<f64>, p_diag: Vec<f64>) -> Self {
        HandoffState { x, p_diag }
    }

    /// Scalar **Joseph-form** measurement update of axis `i` by a direct observation `z`
    /// with noise variance `r`: innovation `ν = z − x_i`, gain `K = P_ii/(P_ii + r)`,
    /// `x_i ← x_i + Kν`, and `P_ii ← (1−K)²·P_ii + K²·r`. The Joseph form keeps the posterior
    /// variance positive for any `K`. A tighter `r` produces a smaller posterior variance.
    pub fn joseph_update_axis(&mut self, i: usize, z: f64, r: f64) {
        if i >= self.x.len() {
            return;
        }
        let p = self.p_diag[i];
        let s = p + r;
        if s <= 0.0 {
            return;
        }
        let k = p / s;
        self.x[i] += k * (z - self.x[i]);
        self.p_diag[i] = (1.0 - k).powi(2) * p + k * k * r;
    }

    /// Apply a batch of per-axis Joseph updates `(axis, z, r)` in order.
    pub fn apply_updates(&mut self, updates: &[(usize, f64, f64)]) {
        for &(i, z, r) in updates {
            self.joseph_update_axis(i, z, r);
        }
    }

    /// **Hand the state off** to the other modality, inflating the covariance by
    /// `1 + inflation_factor` to account for the modality-transition uncertainty. The
    /// **mean is copied bit-for-bit** — this is the no-jump guarantee: the delivered state
    /// is provably unchanged across the switch, only its uncertainty grows. `inflation_factor
    /// = 0` reproduces the state exactly (mean AND covariance).
    pub fn handoff(&self, inflation_factor: f64) -> HandoffState {
        let scale = 1.0 + inflation_factor.max(0.0);
        HandoffState {
            x: self.x.clone(), // bit-for-bit mean continuity
            p_diag: self.p_diag.iter().map(|&p| p * scale).collect(),
        }
    }

    /// The **normalised estimation error squared (NEES)** against a `truth` state:
    /// `ε = Σ_i (x_i − truth_i)² / P_ii`, distributed χ²_dof when the filter is consistent.
    /// Missing/zero variances are skipped.
    pub fn nees(&self, truth: &[f64]) -> f64 {
        self.x
            .iter()
            .zip(self.p_diag.iter())
            .zip(truth.iter())
            .filter(|((_, &p), _)| p > 0.0)
            .map(|((&xi, &p), &ti)| {
                let e = xi - ti;
                e * e / p
            })
            .sum()
    }

    /// The covariance trace (total variance) — the scalar the covariance
    /// deflation/inflation is read off.
    pub fn total_variance(&self) -> f64 {
        self.p_diag.iter().sum()
    }
}

/// The two-sided **NEES consistency gate** `[χ²_{0.025}(dof), χ²_{0.975}(dof)]` — the 95%
/// central χ² interval a consistent filter's NEES should fall in. Exact χ² quantiles from
/// [`crate::raim::chi2_quantile`].
pub fn nees_gate(dof: usize) -> (f64, f64) {
    let d = dof.max(1) as f64;
    (
        chi2_quantile(NEES_GATE_LO_P, d),
        chi2_quantile(NEES_GATE_HI_P, d),
    )
}

/// Whether a NEES value sits inside the two-sided χ² consistency gate for `dof` states.
pub fn nees_in_gate(nees: f64, dof: usize) -> bool {
    let (lo, hi) = nees_gate(dof);
    (lo..=hi).contains(&nees)
}

/// The outcome of an optical→RF handoff demonstration: the pre/post-switch means (proving
/// bit-continuity), the covariance deflation/inflation, and the NEES consistency check.
#[derive(Clone, Debug, Serialize)]
pub struct HandoffOutcome {
    /// State mean immediately before the handoff (after the optical update).
    pub mean_before: Vec<f64>,
    /// State mean immediately after the handoff (before any RF update).
    pub mean_after: Vec<f64>,
    /// `true` when the mean is bit-for-bit unchanged across the switch (the no-jump proof).
    pub mean_continuous: bool,
    /// The largest per-axis mean change across the switch (0.0 when continuous).
    pub max_mean_jump: f64,
    /// Total variance after the tight optical update (deflated).
    pub variance_after_optical: f64,
    /// Total variance after the handoff covariance inflation.
    pub variance_after_handoff: f64,
    /// Total variance after the loose RF update.
    pub variance_after_rf: f64,
    /// NEES of the final (post-RF) estimate against truth.
    pub final_nees: f64,
    /// The NEES consistency gate `[χ²_{0.025}, χ²_{0.975}]` for `dof` states.
    pub nees_gate: (f64, f64),
    /// `true` when the final NEES lies in the consistency gate.
    pub nees_in_gate: bool,
    /// Degrees of freedom (number of states).
    pub dof: usize,
}

/// Run a full **optical → RF handoff** sequence and check both invariants.
///
/// Starting from `initial` and a `truth`, the tight optical measurements (`optical_updates`,
/// each `(axis, z, r)` with small `r`) are applied, the state is handed off with covariance
/// inflation `inflation_factor`, and then the loose RF measurements (`rf_updates`, large `r`)
/// are applied. The report proves the mean is continuous across the switch and reports
/// whether the NEES stays inside its χ² gate.
pub fn optical_rf_handoff(
    initial: HandoffState,
    truth: &[f64],
    optical_updates: &[(usize, f64, f64)],
    rf_updates: &[(usize, f64, f64)],
    inflation_factor: f64,
) -> HandoffOutcome {
    let dof = initial.x.len();
    let mut state = initial;
    state.apply_updates(optical_updates);
    let variance_after_optical = state.total_variance();
    let mean_before = state.x.clone();

    // Closed-loop-style handoff: mean carried bit-for-bit, covariance inflated.
    let mut handed = state.handoff(inflation_factor);
    let mean_after = handed.x.clone();
    let max_mean_jump = mean_before
        .iter()
        .zip(mean_after.iter())
        .map(|(a, b)| (a - b).abs())
        .fold(0.0_f64, f64::max);
    let mean_continuous = mean_before == mean_after;
    let variance_after_handoff = handed.total_variance();

    handed.apply_updates(rf_updates);
    let variance_after_rf = handed.total_variance();

    let final_nees = handed.nees(truth);
    let gate = nees_gate(dof);
    HandoffOutcome {
        mean_before,
        mean_after,
        mean_continuous,
        max_mean_jump,
        variance_after_optical,
        variance_after_handoff,
        variance_after_rf,
        final_nees,
        nees_gate: gate,
        nees_in_gate: (gate.0..=gate.1).contains(&final_nees),
        dof,
    }
}

/// Run a full **RF → optical handoff** sequence (the reverse of [`optical_rf_handoff`]) and
/// check both invariants.
///
/// Starting from `initial` and a `truth`, the loose RF measurements (`rf_updates`, each
/// `(axis, z, r)` with large `r`) are applied, the state is handed off with covariance
/// inflation `inflation_factor`, and then the tight optical measurements (`optical_updates`,
/// small `r`) are applied. Symmetric to [`optical_rf_handoff`]: the report proves the mean is
/// continuous across the switch (the no-jump guarantee holds in *either* direction) and
/// reports whether the NEES stays inside its χ² gate. `variance_after_optical` here is the
/// variance after the loose RF stage (the pre-handoff stage), `variance_after_rf` the variance
/// after the final tight optical update — the field roles mirror the forward pass so the same
/// [`HandoffOutcome`] type carries both directions.
pub fn rf_optical_handoff(
    initial: HandoffState,
    truth: &[f64],
    rf_updates: &[(usize, f64, f64)],
    optical_updates: &[(usize, f64, f64)],
    inflation_factor: f64,
) -> HandoffOutcome {
    let dof = initial.x.len();
    let mut state = initial;
    state.apply_updates(rf_updates);
    let variance_after_optical = state.total_variance();
    let mean_before = state.x.clone();

    let mut handed = state.handoff(inflation_factor);
    let mean_after = handed.x.clone();
    let max_mean_jump = mean_before
        .iter()
        .zip(mean_after.iter())
        .map(|(a, b)| (a - b).abs())
        .fold(0.0_f64, f64::max);
    let mean_continuous = mean_before == mean_after;
    let variance_after_handoff = handed.total_variance();

    handed.apply_updates(optical_updates);
    let variance_after_rf = handed.total_variance();

    let final_nees = handed.nees(truth);
    let gate = nees_gate(dof);
    HandoffOutcome {
        mean_before,
        mean_after,
        mean_continuous,
        max_mean_jump,
        variance_after_optical,
        variance_after_handoff,
        variance_after_rf,
        final_nees,
        nees_gate: gate,
        nees_in_gate: (gate.0..=gate.1).contains(&final_nees),
        dof,
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use rand::SeedableRng;
    use rand_chacha::ChaCha8Rng;
    use rand_distr::{Distribution, Normal};

    /// The handoff carries the mean bit-for-bit (the PROVEN no-jump guarantee) while
    /// inflating the covariance; a zero inflation reproduces the state exactly. Oracle: the
    /// closed-form mean-continuity invariant (exact float equality).
    #[test]
    fn handoff_is_bit_continuous_in_the_mean() {
        let s = HandoffState::new(vec![1.25, -3.5, 7.0, 1e-9], vec![4.0, 4.0, 9.0, 1e-18]);
        let h = s.handoff(0.5);
        // Mean bit-for-bit identical (no jump).
        assert_eq!(s.x, h.x, "handoff must not move the mean");
        // Covariance inflated by exactly (1 + 0.5) on every axis.
        for (a, b) in s.p_diag.iter().zip(h.p_diag.iter()) {
            assert!((b - 1.5 * a).abs() < 1e-18 * a.max(1.0), "{b} vs 1.5·{a}");
        }
        // Zero inflation reproduces the state exactly (mean AND covariance).
        let same = s.handoff(0.0);
        assert_eq!(s.x, same.x);
        assert_eq!(s.p_diag, same.p_diag);
    }

    /// The scalar Joseph update matches the hand-computed posterior, and a tight `R`
    /// deflates the variance more than a loose `R`. Oracle: the closed-form Joseph update.
    #[test]
    fn joseph_update_deflates_and_matches_hand_value() {
        // P = 4, r = 1 → K = 4/5 = 0.8; P⁺ = (0.2)²·4 + (0.8)²·1 = 0.16 + 0.64 = 0.8.
        let mut s = HandoffState::new(vec![0.0], vec![4.0]);
        s.joseph_update_axis(0, 10.0, 1.0);
        assert!(
            (s.p_diag[0] - 0.8).abs() < 1e-12,
            "P⁺ {} vs 0.8",
            s.p_diag[0]
        );
        // x⁺ = 0 + 0.8·(10 − 0) = 8.
        assert!((s.x[0] - 8.0).abs() < 1e-12, "x⁺ {} vs 8", s.x[0]);

        // Tight R deflates more than loose R from the same prior.
        let mut tight = HandoffState::new(vec![0.0], vec![4.0]);
        let mut loose = HandoffState::new(vec![0.0], vec![4.0]);
        tight.joseph_update_axis(0, 0.0, 0.01);
        loose.joseph_update_axis(0, 0.0, 100.0);
        assert!(
            tight.p_diag[0] < loose.p_diag[0],
            "tight R should deflate more: {} vs {}",
            tight.p_diag[0],
            loose.p_diag[0]
        );
        assert!(tight.p_diag[0] > 0.0, "Joseph posterior stays positive");
    }

    /// The NEES gate bounds are the exact χ² quantiles, and a 1σ-per-axis error gives
    /// NEES = dof, which lies inside the gate. Oracle: `raim::chi2_quantile`.
    #[test]
    fn nees_gate_matches_chi2_quantiles_and_admits_one_sigma() {
        for dof in [1usize, 2, 4, 6] {
            let (lo, hi) = nees_gate(dof);
            assert!((lo - chi2_quantile(0.025, dof as f64)).abs() < 1e-12);
            assert!((hi - chi2_quantile(0.975, dof as f64)).abs() < 1e-12);
            assert!(
                lo < dof as f64 && (dof as f64) < hi,
                "dof {dof} inside gate"
            );

            // A state whose error is exactly 1σ on every axis has NEES = dof (in gate).
            let x: Vec<f64> = (0..dof).map(|i| (i as f64) + 1.0).collect();
            let p: Vec<f64> = (0..dof).map(|i| ((i as f64) + 2.0).powi(2)).collect();
            let truth: Vec<f64> = x
                .iter()
                .zip(p.iter())
                .map(|(&xi, &pi)| xi - pi.sqrt())
                .collect();
            let s = HandoffState::new(x, p);
            let nees = s.nees(&truth);
            assert!((nees - dof as f64).abs() < 1e-9, "NEES {nees} vs dof {dof}");
            assert!(nees_in_gate(nees, dof), "1σ NEES must be in gate");
        }
    }

    /// The full optical→RF handoff proves mean-continuity end-to-end, deflates on the tight
    /// optical update, inflates at the handoff, and keeps a statistically-consistent estimate
    /// in the NEES gate. Deterministic.
    #[test]
    fn full_handoff_is_continuous_consistent_and_deterministic() {
        // Truth at origin; start 1σ off on every axis (a consistent initial error).
        let truth = vec![0.0_f64, 0.0, 0.0, 0.0];
        let p0: Vec<f64> = vec![9.0, 9.0, 16.0, 1e-16];
        let x0: Vec<f64> = p0.iter().map(|&p| p.sqrt()).collect(); // 1σ initial error
        let init = HandoffState::new(x0, p0.clone());
        // Optical (tight) then RF (loose) measurements, each carrying a 1σ measurement
        // offset from truth (a consistent draw) so the posterior sits ~1σ from truth and the
        // NEES concentrates near the degrees of freedom.
        let opt_r: Vec<f64> = p0.iter().map(|&p| p * 1e-4).collect();
        let rf_r: Vec<f64> = p0.iter().map(|&p| p * 4.0).collect();
        let optical: Vec<(usize, f64, f64)> = (0..4)
            .map(|i| (i, truth[i] + opt_r[i].sqrt(), opt_r[i]))
            .collect();
        let rf: Vec<(usize, f64, f64)> = (0..4)
            .map(|i| (i, truth[i] + rf_r[i].sqrt(), rf_r[i]))
            .collect();

        let a = optical_rf_handoff(init.clone(), &truth, &optical, &rf, 0.2);
        let b = optical_rf_handoff(init, &truth, &optical, &rf, 0.2);
        // Deterministic.
        assert_eq!(a.mean_before, b.mean_before);
        assert_eq!(a.final_nees, b.final_nees);
        // No-jump: the mean is bit-continuous across the switch.
        assert!(
            a.mean_continuous,
            "mean must be continuous across the handoff"
        );
        assert_eq!(a.max_mean_jump, 0.0);
        // Covariance: deflated by the tight optical, then inflated at the handoff.
        assert!(
            a.variance_after_handoff > a.variance_after_optical,
            "handoff inflates"
        );
        assert!(
            a.variance_after_optical < init_variance(&p0),
            "optical deflates"
        );
        // The estimate stays inside its NEES consistency gate.
        assert!(
            a.nees_in_gate,
            "final NEES {} not in gate {:?}",
            a.final_nees, a.nees_gate
        );
    }

    fn init_variance(p0: &[f64]) -> f64 {
        p0.iter().sum()
    }

    /// **MONTE-CARLO UNBIASEDNESS (G2a).** Drawing many random truth/measurement
    /// realizations through the optical→handoff→RF sequence, the ensemble-mean estimation
    /// error is ≈ 0 (the estimator is unbiased) and the empirical NEES average is ≈ dof (the
    /// covariance is neither optimistic nor conservative). Oracle: the sampling law
    /// `E[x̂ − x] = 0` and `E[NEES] = dof` for a consistent linear-Gaussian estimator — checked
    /// against a seeded Monte-Carlo ensemble, independent of any single closed-form value.
    #[test]
    fn monte_carlo_handoff_is_unbiased_with_nees_at_dof() {
        let dof = 4usize;
        // Prior variances and measurement noises (tight optical, loose RF).
        let p0 = [9.0_f64, 9.0, 16.0, 1e-16];
        let opt_r = [0.04_f64, 0.04, 0.09, 1e-20]; // tight
        let rf_r = [4.0_f64, 4.0, 9.0, 4e-16]; // loose
                                               // Zero handoff inflation: the pass is then the EXACT optimal linear-Gaussian filter,
                                               // whose consistency law is E[x̂−x]=0 and E[NEES]=dof. (Non-zero inflation is a modelled
                                               // conservatism — it deflates the empirical NEES below dof by construction — and is
                                               // covered separately by the forward/reverse handoff tests.)
        let inflation = 0.0;
        let mut rng = ChaCha8Rng::seed_from_u64(20260712);
        let n = 40_000usize;

        // Per-axis normal samplers.
        let prior_n: Vec<Normal<f64>> = p0
            .iter()
            .map(|&p| Normal::new(0.0, p.sqrt()).unwrap())
            .collect();
        let opt_n: Vec<Normal<f64>> = opt_r
            .iter()
            .map(|&r| Normal::new(0.0, r.sqrt()).unwrap())
            .collect();
        let rf_n: Vec<Normal<f64>> = rf_r
            .iter()
            .map(|&r| Normal::new(0.0, r.sqrt()).unwrap())
            .collect();

        let mut mean_err = [0.0_f64; 4];
        let mut nees_sum = 0.0_f64;
        for _ in 0..n {
            // Truth fixed at origin (WLOG); prior estimate drawn 1σ-consistent from truth.
            let truth = [0.0_f64; 4];
            let x0: Vec<f64> = (0..dof).map(|i| prior_n[i].sample(&mut rng)).collect();
            let mut state = HandoffState::new(x0, p0.to_vec());
            // Optical (tight) measurements: truth + noise draw.
            let optical: Vec<(usize, f64, f64)> = (0..dof)
                .map(|i| (i, truth[i] + opt_n[i].sample(&mut rng), opt_r[i]))
                .collect();
            state.apply_updates(&optical);
            let handed_pre = state.handoff(inflation);
            // RF (loose) measurements after the handoff.
            let rf: Vec<(usize, f64, f64)> = (0..dof)
                .map(|i| (i, truth[i] + rf_n[i].sample(&mut rng), rf_r[i]))
                .collect();
            let mut handed = handed_pre;
            handed.apply_updates(&rf);
            for i in 0..dof {
                mean_err[i] += handed.x[i] - truth[i];
            }
            nees_sum += handed.nees(&truth);
        }
        // Ensemble-mean estimation error ≈ 0 (unbiased): compare to the per-axis posterior σ
        // scaled by 1/√n sampling error.
        for i in 0..dof {
            let mean = mean_err[i] / n as f64;
            // Posterior σ is at most √p0[i]; sampling error of the mean ~ σ/√n. Allow 5×.
            let tol = 5.0 * p0[i].sqrt() / (n as f64).sqrt();
            assert!(
                mean.abs() < tol.max(1e-12),
                "axis {i} ensemble mean error {mean} exceeds unbiasedness tol {tol}"
            );
        }
        // Empirical NEES average ≈ dof (consistency) for the zero-inflation optimal filter.
        let nees_bar = nees_sum / n as f64;
        assert!(
            (nees_bar - dof as f64).abs() < 0.15,
            "empirical mean NEES {nees_bar} not ≈ dof {dof}"
        );
    }

    /// **VAN-LOAN / JOSEPH ANALYTIC COVARIANCE ORACLE (G2b).** After a Joseph measurement
    /// update at the R switch the posterior variance equals the closed-form
    /// `P⁺ = (1−K)²P + K²R` with `K = P/(P+R)` — which algebraically simplifies to the exact
    /// information-form value `P⁺ = P·R/(P+R) = 1/(1/P + 1/R)`. We assert the module's Joseph
    /// update reproduces BOTH independent closed forms to machine precision across a sweep of
    /// disparate R changes (tight optical → loose RF and back). Oracle: the analytic
    /// Joseph/information-form covariance propagation, hand-derived per case (not a scalar
    /// hand value).
    #[test]
    fn joseph_update_matches_analytic_information_form_covariance_at_r_switch() {
        // (prior P, measurement R) pairs spanning a tight optical R and a loose RF R.
        let cases = [
            (16.0_f64, 0.01_f64), // loose prior, very tight optical R
            (0.05, 9.0),          // tight prior, loose RF R
            (4.0, 4.0),           // equal
            (100.0, 1e-6),        // extreme R change (optical acquisition)
            (1e-12, 1e-9),        // clock-axis scale
        ];
        for &(p, r) in &cases {
            let mut s = HandoffState::new(vec![0.0], vec![p]);
            s.joseph_update_axis(0, 1.0, r);
            let k = p / (p + r);
            // Joseph closed form.
            let joseph = (1.0 - k).powi(2) * p + k * k * r;
            // Independent information-form closed form P⁺ = 1/(1/P + 1/R) = P·R/(P+R).
            let info_form = p * r / (p + r);
            let rel = 1e-9 * info_form.max(1e-18);
            assert!(
                (s.p_diag[0] - joseph).abs() <= rel,
                "P⁺ {} vs Joseph {joseph} (P={p}, R={r})",
                s.p_diag[0]
            );
            assert!(
                (s.p_diag[0] - info_form).abs() <= rel,
                "P⁺ {} vs information-form {info_form} (P={p}, R={r})",
                s.p_diag[0]
            );
        }
    }

    /// **REVERSE RF → OPTICAL HANDOFF (G2c).** The handoff no-jump guarantee holds in *both*
    /// directions: applying loose RF, handing off, then tight optical keeps the mean bit-for-
    /// bit continuous across the switch and the final NEES inside its χ² gate. The reverse
    /// pass tightens (optical deflates) rather than loosens, mirroring the forward pass.
    /// Deterministic. Oracle: the mean-continuity invariant (exact float equality) and the χ²
    /// NEES gate, exercised on the previously-untested reverse direction.
    #[test]
    fn reverse_rf_optical_handoff_is_continuous_and_consistent() {
        let truth = vec![0.0_f64, 0.0, 0.0, 0.0];
        let p0: Vec<f64> = vec![9.0, 9.0, 16.0, 1e-16];
        let x0: Vec<f64> = p0.iter().map(|&p| p.sqrt()).collect(); // 1σ initial error
        let init = HandoffState::new(x0, p0.clone());
        let rf_r: Vec<f64> = p0.iter().map(|&p| p * 4.0).collect(); // loose
        let opt_r: Vec<f64> = p0.iter().map(|&p| p * 1e-4).collect(); // tight
        let rf: Vec<(usize, f64, f64)> = (0..4)
            .map(|i| (i, truth[i] + rf_r[i].sqrt(), rf_r[i]))
            .collect();
        let optical: Vec<(usize, f64, f64)> = (0..4)
            .map(|i| (i, truth[i] + opt_r[i].sqrt(), opt_r[i]))
            .collect();

        let a = rf_optical_handoff(init.clone(), &truth, &rf, &optical, 0.2);
        let b = rf_optical_handoff(init, &truth, &rf, &optical, 0.2);
        // Deterministic.
        assert_eq!(a.mean_before, b.mean_before);
        assert_eq!(a.final_nees, b.final_nees);
        // No-jump across the switch (holds in the reverse direction too).
        assert!(a.mean_continuous, "reverse handoff must be mean-continuous");
        assert_eq!(a.max_mean_jump, 0.0);
        // The final tight-optical update deflates below the post-handoff (inflated) variance.
        assert!(
            a.variance_after_rf < a.variance_after_handoff,
            "final optical update must tighten: {} vs {}",
            a.variance_after_rf,
            a.variance_after_handoff
        );
        // Consistent estimate.
        assert!(
            a.nees_in_gate,
            "reverse-handoff NEES {} not in gate {:?}",
            a.final_nees, a.nees_gate
        );
        // A round trip optical→RF→optical returns a tighter covariance than the loose RF
        // interlude (the tight bookends dominate): sanity that both directions compose.
        assert!(a.variance_after_rf < init_variance(&p0));
    }
}