kshana 0.24.0

// SPDX-License-Identifier: AGPL-3.0-only
//! Independent SRIF cross-validation of the cislunar observability transition (paper P6, L32).
//!
//! The observability core [`crate::observability_gramian`] reads *how much* of a
//! spacecraft's planar four-state an inter-satellite-ranging arc constrains from the **rank**
//! of the stacked observability matrix `O = stack_k[H_k·Φ_k]` and the **conditioning** of its
//! Gram matrix, both via the crate's Jacobi eigensolver. That is one estimator's verdict. This
//! module cross-checks that verdict against a *second, independent* estimator — the crate's
//! **Square-Root Information Filter** [`crate::deepspace_od::Srif`], which never forms `OᵀO`:
//! it folds the observability rows one at a time through **Householder triangularization** into
//! an upper-triangular information square root `R`, and recovers the posterior covariance as the
//! Gram matrix `P = R⁻¹R⁻ᵀ`.
//!
//! Folding the `k`-th observability row `H_k·Φ_k` (the partial of the `k`-th range with respect
//! to the *initial* planar state, by the chain rule through the variational STM `Φ_k`) into a
//! fully-diffuse SRIF with unit weight makes `RᵀR = Σ_k (H_kΦ_k)ᵀ(H_kΦ_k) = OᵀO` — the same
//! information content the observability Gramian carries, reached by a completely different
//! numerical machine. The cross-validation is then sharp:
//!
//! * the SRIF **posterior covariance becomes finite / well-conditioned exactly when the
//!   observable rank reaches the full four-state** — below full rank `R` has a zero pivot, so
//!   `P` is singular (infinite condition); at full rank `P = (OᵀO)⁻¹` is finite; and
//! * the SRIF posterior-covariance **condition number tracks the Gramian conditioning** as the
//!   arc grows — `cond(P) = cond((OᵀO)⁻¹) = cond(OᵀO)`, so the two independent estimators agree
//!   on the geometry's conditioning to numerical precision.
//!
//! This upgrades P6's rank-transition claim from *rank-only* to *Validated against an
//! independent estimator*: the SRIF, which shares no code with the eigen-Gramian beyond the
//! measurement partials, confirms the same transition arc and the same conditioning.
//!
//! ## Validated vs Modelled
//! * **Validated.** The rank at which the SRIF posterior covariance turns finite equals the
//!   observability-matrix rank transition, and its condition number equals the observability-Gram
//!   condition number — two independent routes (Householder square-root information filtering vs
//!   the Jacobi eigen-Gramian) agreeing on the same subspace and conditioning (the module's unit
//!   tests assert both). Thresholds are consistent (a relative singular-value floor, applied as
//!   its square on the eigenvalue side), so the two rank reads cannot silently disagree.
//! * **Modelled.** The tracking geometry (which spacecraft, links, arc, epoch grid) is the same
//!   Modelled scenario input as the rest of P6; the *specific* transition arc is a property of
//!   that geometry, not a certified universal.

use crate::deepspace_od::Srif;
use crate::fim::{information_matrix, sym_eig};
use crate::observability_gramian::{
    observability_matrix, singular_values, Mat, ObsEpoch, N_PLANAR,
};

/// One arc point of the SRIF ↔ observability-Gramian cross-validation: the observable rank and
/// conditioning read two independent ways over the tracking arc truncated at `epoch_index`.
#[derive(Clone, Debug)]
pub struct SrifArcPoint {
    /// Index of the last epoch in this growing prefix.
    pub epoch_index: usize,
    /// Elapsed arc time from the first epoch (normalised rotating-frame time units).
    pub arc_time: f64,
    /// Stacked observability rows accumulated up to and including this epoch.
    pub n_rows: usize,
    /// Observable rank from the observability matrix `O` (singular-value threshold) — the same
    /// rank the P6 `rank-vs-arc` table reports.
    pub gramian_rank: usize,
    /// Full-space condition number of the observability Gram `OᵀO` (`+∞` below full rank): the
    /// eigen-Gramian estimator's conditioning verdict.
    pub gramian_condition: f64,
    /// Whether the SRIF posterior covariance `P = R⁻¹R⁻ᵀ` is finite **and** well-conditioned
    /// (full rank): `true` exactly when the observable rank reaches the full four-state.
    pub srif_posterior_wellposed: bool,
    /// SRIF posterior-covariance condition number (`+∞` when rank-deficient / singular). At full
    /// rank this equals [`Self::gramian_condition`] to numerical precision.
    pub srif_condition: f64,
}

/// Full-space condition number `λ_max/λ_min` of a symmetric PSD spectrum (ascending), with a
/// relative floor `floor·λ_max`: a smallest eigenvalue at or below the floor means the matrix is
/// singular on the full space, so the condition is `+∞`.
fn floored_condition(eigs_ascending: &[f64], floor: f64) -> f64 {
    let lmax = eigs_ascending.last().copied().unwrap_or(0.0);
    let lmin = eigs_ascending.first().copied().unwrap_or(0.0);
    if lmax <= 0.0 || lmin <= floor * lmax {
        f64::INFINITY
    } else {
        lmax / lmin
    }
}

/// Fold the stacked observability rows into a fully-diffuse SRIF over the initial planar state
/// with **unit weight**, so the resulting information square root satisfies `RᵀR = OᵀO`. The
/// measurement residual is irrelevant to the covariance, so a zero residual is folded on each
/// row; `σ = 1` gives the unit-weight Gram. `R` is reached by Householder triangularization —
/// no normal matrix is ever formed.
fn srif_over_arc(o: &Mat, n: usize) -> Srif {
    let mut srif = Srif::new(n);
    for row in o {
        srif.measurement_update(row, 0.0, 1.0);
    }
    srif
}

/// The SRIF ↔ observability-Gramian cross-validation over growing prefixes of the epoch
/// sequence. For each prefix the observable rank and conditioning are read from the observability
/// matrix (singular-value threshold, the eigen-Gramian route) and, **independently**, from a
/// diffuse SRIF folded with the same observability rows (Householder square-root information
/// filtering). `rel_tol` is the relative singular-value floor (applied as its square on the
/// eigenvalue side, so the two rank reads use a consistent threshold).
pub fn srif_cross_validation(epochs: &[ObsEpoch], rel_tol: f64) -> Vec<SrifArcPoint> {
    let floor = rel_tol * rel_tol; // singular-value floor rel_tol ⇒ eigenvalue floor rel_tol²
    let mut out = Vec::with_capacity(epochs.len());
    let mut arc = 0.0;
    for (k, ep) in epochs.iter().enumerate() {
        arc += ep.dt;
        let sub = &epochs[..=k];
        let (o, _w) = observability_matrix(sub);

        // Eigen-Gramian estimator: rank via singular-value threshold, condition of OᵀO.
        let sv = singular_values(&o);
        let smax = sv.first().copied().unwrap_or(0.0);
        let gramian_rank = if smax > 0.0 {
            sv.iter().filter(|&&s| s > rel_tol * smax).count()
        } else {
            0
        };
        let ones = vec![1.0; o.len()];
        let gram = information_matrix(&o, &ones);
        let gram_eig = sym_eig(&gram);
        let gramian_condition = if gramian_rank == N_PLANAR {
            floored_condition(&gram_eig.values, floor)
        } else {
            f64::INFINITY
        };

        // Independent SRIF estimator: posterior covariance finiteness + condition.
        let srif = srif_over_arc(&o, N_PLANAR);
        let (_x, p) = srif.solve();
        let p_finite = p.iter().flatten().all(|v| v.is_finite());
        let p_eig = sym_eig(&p);
        let srif_condition = if p_finite {
            floored_condition(&p_eig.values, floor)
        } else {
            f64::INFINITY
        };
        let srif_posterior_wellposed = srif_condition.is_finite();

        out.push(SrifArcPoint {
            epoch_index: k,
            arc_time: arc,
            n_rows: o.len(),
            gramian_rank,
            gramian_condition,
            srif_posterior_wellposed,
            srif_condition,
        });
    }
    out
}

/// The first arc index at which the observable rank reaches the full four-state, or `None` if it
/// never does over the arc — the P6 transition the SRIF is cross-checked against.
pub fn full_rank_transition(points: &[SrifArcPoint]) -> Option<usize> {
    points
        .iter()
        .find(|p| p.gramian_rank == N_PLANAR)
        .map(|p| p.epoch_index)
}

/// The first arc index at which the independent SRIF posterior covariance turns finite /
/// well-conditioned, or `None`.
pub fn srif_finite_transition(points: &[SrifArcPoint]) -> Option<usize> {
    points
        .iter()
        .find(|p| p.srif_posterior_wellposed)
        .map(|p| p.epoch_index)
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::cr3bp::EARTH_MOON_MU;
    use crate::intersat_range::{range_row, PlanarState};
    use crate::observability_gramian::{planar_propagate, planar_state_stm};

    /// A single-link range-only arc (chief ↔ reference): each epoch carries the chief STM and
    /// one range row — the P6 arc that grows from rank-1 toward the full four-state.
    fn single_link_arc(n_epochs: usize, arc: f64) -> Vec<ObsEpoch> {
        let chief: PlanarState = [1.10, 0.02, 0.05, -0.50];
        let reference: PlanarState = [1.02, -0.03, -0.06, -0.55];
        let mu = EARTH_MOON_MU;
        let mut epochs = Vec::with_capacity(n_epochs);
        let mut prev = 0.0;
        for k in 0..n_epochs {
            let t = arc * (k as f64) / ((n_epochs - 1) as f64);
            let (cs, phi) = planar_state_stm(&chief, mu, t, 3000);
            let rs = planar_propagate(&reference, mu, t, 3000);
            let (_rho, r_row) = range_row(&cs, &rs);
            epochs.push(ObsEpoch {
                h: vec![r_row.to_vec()],
                phi: phi.iter().map(|r| r.to_vec()).collect(),
                dt: t - prev,
            });
            prev = t;
        }
        epochs
    }

    // ── ORACLE (Validated): the independent SRIF agrees with the rank transition ──
    /// (a) at a short arc the SRIF information is singular (posterior covariance non-finite /
    /// infinite condition); (b) at the arc where the observable rank reaches four the SRIF
    /// posterior covariance is finite and its condition number is finite and the *same order* as
    /// the observability-Gram condition number.
    #[test]
    fn srif_posterior_finite_exactly_at_full_rank() {
        let rel_tol = 1e-6;
        let epochs = single_link_arc(24, 0.06);
        let points = srif_cross_validation(&epochs, rel_tol);

        // (a) A short arc (first two epochs) is rank-deficient: the SRIF posterior is singular.
        let short = &points[1];
        assert!(
            short.gramian_rank < N_PLANAR,
            "short arc should be rank-deficient, got rank {}",
            short.gramian_rank
        );
        assert!(
            !short.srif_posterior_wellposed && short.srif_condition.is_infinite(),
            "short-arc SRIF posterior must be singular (cond {})",
            short.srif_condition
        );

        // (b) By the end of the arc the four-state is fully observable and the SRIF posterior is
        // finite, well-conditioned, and the same order as the Gram condition.
        let full = points.last().unwrap();
        assert_eq!(full.gramian_rank, N_PLANAR, "full observability over arc");
        assert!(
            full.srif_posterior_wellposed && full.srif_condition.is_finite(),
            "full-rank SRIF posterior must be finite (cond {})",
            full.srif_condition
        );
        assert!(
            full.gramian_condition.is_finite(),
            "full-rank Gram condition must be finite"
        );
        // cond(P) = cond((OᵀO)⁻¹) = cond(OᵀO): the two independent estimators agree to well
        // within an order of magnitude (in practice to a few ULP of the eigensolvers).
        let ratio = full.srif_condition / full.gramian_condition;
        assert!(
            (0.1..=10.0).contains(&ratio),
            "SRIF condition {:.3e} not the same order as Gram condition {:.3e} (ratio {ratio:.3e})",
            full.srif_condition,
            full.gramian_condition
        );

        // The independent transition arcs coincide: the SRIF posterior turns finite exactly at
        // the rank-4 arc — the cross-validation's core claim. The wellposedness floor is the
        // singular-value floor transported to the covariance spectrum, so the two transitions are
        // threshold-consistent, not coincidentally aligned.
        assert_eq!(
            full_rank_transition(&points),
            srif_finite_transition(&points),
            "SRIF posterior-finite transition must equal the observable rank-4 transition"
        );
    }

    /// The posterior condition number is monotone-ish informative: once the SRIF is well-posed it
    /// stays well-posed as the arc only adds information (rank is non-decreasing).
    #[test]
    fn wellposed_is_persistent_along_the_arc() {
        let points = srif_cross_validation(&single_link_arc(24, 0.06), 1e-6);
        let mut seen_wellposed = false;
        for p in &points {
            if p.srif_posterior_wellposed {
                seen_wellposed = true;
            }
            if seen_wellposed {
                assert!(
                    p.srif_posterior_wellposed,
                    "SRIF lost well-posedness at epoch {} after gaining it",
                    p.epoch_index
                );
            }
        }
        assert!(
            seen_wellposed,
            "the arc should reach a well-posed posterior"
        );
    }
}