kshana 0.34.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.
//!
//! ## Honest scope of the SRIF cross-check (how the STM enters)
//! The variational STM `Φ_k` is folded **analytically into the measurement rows** `H_k·Φ_k` by
//! the chain rule *before* they reach the filter — the SRIF's Bierman square-root
//! **time-update** ([`crate::deepspace_od::Srif::time_update`]) is deliberately **not** called
//! here. Each row is a partial with respect to the *initial* epoch's state, so the whole batch is a
//! single static-state estimation of `δs_0` and only `measurement_update` is needed. This is
//! therefore a **different-algorithm consistency check** (Householder square-root information
//! filtering vs the Jacobi eigen-Gramian), not a fully external oracle: both reduce to the same
//! `OᵀO`. The genuinely **external** leg that closes that independence gap is a NumPy/SciPy
//! **batch least-squares** on the identical stacked system `O` — its posterior covariance
//! `(OᵀO)⁻¹` and condition number are computed by LAPACK and cross-checked against this SRIF's
//! `P` at the rank-4 arc in `tests/cislunar_srif_batch_ls_reference.rs`. (The separate Bierman
//! `time_update`, propagating an SRIF between epochs through `Φ`, is exercised on its own in
//! [`crate::deepspace_od`]'s unit tests; it is not part of this static-state cross-check.)
//!
//! ## 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: every P6 rank read uses the one singular-value
//!   floor `σ > rel_tol·σ_max` (on the Gram `OᵀO`, whose eigenvalues are `σ²`, applied as
//!   `λ > rel_tol²·λ_max`), so the two rank reads cannot silently disagree — and both match
//!   `numpy.linalg.matrix_rank` on the same `O` (see `tests/cislunar_srif_batch_ls_reference.rs`).
//! * **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::{
    bounded_rank_from_singular_values, observability_matrix, singular_values, Mat, ObsEpoch,
    N_PLANAR,
};

/// The state dimension an epoch sequence estimates, read off the width of its variational
/// STM (4 for the planar `[x, y, ẋ, ẏ]` path, 6 for the spatial `[x, y, z, ẋ, ẏ, ż]` one).
/// Falls back to the planar dimension for an empty sequence so the planar contract is
/// unchanged.
fn state_dim_of(epochs: &[ObsEpoch]) -> usize {
    epochs.first().map(|e| e.phi.len()).unwrap_or(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,
    /// The state dimension this arc point is read against — 4 for the planar path, 6 for
    /// the spatial one. Full observability means `gramian_rank == state_dim`.
    pub state_dim: 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 state dimension.
    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 one relative singular-value floor (`σ > rel_tol·σ_max`) used by
/// every P6 rank read; on the eigenvalue side of the Gram `OᵀO` (whose eigenvalues are `σ²`) it is
/// applied as `λ > rel_tol²·λ_max`, so the rank read and the well-posedness/condition read use the
/// SAME effective floor and cannot disagree.
///
/// The state dimension is read off the width of the epochs' variational STM, so the SAME function
/// serves the planar four-state and the spatial six-state paths; the planar behaviour is unchanged.
pub fn srif_cross_validation(epochs: &[ObsEpoch], rel_tol: f64) -> Vec<SrifArcPoint> {
    cross_validation_with(epochs, rel_tol, false)
}

/// [`srif_cross_validation`] with the Gramian-side singular values read by the opt-in
/// square-root route: Householder QR of the observability matrix `O` and a one-sided Jacobi SVD of
/// its triangular factor ([`crate::linalg_sr`]), instead of the eigenvalues of the Gram `OᵀO`,
/// which square the condition number. Rank rule, floors and the SRIF side are unchanged; the
/// default [`srif_cross_validation`] is unchanged.
pub fn srif_cross_validation_sqrt(epochs: &[ObsEpoch], rel_tol: f64) -> Vec<SrifArcPoint> {
    cross_validation_with(epochs, rel_tol, true)
}

/// Singular values of `o` (descending, `n` of them, zeros where `o` has fewer rows) from the
/// triangular factor, never forming `oᵀo`.
fn sqrt_singular_values(o: &[Vec<f64>], n: usize) -> Vec<f64> {
    let r = crate::linalg_sr::householder_r(o, n);
    let (mut sv, _) = crate::linalg_sr::jacobi_svd(&r);
    sv.reverse();
    sv
}

fn cross_validation_with(epochs: &[ObsEpoch], rel_tol: f64, sqrt: bool) -> Vec<SrifArcPoint> {
    // The one convention on the eigenvalue side: σ-floor rel_tol·σ_max ⇔ λ-floor rel_tol²·λ_max
    // (the Gram OᵀO squares the singular values), matching `rank_from_singular_values`.
    let floor = rel_tol * rel_tol;
    let n_state = state_dim_of(epochs);
    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 the one observability rank convention
        // (σ > rel_tol·σ_max ⇔ λ > rel_tol²·λ_max on the Gram OᵀO — the SAME relative floor every
        // P6 rank read uses, and the SAME `rank_from_singular_values` the rank-vs-arc table and
        // gramian_spectrum use, so the two rank reads cannot silently disagree), and the condition
        // of OᵀO.
        let sv = if sqrt {
            sqrt_singular_values(&o, n_state)
        } else {
            singular_values(&o)
        };
        // …held additionally to the algebraic bound rank <= min(rows, cols): a prefix carrying
        // fewer measurement rows than states cannot observe more directions than it has rows,
        // whatever the tolerance (see `bounded_rank_from_singular_values`).
        let gramian_rank = bounded_rank_from_singular_values(&sv, rel_tol, o.len(), n_state).rank;
        let gram_values: Vec<f64> = if sqrt {
            // Eigenvalues of OᵀO as the squared singular values, ascending.
            sv.iter().rev().map(|s| s * s).collect()
        } else {
            let ones = vec![1.0; o.len()];
            let gram = information_matrix(&o, &ones);
            sym_eig(&gram).values
        };
        let gramian_condition = if gramian_rank == n_state {
            floored_condition(&gram_values, floor)
        } else {
            f64::INFINITY
        };

        // Independent SRIF estimator: posterior covariance finiteness + condition.
        let srif = srif_over_arc(&o, n_state);
        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(),
            state_dim: n_state,
            gramian_rank,
            gramian_condition,
            srif_posterior_wellposed,
            srif_condition,
        });
    }
    out
}

/// The first arc index at which the observable rank reaches the full state dimension
/// ([`SrifArcPoint::state_dim`] — four in the planar path, six in the spatial one), 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 == p.state_dim)
        .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
    }

    #[test]
    fn the_square_root_gramian_read_matches_the_default_read() {
        let rel_tol = 1e-6;
        let epochs = single_link_arc(24, 0.06);
        let a = srif_cross_validation(&epochs, rel_tol);
        let b = srif_cross_validation_sqrt(&epochs, rel_tol);
        assert_eq!(full_rank_transition(&a), full_rank_transition(&b));
        for (p, q) in a.iter().zip(&b) {
            assert_eq!(p.gramian_rank, q.gramian_rank, "epoch {}", p.epoch_index);
            assert_eq!(p.srif_condition.to_bits(), q.srif_condition.to_bits());
            if p.gramian_condition.is_finite() {
                let r = (p.gramian_condition - q.gramian_condition).abs() / p.gramian_condition;
                assert!(
                    r < 1e-6,
                    "epoch {}: {} vs {}",
                    p.epoch_index,
                    p.gramian_condition,
                    q.gramian_condition
                );
            } else {
                assert!(q.gramian_condition.is_infinite());
            }
        }
    }

    // ── 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"
        );
    }
}