kshana 0.37.0

// SPDX-License-Identifier: AGPL-3.0-only
//! Cold-atom gravimeter measurement model and a spherical-harmonic gravity-anomaly
//! reference field for **gravity-map-matching navigation** (GPS-denied alt-PNT).
//!
//! This is the alt-PNT capability layer ESA NAVISP's *Quantum Wayfarer* and QT-CCI
//! gravity-map-matching studies call for: a vehicle carrying an inertial unit and a
//! quantum (cold-atom-interferometer) gravimeter, navigating without GNSS by matching
//! the gravity-anomaly *signature it flies through* against a stored reference map. It
//! composes three pieces already in the tree:
//!
//! 1. [`crate::inertial::quantum_imu::CaiAccelerometer`] — the CAI physics that fixes
//!    the gravimeter's white-noise floor (a gravimeter is a CAI accelerometer measuring
//!    `g` over an averaging time);
//! 2. [`GravityAnomalyModel`] — the reference field, evaluated here as a low-degree,
//!    fully-normalised spherical-harmonic synthesis of the gravity anomaly plus optional
//!    localized anomalies (mascons);
//! 3. [`crate::mapmatch`] + [`crate::particle_filter`] — the map-matching particle
//!    filter that collapses the position uncertainty onto the true track.
//!
//! ## Scope (honest)
//!
//! The spherical-harmonic synthesis here is **correct geodesy at low degree** (validated below
//! against the closed-form normalised Legendre functions and a hand-derived single-term
//! anomaly). This benchmark's reference map is the low-degree trend from a handful of
//! well-known coefficients plus synthetic mascons (Gaussian anomalies) standing in for the
//! high-degree local features that make gravity-map matching observable. Kshana does not
//! bundle the full EGM2008 2190×2190 set (252 MB), but it can now load it at run time
//! ([`crate::gravity_sh::SphericalHarmonicField::from_gfc_file`], fetched by
//! `scripts/fetch_egm2008.sh`) and evaluate it to degree 2190 at any latitude
//! ([`crate::gravity_sh::SphericalHarmonicField::acceleration_hf`], checked against Orekit);
//! that field does not feed this map. The benchmark runs as the `gravity-map` scenario kind
//! with an SVG chart ([`crate::altpnt::terrain::gravity_nav_svg`]). A real EGM2008/EIGEN
//! gravity-anomaly map here, a crustal magnetic-anomaly map (the magnetic channel is the
//! IGRF-14 main field) and joint map estimation (full SLAM; sequential terrain-referenced
//! navigation is in [`crate::altpnt::sequential`]) remain follow-ons. See `docs/CAPABILITY.md`.

use crate::inertial::quantum_imu::CaiAccelerometer;
use crate::mapmatch::{hierarchical_offset_search, map_match_likelihood};
use crate::particle_filter::ParticleFilter;
use crate::portable_distr::{Distribution, Normal};
use crate::portable_math::PortableFloat;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
use serde::Deserialize;

/// Earth gravitational parameter `GM` (m³/s², WGS-84/EGM).
const GM_EARTH: f64 = 3.986_004_418e14;
/// Earth reference radius `R` (m, WGS-84 semi-major).
const R_EARTH: f64 = 6_378_137.0;
/// Metres per degree of latitude (spherical approximation, `π·R/180`). `pub(crate)` so the
/// alt-PNT navigators in [`crate::altpnt`] reuse the identical constant rather than
/// re-defining it (a divergent copy would be a silent unit bug).
pub(crate) const M_PER_DEG: f64 = 111_319.490_793_27;

/// One milligal in SI units (m/s²). `1 Gal = 0.01 m/s²`, so `1 mGal = 1e-5 m/s²`.
pub const MGAL: f64 = 1.0e-5;

// ---------------------------------------------------------------------------
// Fully-normalised associated Legendre functions P̄ₙₘ(sin φ)
// ---------------------------------------------------------------------------

/// Fully-normalised associated Legendre functions `P̄ₙₘ(t)` with `t = sin φ`, for all
/// `0 ≤ m ≤ n ≤ nmax`, returned as `p[n][m]`.
///
/// Built from the unnormalised Ferrers functions `Pₙₘ` via the standard recurrences and
/// then scaled by the geodesy normalisation `N̄ₙₘ = √((2−δ₀ₘ)(2n+1)(n−m)!/(n+m)!)`. This
/// "unnormalised recurrence × explicit factor" route is chosen because every low-degree
/// term has a textbook closed form to check against (see the unit tests):
/// `P̄₁₁ = √3·cos φ`, `P̄₂₀ = (√5/2)(3sin²φ−1)`, `P̄₂₂ = (√15/2)cos²φ`.
// The recurrences are inherently index-based (Pₙₘ depends on Pₙ₋₁,ₘ and Pₙ₋₂,ₘ), so the
// `0..=n` loops read clearer than iterator gymnastics over a triangular table.
#[allow(clippy::needless_range_loop)]
fn normalized_legendre(nmax: usize, t: f64) -> Vec<Vec<f64>> {
    let u = (1.0 - t * t).max(0.0).sqrt(); // cos φ ≥ 0 for φ ∈ [−90°, 90°]
    let mut p = vec![vec![0.0_f64; nmax + 1]; nmax + 1];
    p[0][0] = 1.0;
    // Sectorial: Pₘₘ = (2m−1)·u·Pₘ₋₁,ₘ₋₁  (so Pₘₘ = (2m−1)!!·uᵐ).
    for m in 1..=nmax {
        p[m][m] = (2 * m - 1) as f64 * u * p[m - 1][m - 1];
    }
    // First sub-diagonal: Pₘ₊₁,ₘ = t·(2m+1)·Pₘₘ.
    for m in 0..nmax {
        p[m + 1][m] = t * (2 * m + 1) as f64 * p[m][m];
    }
    // General column recurrence:
    // Pₙₘ = [ (2n−1)·t·Pₙ₋₁,ₘ − (n+m−1)·Pₙ₋₂,ₘ ] / (n−m).
    for m in 0..=nmax {
        for n in (m + 2)..=nmax {
            p[n][m] = ((2 * n - 1) as f64 * t * p[n - 1][m] - (n + m - 1) as f64 * p[n - 2][m])
                / (n - m) as f64;
        }
    }
    // Normalise in place.
    let mut pbar = vec![vec![0.0_f64; nmax + 1]; nmax + 1];
    for n in 0..=nmax {
        for m in 0..=n {
            let delta = if m == 0 { 1.0 } else { 2.0 };
            // (n−m)!/(n+m)! = 1 / ∏_{k=n−m+1}^{n+m} k.
            let mut ratio = 1.0_f64;
            for k in (n - m + 1)..=(n + m) {
                ratio /= k as f64;
            }
            let norm = (delta * (2 * n + 1) as f64 * ratio).sqrt();
            pbar[n][m] = norm * p[n][m];
        }
    }
    pbar
}

// ---------------------------------------------------------------------------
// Gravity-anomaly reference field
// ---------------------------------------------------------------------------

/// A localized Gaussian gravity anomaly ("mascon"): the synthetic stand-in for the
/// high-degree EGM content Kshana does not bundle, used to give a map-matching benchmark a
/// distinctive, well-conditioned local signature.
#[derive(Clone, Copy, Debug, Deserialize)]
pub struct Mascon {
    /// Centre latitude (degrees).
    pub lat_deg: f64,
    /// Centre longitude (degrees).
    pub lon_deg: f64,
    /// Peak amplitude (mGal).
    pub amp_mgal: f64,
    /// Gaussian 1σ width (degrees of arc).
    pub sigma_deg: f64,
}

/// A spherical-harmonic gravity-anomaly model evaluated on the reference sphere `r = R`,
/// plus optional [`Mascon`] features. Coefficients are the *anomalous* fully-normalised
/// `C̄ₙₘ`, `S̄ₙₘ` (i.e. relative to the reference ellipsoid, so the even-zonal `J₂`, `J₄`
/// reference field is already removed and degree-2 terms are the disturbing field).
#[derive(Clone, Debug)]
pub struct GravityAnomalyModel {
    nmax: usize,
    cbar: Vec<Vec<f64>>,
    sbar: Vec<Vec<f64>>,
    mascons: Vec<Mascon>,
}

impl GravityAnomalyModel {
    /// An empty model (zero field) up to degree/order `nmax`.
    pub fn new(nmax: usize) -> Self {
        Self {
            nmax,
            cbar: vec![vec![0.0; nmax + 1]; nmax + 1],
            sbar: vec![vec![0.0; nmax + 1]; nmax + 1],
            mascons: Vec::new(),
        }
    }

    /// Set the normalised anomalous coefficients `C̄ₙₘ`, `S̄ₙₘ` for degree `n`, order `m`.
    pub fn set_coeff(&mut self, n: usize, m: usize, cbar: f64, sbar: f64) {
        if n <= self.nmax && m <= n {
            self.cbar[n][m] = cbar;
            self.sbar[n][m] = sbar;
        }
    }

    /// Add a localized Gaussian anomaly.
    pub fn add_mascon(&mut self, m: Mascon) {
        self.mascons.push(m);
    }

    /// Gravity anomaly Δg (mGal) at geocentric latitude/longitude (radians) on the
    /// reference sphere `r = R`:
    ///
    /// `Δg = (GM/R²) · Σₙ₌₂ᴺ (n−1) · Σₘ (C̄ₙₘ cos mλ + S̄ₙₘ sin mλ) · P̄ₙₘ(sin φ)`
    ///
    /// plus the sum of the Gaussian mascons.
    // Indexing the parallel C̄/S̄/P̄ tables by (n, m) is clearer than zipping three
    // triangular arrays.
    #[allow(clippy::needless_range_loop)]
    pub fn anomaly_mgal(&self, lat_rad: f64, lon_rad: f64) -> f64 {
        let t = lat_rad.psin();
        let pbar = normalized_legendre(self.nmax, t);
        let mut sum = 0.0;
        for n in 2..=self.nmax {
            let scale = (n as f64) - 1.0;
            for m in 0..=n {
                let ml = (m as f64) * lon_rad;
                let trig = self.cbar[n][m] * ml.pcos() + self.sbar[n][m] * ml.psin();
                sum += scale * trig * pbar[n][m];
            }
        }
        let mut anom = (GM_EARTH / (R_EARTH * R_EARTH)) * sum / MGAL;

        let lat_deg = lat_rad.to_degrees();
        let lon_deg = lon_rad.to_degrees();
        let cos_lat = lat_rad.pcos();
        for ms in &self.mascons {
            let dlat = lat_deg - ms.lat_deg;
            let dlon = (lon_deg - ms.lon_deg) * cos_lat; // metric east-west scaling
            let r2 = (dlat * dlat + dlon * dlon) / (2.0 * ms.sigma_deg * ms.sigma_deg);
            anom += ms.amp_mgal * (-r2).pexp();
        }
        anom
    }

    /// Field sampler in **degrees** matching the `Fn(lat, lon) -> value` signature
    /// [`crate::mapmatch::map_match_likelihood`] expects.
    pub fn sampler_deg(&self) -> impl Fn(f64, f64) -> f64 + '_ {
        move |lat_deg: f64, lon_deg: f64| {
            self.anomaly_mgal(lat_deg.to_radians(), lon_deg.to_radians())
        }
    }
}

// ---------------------------------------------------------------------------
// Cold-atom gravimeter measurement model
// ---------------------------------------------------------------------------

/// A cold-atom gravimeter: a CAI accelerometer pointed at local gravity. Its white
/// measurement-noise floor is fixed by the interferometer's acceleration ASD and the
/// averaging time; a static bias models the residual systematic offset.
#[derive(Clone, Copy, Debug)]
pub struct Gravimeter {
    /// Acceleration amplitude spectral density (m/s²/√Hz).
    asd_si: f64,
    /// Static measurement bias (mGal).
    bias_mgal: f64,
}

impl Gravimeter {
    /// A gravimeter with an explicit acceleration ASD (m/s²/√Hz) and bias (mGal).
    pub fn new(asd_si: f64, bias_mgal: f64) -> Self {
        Self { asd_si, bias_mgal }
    }

    /// A gravimeter whose noise floor is derived from a [`CaiAccelerometer`]'s ASD.
    pub fn from_cai(sensor: &CaiAccelerometer, bias_mgal: f64) -> Self {
        Self::new(sensor.accel_asd(), bias_mgal)
    }

    /// 1σ white measurement noise (mGal) after averaging for `tau_s`: a white-ASD sensor
    /// averages down as `σ = ASD/√τ`.
    pub fn measurement_sigma_mgal(&self, tau_s: f64) -> f64 {
        if tau_s <= 0.0 {
            return f64::INFINITY;
        }
        (self.asd_si / tau_s.sqrt()) / MGAL
    }

    /// A measured anomaly (mGal): truth + static bias + a caller-supplied noise sample.
    /// Noise is injected by the caller (deterministic in tests) rather than drawn here.
    pub fn measure(&self, true_anomaly_mgal: f64, noise_sample_mgal: f64) -> f64 {
        true_anomaly_mgal + self.bias_mgal + noise_sample_mgal
    }
}

// ---------------------------------------------------------------------------
// Gravity-map-matching navigation benchmark
// ---------------------------------------------------------------------------

/// One gravity-anomaly coefficient, for the benchmark TOML.
#[derive(Clone, Copy, Debug, Deserialize)]
pub struct CoeffEntry {
    /// Degree n.
    pub n: usize,
    /// Order m.
    pub m: usize,
    /// Normalised cosine coefficient.
    pub cbar: f64,
    /// Normalised sine coefficient.
    pub sbar: f64,
}

/// Benchmark configuration (deserialised from `scenarios/gravity-map-nav.toml`).
#[derive(Clone, Debug, Deserialize)]
pub struct GravityMapBenchmarkCfg {
    /// Maximum spherical-harmonic degree/order of the reference field.
    pub nmax: usize,
    /// Low-degree anomalous coefficients (regional trend).
    #[serde(default)]
    pub coeffs: Vec<CoeffEntry>,
    /// Localized Gaussian anomalies (the high-degree stand-in).
    #[serde(default)]
    pub mascons: Vec<Mascon>,
    /// Track start latitude / longitude (degrees).
    pub start_lat_deg: f64,
    /// Track start longitude (degrees).
    pub start_lon_deg: f64,
    /// Per-waypoint latitude / longitude step (degrees) — an oblique leg samples the
    /// anomaly gradient in both axes so a single scalar measurement sequence is 2-D
    /// observable.
    pub step_lat_deg: f64,
    /// Per-waypoint longitude step (degrees).
    pub step_lon_deg: f64,
    /// Number of waypoints flown GPS-denied.
    pub waypoints: usize,
    /// True constant INS position-drift error over the outage (degrees lat, lon).
    pub drift_lat_deg: f64,
    /// True constant INS position-drift error, longitude component (degrees).
    pub drift_lon_deg: f64,
    /// Gravimeter acceleration ASD (m/s²/√Hz).
    pub gravimeter_asd: f64,
    /// Per-waypoint gravimeter averaging time (s).
    pub averaging_time_s: f64,
    /// Map/representation error (mGal): the reference field is never exact, and the
    /// candidate grid quantises position — both enter the matching budget alongside the
    /// gravimeter white noise. The effective matching σ is `hypot(sensor σ, this)`.
    pub map_sigma_mgal: f64,
    /// Half-width of the offset search grid (degrees).
    pub search_half_deg: f64,
    /// Offset search-grid step (degrees).
    pub search_step_deg: f64,
    /// Number of coarse-to-fine refinement stages for the offset search. `1` (the default)
    /// is the original single-grid behaviour; each extra stage recentres on the running
    /// estimate and shrinks the window and step by [`Self::refine_factor`], buying sub-grid
    /// resolution without an intractably fine single grid. Used by
    /// [`run_gps_denied_gravity_nav`]; [`run_gravity_map_benchmark`] ignores it.
    #[serde(default = "default_refine_stages")]
    pub refine_stages: usize,
    /// Per-stage shrink factor applied to the refinement window half-width and step.
    #[serde(default = "default_refine_factor")]
    pub refine_factor: f64,
    /// Seed for the deterministic gravimeter measurement-noise sequence injected by
    /// [`run_gps_denied_gravity_nav`] (per-seed reproducible; vary it to sweep realisations).
    #[serde(default)]
    pub noise_seed: u64,
    /// Optional **real reference field** (RM-17). When present the matcher's map and the
    /// simulated gravimeter both use the gravity anomaly synthesised from a
    /// spherical-harmonic model instead of `coeffs` + `mascons` (which must then be empty).
    /// Absent (the default), the synthetic field is used exactly as before.
    #[serde(default)]
    pub reference: Option<GravityReferenceCfg>,
    /// Optional **map-error Monte Carlo** (RM-15): rerun the benchmark over seeded
    /// realisations of a correlated map error added to the stored map (the gravimeter still
    /// measures the true field), and report the distribution of the matched error.
    #[serde(default)]
    pub map_error_mc: Option<MapErrorMcCfg>,
}

/// A real spherical-harmonic reference field for the gravity-map benchmark.
#[derive(Clone, Debug, Deserialize)]
#[serde(deny_unknown_fields)]
pub struct GravityReferenceCfg {
    /// `"egm2008"`: the bundled EGM2008 coefficients (to degree 70). `"gfc"`: an ICGEM
    /// `.gfc` file named by `gfc_path` (any degree; a high-degree model resolves the
    /// tens-of-kilometre features the bundled one cannot).
    pub model: String,
    /// The `.gfc` file to read when `model = "gfc"` (native builds only).
    #[serde(default)]
    pub gfc_path: Option<String>,
    /// Maximum degree used (clamped to the model's own).
    pub nmax: usize,
    /// Normal-field ellipsoid the anomaly is taken against: `"wgs84"` (default) or `"grs80"`.
    #[serde(default = "default_ellipsoid")]
    pub ellipsoid: String,
    /// Keep the degree-0 term `1 − GM_ell/GM` (ICGEM's "zero degree term"; default true).
    #[serde(default = "default_true")]
    pub zero_degree: bool,
    /// Node spacing of the regional anomaly grid the navigator samples bilinearly (deg).
    #[serde(default = "default_reference_step")]
    pub grid_step_deg: f64,
}

/// The map-error Monte Carlo settings.
#[derive(Clone, Debug, Deserialize)]
#[serde(deny_unknown_fields)]
pub struct MapErrorMcCfg {
    /// Number of map-error realisations.
    pub realisations: usize,
    /// 1σ of the map error added to the stored map (mGal).
    pub sigma_mgal: f64,
    /// Correlation length of the Gaussian map-error correlation function (deg of arc).
    pub correlation_length_deg: f64,
    /// Node spacing of the error lattice (deg; default 0.01).
    #[serde(default = "default_mc_step")]
    pub grid_step_deg: f64,
    /// Seed of realisation 0; realisation `k` uses `seed + k` for its map error and
    /// `noise_seed + k` for its gravimeter noise.
    #[serde(default)]
    pub seed: u64,
    /// Matched-error target the success fraction is counted against (m; default 500).
    #[serde(default = "default_target_m")]
    pub target_m: f64,
}

fn default_ellipsoid() -> String {
    "wgs84".into()
}
fn default_true() -> bool {
    true
}
fn default_reference_step() -> f64 {
    0.02
}
fn default_mc_step() -> f64 {
    0.01
}
fn default_target_m() -> f64 {
    500.0
}

fn default_refine_stages() -> usize {
    1
}

fn default_refine_factor() -> f64 {
    8.0
}

/// Unit and provenance class for every numeric field the `gravity-map` report emits.
///
/// The report is the three fields of [`GravityMapNavResult`], serialised by the scenario
/// dispatcher; all three are metric quantities the run derives from the configured drift,
/// the recovered offset and the sensor/map noise budget.
pub const UNITS: &[crate::field_schema::FieldUnit] = {
    use crate::field_schema::{FieldUnit, ProvenanceClass::*};
    &[
        FieldUnit {
            path: "free_inertial_drift_m",
            unit: "m",
            provenance: Computed,
            definition: "horizontal position error of the unaided inertial solution at the \
                         end of the outage: the configured constant drift in degrees \
                         converted to metres at the track's mid-latitude",
        },
        FieldUnit {
            path: "map_matched_error_m",
            unit: "m",
            provenance: Computed,
            definition: "residual horizontal position error after gravity-map matching: the \
                         true drift minus the recovered offset estimate, in metres",
        },
        FieldUnit {
            path: "measurement_sigma_mgal",
            unit: "mGal",
            provenance: Computed,
            definition: "1 sigma matching noise used by the likelihood: the gravimeter white \
                         floor ASD/sqrt(tau) combined in quadrature with the configured map \
                         representation error",
        },
    ]
};

/// Units of the optional reference-field fields of a `gravity-map` document, appended to
/// [`UNITS`] only when a `reference` is configured (the fields are flattened into the root).
pub const REFERENCE_UNITS: &[crate::field_schema::FieldUnit] = {
    use crate::field_schema::{FieldUnit, ProvenanceClass::*};
    &[
        FieldUnit {
            path: "reference_nmax",
            unit: "count",
            provenance: Input,
            definition: "maximum spherical-harmonic degree of the reference field used",
        },
        FieldUnit {
            path: "reference_grid_step_deg",
            unit: "deg",
            provenance: Input,
            definition: "node spacing of the regional anomaly grid the matcher samples \
                         bilinearly",
        },
        FieldUnit {
            path: "reference_anomaly_mean_mgal",
            unit: "mGal",
            provenance: Computed,
            definition: "mean of the synthesised gravity anomaly over the run's sampling box",
        },
        FieldUnit {
            path: "reference_anomaly_std_mgal",
            unit: "mGal",
            provenance: Computed,
            definition: "population standard deviation of the synthesised gravity anomaly \
                         over the run's sampling box: the matchable signal",
        },
    ]
};

/// Units of the optional `map_error_mc` block of a `gravity-map` document, appended to
/// [`UNITS`] only when the map-error Monte Carlo is configured.
pub const MAP_ERROR_MC_UNITS: &[crate::field_schema::FieldUnit] = {
    use crate::field_schema::{FieldUnit, ProvenanceClass::*};
    &[
        FieldUnit {
            path: "map_error_mc.realisations",
            unit: "count",
            provenance: Input,
            definition: "number of seeded map-error realisations run",
        },
        FieldUnit {
            path: "map_error_mc.sigma_mgal",
            unit: "mGal",
            provenance: ModelledInput,
            definition: "1 sigma of the correlated map error added to the stored map",
        },
        FieldUnit {
            path: "map_error_mc.correlation_length_deg",
            unit: "deg",
            provenance: ModelledInput,
            definition: "correlation length L of the Gaussian map-error correlation \
                         exp(-r^2/(2 L^2)), in degrees of arc",
        },
        FieldUnit {
            path: "map_error_mc.target_m",
            unit: "m",
            provenance: Input,
            definition: "matched-error target the success fraction is counted against",
        },
        FieldUnit {
            path: "map_error_mc.matched_error_mean_m",
            unit: "m",
            provenance: Computed,
            definition: "mean matched position error over the realisations",
        },
        FieldUnit {
            path: "map_error_mc.matched_error_p50_m",
            unit: "m",
            provenance: Computed,
            definition: "median matched position error (nearest rank)",
        },
        FieldUnit {
            path: "map_error_mc.matched_error_p95_m",
            unit: "m",
            provenance: Computed,
            definition: "95th-percentile matched position error (nearest rank)",
        },
        FieldUnit {
            path: "map_error_mc.matched_error_max_m",
            unit: "m",
            provenance: Computed,
            definition: "largest matched position error over the realisations",
        },
        FieldUnit {
            path: "map_error_mc.fraction_within_target",
            unit: "1",
            provenance: Computed,
            definition: "fraction of realisations whose matched error is at or under \
                         target_m",
        },
    ]
};

/// Result of the gravity-map-matching benchmark.
#[derive(Clone, Copy, Debug)]
pub struct GravityMapNavResult {
    /// Position error of the unaided inertial solution at end of the outage (m) — the
    /// magnitude of the true INS drift.
    pub free_inertial_drift_m: f64,
    /// Position error after gravity-map matching (m).
    pub map_matched_error_m: f64,
    /// Gravimeter 1σ measurement noise used (mGal).
    pub measurement_sigma_mgal: f64,
}

/// Run the gravity-map-matching benchmark.
///
/// A vehicle flies a known-shape oblique track GPS-denied. Its inertial solution has
/// accumulated a constant, unknown position offset `d` (the drift). At each waypoint the
/// quantum gravimeter measures the local gravity anomaly. A particle filter searches over
/// candidate offsets `δ`: each candidate hypothesises the true positions as
/// `(INS-reported − δ)` and predicts the anomaly there; the product likelihood over the
/// waypoint sequence peaks when `δ = d`. The matched solution's residual error is reported
/// against the unaided drift.
pub fn run_gravity_map_benchmark(cfg: &GravityMapBenchmarkCfg) -> GravityMapNavResult {
    // Reference field.
    let mut model = GravityAnomalyModel::new(cfg.nmax);
    for c in &cfg.coeffs {
        model.set_coeff(c.n, c.m, c.cbar, c.sbar);
    }
    for m in &cfg.mascons {
        model.add_mascon(*m);
    }
    let field = model.sampler_deg();

    // Gravimeter white-noise floor, combined with the map/representation error into the
    // effective matching σ used by the likelihood.
    let grav = Gravimeter::new(cfg.gravimeter_asd, 0.0);
    let sensor_sigma = grav.measurement_sigma_mgal(cfg.averaging_time_s);
    let sigma_mgal = (sensor_sigma * sensor_sigma + cfg.map_sigma_mgal * cfg.map_sigma_mgal).sqrt();

    // True track and the gravimeter measurements taken along it (noise-free truth — the
    // benchmark isolates geometric observability, not a Monte-Carlo noise realisation).
    let truth: Vec<(f64, f64)> = (0..cfg.waypoints)
        .map(|k| {
            (
                cfg.start_lat_deg + cfg.step_lat_deg * k as f64,
                cfg.start_lon_deg + cfg.step_lon_deg * k as f64,
            )
        })
        .collect();
    let measured: Vec<f64> = truth.iter().map(|&(la, lo)| field(la, lo)).collect();

    // INS-reported positions carry the true constant drift d.
    let ins: Vec<(f64, f64)> = truth
        .iter()
        .map(|&(la, lo)| (la + cfg.drift_lat_deg, lo + cfg.drift_lon_deg))
        .collect();

    // Candidate-offset grid δ (centred on zero — the filter's prior is "no drift").
    let mut particles = Vec::new();
    let n_side = (cfg.search_half_deg / cfg.search_step_deg).round() as i64;
    for i in -n_side..=n_side {
        for j in -n_side..=n_side {
            particles.push(vec![
                i as f64 * cfg.search_step_deg,
                j as f64 * cfg.search_step_deg,
            ]);
        }
    }
    let mut pf = ParticleFilter::new(particles);

    // Reweight by the product likelihood over the whole measurement sequence.
    pf.update(|delta| {
        let mut like = 1.0;
        for (k, &(la, lo)) in ins.iter().enumerate() {
            // Hypothesised true position = INS-reported − δ.
            let hlat = la - delta[0];
            let hlon = lo - delta[1];
            like *= map_match_likelihood(&field, hlat, hlon, measured[k], sigma_mgal);
        }
        like
    });
    let est = pf.estimate(); // δ̂ (degrees lat, lon)

    // Representative latitude for the east-west metric scale.
    let mid_lat = cfg.start_lat_deg + cfg.step_lat_deg * (cfg.waypoints as f64 - 1.0) / 2.0;
    let cos_lat = mid_lat.to_radians().pcos();
    let to_m = |dlat: f64, dlon: f64| {
        let north = dlat * M_PER_DEG;
        let east = dlon * M_PER_DEG * cos_lat;
        (north * north + east * east).sqrt()
    };

    let free_inertial_drift_m = to_m(cfg.drift_lat_deg, cfg.drift_lon_deg);
    // Residual = (true drift − estimated offset), i.e. the position error the matched
    // solution still carries.
    let map_matched_error_m = to_m(cfg.drift_lat_deg - est[0], cfg.drift_lon_deg - est[1]);

    GravityMapNavResult {
        free_inertial_drift_m,
        map_matched_error_m,
        measurement_sigma_mgal: sigma_mgal,
    }
}

/// Run the full **60-minute GPS-denied** gravity-map-matching benchmark.
///
/// This is the harder, validation-grade form of [`run_gravity_map_benchmark`] and the
/// capability target of ESA NAVISP's *Quantum Wayfarer* gravity-aided alt-PNT: a vehicle
/// flies a long known-shape track with **no GNSS**, its inertial solution carrying an
/// unknown constant position drift `d`. A quantum (cold-atom) gravimeter samples the local
/// gravity anomaly at each waypoint; the samples carry the sensor's real white-noise floor,
/// injected here as a **deterministic, seeded** Gaussian sequence (so the matcher is never
/// handed noise-free truth, yet the test is exactly reproducible). The offset is recovered
/// by a **hierarchical coarse-to-fine** particle/grid search: stage 1 sweeps the full
/// `±search_half_deg` window at `search_step_deg`; each later stage recentres on the running
/// estimate and shrinks the window and step by `refine_factor`, so the final offset
/// resolution is `search_step_deg / refine_factor^(refine_stages−1)` — sub-grid accuracy
/// without an intractably fine single grid.
///
/// Returns the unaided inertial drift (which, for the committed scenario, exceeds 10 km) and
/// the matched residual (which falls below a few hundred metres).
///
/// ## Scope (honest)
///
/// The injected noise is the gravimeter's white **sensor** floor; the matching `σ` also
/// budgets a map representation-error term ([`GravityMapBenchmarkCfg::map_sigma_mgal`]), but
/// a full Monte-Carlo over map-error *realisations* — perturbing the stored map away from the
/// truth field — and a map built from the full EGM2008 field (loadable through
/// [`crate::gravity_sh::SphericalHarmonicField::from_gfc_file`]) are follow-ons (see
/// `docs/CAPABILITY.md`). The reference field here is the low-degree spherical-harmonic trend
/// plus synthetic mascons validated in this module's unit tests.
pub fn run_gps_denied_gravity_nav(cfg: &GravityMapBenchmarkCfg) -> GravityMapNavResult {
    let model = synthetic_model(cfg);
    let field = model.sampler_deg();
    gps_denied_core(cfg, &field, &field, cfg.noise_seed)
}

/// The synthetic reference field of `cfg`: the low-degree coefficients plus the mascons.
fn synthetic_model(cfg: &GravityMapBenchmarkCfg) -> GravityAnomalyModel {
    let mut model = GravityAnomalyModel::new(cfg.nmax);
    for c in &cfg.coeffs {
        model.set_coeff(c.n, c.m, c.cbar, c.sbar);
    }
    for m in &cfg.mascons {
        model.add_mascon(*m);
    }
    model
}

/// The GPS-denied matcher, with the field the gravimeter measures (`truth`) and the field
/// the matcher holds (`stored`) given separately so a map error can sit between them. With
/// the same closure for both, this is exactly the original single-field benchmark.
fn gps_denied_core(
    cfg: &GravityMapBenchmarkCfg,
    truth_field: &dyn Fn(f64, f64) -> f64,
    stored_field: &dyn Fn(f64, f64) -> f64,
    noise_seed: u64,
) -> GravityMapNavResult {
    // Gravimeter white-noise floor; the matching σ additionally budgets the map error.
    let grav = Gravimeter::new(cfg.gravimeter_asd, 0.0);
    let sensor_sigma = grav.measurement_sigma_mgal(cfg.averaging_time_s);
    let sigma_mgal = (sensor_sigma * sensor_sigma + cfg.map_sigma_mgal * cfg.map_sigma_mgal).sqrt();

    // The GPS-denied track and the noisy gravimeter samples taken along it.
    let truth = track(cfg);
    let mut rng = ChaCha8Rng::seed_from_u64(noise_seed);
    // `Normal::new` (rand_distr 0.4) rejects only a non-finite std_dev; `sensor_sigma`
    // is derived from caller config and could be non-finite, so floor it to a finite,
    // strictly-positive value first.
    let noise_sigma = {
        let s = sensor_sigma.max(f64::MIN_POSITIVE);
        if s.is_finite() {
            s
        } else {
            f64::MIN_POSITIVE
        }
    };
    let noise = Normal::new(0.0, noise_sigma)
        .expect("noise_sigma is finite and strictly positive, which Normal::new always accepts");
    let measured: Vec<f64> = truth
        .iter()
        .map(|&(la, lo)| truth_field(la, lo) + noise.sample(&mut rng))
        .collect();

    // INS-reported positions carry the true constant drift d.
    let ins: Vec<(f64, f64)> = truth
        .iter()
        .map(|&(la, lo)| (la + cfg.drift_lat_deg, lo + cfg.drift_lon_deg))
        .collect();

    // Product likelihood over the whole waypoint sequence for a candidate offset δ.
    let weigh = |delta: &[f64]| -> f64 {
        let mut like = 1.0;
        for (k, &(la, lo)) in ins.iter().enumerate() {
            like *= map_match_likelihood(
                stored_field,
                la - delta[0],
                lo - delta[1],
                measured[k],
                sigma_mgal,
            );
        }
        like
    };

    // Hierarchical coarse-to-fine offset search (one shared implementation in `mapmatch`).
    let est = hierarchical_offset_search(
        weigh,
        cfg.search_half_deg,
        cfg.search_step_deg,
        cfg.refine_stages,
        cfg.refine_factor,
    );

    let mid_lat = cfg.start_lat_deg + cfg.step_lat_deg * (cfg.waypoints as f64 - 1.0) / 2.0;
    let cos_lat = mid_lat.to_radians().pcos();
    let to_m = |dlat: f64, dlon: f64| {
        let north = dlat * M_PER_DEG;
        let east = dlon * M_PER_DEG * cos_lat;
        (north * north + east * east).sqrt()
    };

    GravityMapNavResult {
        free_inertial_drift_m: to_m(cfg.drift_lat_deg, cfg.drift_lon_deg),
        map_matched_error_m: to_m(cfg.drift_lat_deg - est[0], cfg.drift_lon_deg - est[1]),
        measurement_sigma_mgal: sigma_mgal,
    }
}

/// The true waypoint track of `cfg`.
fn track(cfg: &GravityMapBenchmarkCfg) -> Vec<(f64, f64)> {
    (0..cfg.waypoints)
        .map(|k| {
            (
                cfg.start_lat_deg + cfg.step_lat_deg * k as f64,
                cfg.start_lon_deg + cfg.step_lon_deg * k as f64,
            )
        })
        .collect()
}

/// The box (south, north, west, east; deg) every field sample of a run can fall in: the
/// true and INS-reported tracks widened by the search half-width, one search step and a
/// 0.05 deg guard.
fn sample_box(cfg: &GravityMapBenchmarkCfg) -> (f64, f64, f64, f64) {
    let t = track(cfg);
    let pad = cfg.search_half_deg.abs() + cfg.search_step_deg.abs() + 0.05;
    let lats = t.iter().flat_map(|&(la, _)| [la, la + cfg.drift_lat_deg]);
    let lons = t.iter().flat_map(|&(_, lo)| [lo, lo + cfg.drift_lon_deg]);
    let (s, n) = lats.fold((f64::MAX, f64::MIN), |(a, b), x| (a.min(x), b.max(x)));
    let (w, e) = lons.fold((f64::MAX, f64::MIN), |(a, b), x| (a.min(x), b.max(x)));
    (s - pad, n + pad, w - pad, e + pad)
}

/// What the real reference field looked like over the run's sampling box.
#[derive(Clone, Copy, Debug, serde::Serialize)]
pub struct ReferenceFieldSummary {
    /// Maximum spherical-harmonic degree used.
    pub reference_nmax: usize,
    /// Node spacing of the sampled anomaly grid (deg).
    pub reference_grid_step_deg: f64,
    /// Mean of the anomaly over the sampling box (mGal).
    pub reference_anomaly_mean_mgal: f64,
    /// Standard deviation of the anomaly over the sampling box (mGal): the matchable signal.
    pub reference_anomaly_std_mgal: f64,
}

/// The distribution of the matched error over the map-error Monte Carlo.
#[derive(Clone, Copy, Debug, serde::Serialize)]
pub struct MapErrorMcSummary {
    /// Realisations run.
    pub realisations: usize,
    /// Map-error 1σ (mGal).
    pub sigma_mgal: f64,
    /// Map-error correlation length (deg).
    pub correlation_length_deg: f64,
    /// Target the success fraction counts against (m).
    pub target_m: f64,
    /// Mean matched error (m).
    pub matched_error_mean_m: f64,
    /// Median matched error (m, nearest rank).
    pub matched_error_p50_m: f64,
    /// 95th-percentile matched error (m, nearest rank).
    pub matched_error_p95_m: f64,
    /// Largest matched error (m).
    pub matched_error_max_m: f64,
    /// Fraction of realisations whose matched error is at or under `target_m`.
    pub fraction_within_target: f64,
}

/// A full `gravity-map` scenario run: the benchmark result plus the optional reference-field
/// and map-error summaries.
#[derive(Clone, Copy, Debug)]
pub struct GravityMapRun {
    /// The single-run benchmark result (no map error).
    pub nav: GravityMapNavResult,
    /// Present when a real reference field was configured.
    pub reference: Option<ReferenceFieldSummary>,
    /// Present when the map-error Monte Carlo was configured.
    pub map_error_mc: Option<MapErrorMcSummary>,
}

/// Build the real reference anomaly grid of `r` over `cfg`'s sampling box.
fn reference_grid(
    cfg: &GravityMapBenchmarkCfg,
    r: &GravityReferenceCfg,
) -> Result<crate::altpnt::maps::GeoGrid, String> {
    use crate::gravity_sh::{SphericalHarmonicField, GRS80_ELLIPSOID, WGS84_ELLIPSOID};
    if !cfg.coeffs.is_empty() || !cfg.mascons.is_empty() {
        return Err(
            "gravity-map: `reference` replaces the synthetic field, so `coeffs` and \
                    `mascons` must be empty when it is set"
                .into(),
        );
    }
    let field = match r.model.as_str() {
        "egm2008" => SphericalHarmonicField::egm2008_truncated(r.nmax),
        "gfc" => {
            let path = r
                .gfc_path
                .as_deref()
                .ok_or("gravity-map: reference model \"gfc\" needs `gfc_path`")?;
            let text = std::fs::read_to_string(path)
                .map_err(|e| format!("gravity-map: cannot read gfc_path {path}: {e}"))?;
            SphericalHarmonicField::from_gfc(&text, r.nmax)?
        }
        other => {
            return Err(format!(
                "gravity-map: unknown reference model `{other}` (use \"egm2008\" or \"gfc\")"
            ))
        }
    };
    let ell = match r.ellipsoid.as_str() {
        "wgs84" => WGS84_ELLIPSOID,
        "grs80" => GRS80_ELLIPSOID,
        other => {
            return Err(format!(
                "gravity-map: unknown ellipsoid `{other}` (use \"wgs84\" or \"grs80\")"
            ))
        }
    };
    let (s, n, w, e) = sample_box(cfg);
    crate::altpnt::maps::GeoGrid::gravity_anomaly_from_sh(
        &field,
        &ell,
        r.zero_degree,
        s,
        n,
        w,
        e,
        r.grid_step_deg,
    )
}

/// Run a `gravity-map` scenario: the GPS-denied benchmark on the synthetic field, or on a
/// real spherical-harmonic reference field when `reference` is set, plus the map-error
/// Monte Carlo when `map_error_mc` is set. With neither set this is exactly
/// [`run_gps_denied_gravity_nav`].
pub fn run_gravity_map_scenario(cfg: &GravityMapBenchmarkCfg) -> Result<GravityMapRun, String> {
    let model = synthetic_model(cfg);
    let synthetic = model.sampler_deg();
    let grid = match &cfg.reference {
        Some(r) => Some((reference_grid(cfg, r)?, r)),
        None => None,
    };
    let grid_sampler = grid.as_ref().map(|(g, _)| g.sampler_deg());
    let truth: &dyn Fn(f64, f64) -> f64 = match &grid_sampler {
        Some(s) => s,
        None => &synthetic,
    };
    let nav = gps_denied_core(cfg, truth, truth, cfg.noise_seed);
    let reference = grid.as_ref().map(|(g, r)| {
        let (mean, sd) = g.mean_std();
        ReferenceFieldSummary {
            reference_nmax: r.nmax,
            reference_grid_step_deg: r.grid_step_deg,
            reference_anomaly_mean_mgal: mean,
            reference_anomaly_std_mgal: sd,
        }
    });
    let map_error_mc = match &cfg.map_error_mc {
        Some(mc) => Some(map_error_monte_carlo(cfg, mc, truth)?),
        None => None,
    };
    Ok(GravityMapRun {
        nav,
        reference,
        map_error_mc,
    })
}

/// The map-error Monte Carlo: for each realisation `k`, a correlated error field
/// ([`crate::altpnt::maps::correlated_error_field`], seed `mc.seed + k`) is added to the
/// stored map, the gravimeter still measures the true field (noise seed `noise_seed + k`),
/// and the matched error is recorded.
fn map_error_monte_carlo(
    cfg: &GravityMapBenchmarkCfg,
    mc: &MapErrorMcCfg,
    truth: &dyn Fn(f64, f64) -> f64,
) -> Result<MapErrorMcSummary, String> {
    if mc.realisations == 0 {
        return Err("gravity-map: map_error_mc.realisations must be at least 1".into());
    }
    if mc.realisations > 10_000 {
        return Err("gravity-map: map_error_mc.realisations is capped at 10000".into());
    }
    let (s, n, w, e) = sample_box(cfg);
    let mut errs = Vec::with_capacity(mc.realisations);
    for k in 0..mc.realisations {
        let err = crate::altpnt::maps::correlated_error_field(
            s,
            n,
            w,
            e,
            mc.grid_step_deg,
            mc.sigma_mgal,
            mc.correlation_length_deg,
            mc.seed.wrapping_add(k as u64),
        )?;
        let stored = |la: f64, lo: f64| truth(la, lo) + err.value_at(la, lo);
        let r = gps_denied_core(cfg, truth, &stored, cfg.noise_seed.wrapping_add(k as u64));
        errs.push(r.map_matched_error_m);
    }
    let nf = errs.len() as f64;
    let mean = errs.iter().sum::<f64>() / nf;
    let within = errs.iter().filter(|&&x| x <= mc.target_m).count() as f64 / nf;
    let mut sorted = errs.clone();
    sorted.sort_by(|a, b| a.total_cmp(b));
    let rank = |q: f64| sorted[((q * nf).ceil() as usize).clamp(1, sorted.len()) - 1];
    Ok(MapErrorMcSummary {
        realisations: mc.realisations,
        sigma_mgal: mc.sigma_mgal,
        correlation_length_deg: mc.correlation_length_deg,
        target_m: mc.target_m,
        matched_error_mean_m: mean,
        matched_error_p50_m: rank(0.5),
        matched_error_p95_m: rank(0.95),
        matched_error_max_m: sorted[sorted.len() - 1],
        fraction_within_target: within,
    })
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn normalized_legendre_matches_closed_forms_at_the_equator() {
        // At the equator sin φ = 0, cos φ = 1.
        let p = normalized_legendre(2, 0.0);
        assert!((p[0][0] - 1.0).abs() < 1e-12);
        assert!((p[1][1] - 3.0_f64.sqrt()).abs() < 1e-12); // √3·cos φ
        assert!(p[1][0].abs() < 1e-12); // √3·sin φ = 0
                                        // P̄₂₀ = (√5/2)(3sin²φ − 1) = −√5/2 at the equator.
        assert!((p[2][0] + (5.0_f64.sqrt() / 2.0)).abs() < 1e-12);
        assert!(p[2][1].abs() < 1e-12); // √15·sin φ·cos φ = 0
                                        // P̄₂₂ = (√15/2)cos²φ = √15/2.
        assert!((p[2][2] - 15.0_f64.sqrt() / 2.0).abs() < 1e-12);
    }

    #[test]
    fn normalized_legendre_matches_closed_forms_off_equator() {
        let phi = 30.0_f64.to_radians();
        let (t, u) = (phi.psin(), phi.pcos()); // 0.5, √3/2
        let p = normalized_legendre(2, t);
        assert!((p[1][0] - 3.0_f64.sqrt() * t).abs() < 1e-12);
        assert!((p[1][1] - 3.0_f64.sqrt() * u).abs() < 1e-12);
        assert!((p[2][0] - (5.0_f64.sqrt() / 2.0) * (3.0 * t * t - 1.0)).abs() < 1e-12);
        assert!((p[2][1] - 15.0_f64.sqrt() * t * u).abs() < 1e-12);
        assert!((p[2][2] - (15.0_f64.sqrt() / 2.0) * u * u).abs() < 1e-12);
    }

    #[test]
    fn single_term_anomaly_matches_a_hand_derived_value() {
        // Only C̄₂₂ = 1e-6; at the equator (φ = 0), λ = 0:
        //   Δg = (GM/R²)·(2−1)·C̄₂₂·cos(0)·P̄₂₂(0)
        //      = 9.798286 m/s² · 1e-6 · (√15/2)
        //      = 9.798286 · 1.9364917e-6  = 1.89744e-5 m/s²  = 1.89744 mGal.
        let mut model = GravityAnomalyModel::new(2);
        model.set_coeff(2, 2, 1.0e-6, 0.0);
        let dg = model.anomaly_mgal(0.0, 0.0);
        let gm_over_r2 = GM_EARTH / (R_EARTH * R_EARTH);
        let expected = gm_over_r2 * 1.0e-6 * (15.0_f64.sqrt() / 2.0) / MGAL;
        assert!((expected - 1.897_44).abs() < 1e-3, "hand value {expected}");
        assert!((dg - expected).abs() < 1e-9, "dg = {dg}");

        // The S̄₂₂ term rotates the pattern by 45°/m in longitude: at λ = 45° the cos(2λ)
        // term vanishes, so a pure C̄₂₂ field is zero there.
        let dg_quarter = model.anomaly_mgal(0.0, 45.0_f64.to_radians());
        assert!(dg_quarter.abs() < 1e-9, "dg(45°) = {dg_quarter}");
    }

    #[test]
    fn mascon_adds_its_peak_at_its_centre() {
        let mut model = GravityAnomalyModel::new(2);
        model.add_mascon(Mascon {
            lat_deg: 10.0,
            lon_deg: 20.0,
            amp_mgal: 30.0,
            sigma_deg: 0.4,
        });
        // At the centre the Gaussian is exactly its amplitude (field has no SH part here).
        let at_centre = model.anomaly_mgal(10.0_f64.to_radians(), 20.0_f64.to_radians());
        assert!((at_centre - 30.0).abs() < 1e-9, "centre = {at_centre}");
        // One σ away (north) it falls to amp·e^{−1/2}.
        let one_sigma = model.anomaly_mgal((10.4_f64).to_radians(), 20.0_f64.to_radians());
        assert!(
            (one_sigma - 30.0 * (-0.5_f64).pexp()).abs() < 1e-6,
            "1σ = {one_sigma}"
        );
    }

    #[test]
    fn gravimeter_white_noise_averages_down_as_asd_over_sqrt_tau() {
        // ASD 1e-7 m/s²/√Hz (≈10 µGal/√Hz, cold-atom grade), τ = 100 s ⇒
        //   σ = 1e-7/√100 = 1e-8 m/s² = 1e-3 mGal.
        let g = Gravimeter::new(1.0e-7, 0.0);
        assert!((g.measurement_sigma_mgal(100.0) - 1.0e-3).abs() < 1e-12);
        // Quadrupling the averaging time halves the noise.
        assert!((g.measurement_sigma_mgal(400.0) - 0.5e-3).abs() < 1e-12);
    }

    #[test]
    fn gravimeter_derives_its_floor_from_the_cai_physics() {
        // A CAI accelerometer's ASD must flow straight through to the gravimeter floor.
        let cai = CaiAccelerometer {
            wavelength_m: 780.0e-9,
            pulse_sep_t: 0.01,
            atom_number: 1.0e6,
            contrast: 0.5,
            cycle_time_s: 0.5,
        };
        let g = Gravimeter::from_cai(&cai, 0.0);
        let tau = 10.0_f64;
        let expected = (cai.accel_asd() / tau.sqrt()) / MGAL;
        assert!((g.measurement_sigma_mgal(tau) - expected).abs() < 1e-15);
        assert!(g.measurement_sigma_mgal(tau) > 0.0);
    }

    #[test]
    fn map_matching_recovers_the_track_far_better_than_free_inertial_drift() {
        // A distinctive field: a gentle degree-2/3 regional trend plus two mascons the
        // oblique track flies across. The true INS drift is ≈ 71 km; the matched solution
        // must cut that by a large factor.
        let cfg = GravityMapBenchmarkCfg {
            nmax: 3,
            coeffs: vec![
                CoeffEntry {
                    n: 2,
                    m: 0,
                    cbar: 4.0e-6,
                    sbar: 0.0,
                },
                CoeffEntry {
                    n: 3,
                    m: 1,
                    cbar: 2.0e-6,
                    sbar: 1.0e-6,
                },
            ],
            mascons: vec![
                Mascon {
                    lat_deg: 11.0,
                    lon_deg: 21.0,
                    amp_mgal: 30.0,
                    sigma_deg: 0.45,
                },
                Mascon {
                    lat_deg: 10.6,
                    lon_deg: 20.6,
                    amp_mgal: -25.0,
                    sigma_deg: 0.35,
                },
            ],
            start_lat_deg: 10.0,
            start_lon_deg: 20.0,
            step_lat_deg: 0.18,
            step_lon_deg: 0.13,
            waypoints: 12,
            drift_lat_deg: 0.52,
            drift_lon_deg: -0.41,
            gravimeter_asd: 1.0e-7,
            averaging_time_s: 100.0,
            map_sigma_mgal: 2.0,
            search_half_deg: 1.0,
            search_step_deg: 0.05,
            refine_stages: 1,
            refine_factor: 8.0,
            noise_seed: 0,
            reference: None,
            map_error_mc: None,
        };
        let r = run_gravity_map_benchmark(&cfg);

        // Drift of ≈ 71 km (0.52° N, 0.41° E at ~11° lat).
        assert!(
            (r.free_inertial_drift_m - 71_000.0).abs() < 3_000.0,
            "drift = {} m",
            r.free_inertial_drift_m
        );
        // Gravity-map matching cuts the error by at least 5×, to within a few km.
        assert!(
            r.map_matched_error_m < 0.2 * r.free_inertial_drift_m,
            "matched {} m vs drift {} m",
            r.map_matched_error_m,
            r.free_inertial_drift_m
        );
        assert!(
            r.map_matched_error_m < 8_000.0,
            "matched error {} m should be within grid resolution",
            r.map_matched_error_m
        );
        assert!(r.measurement_sigma_mgal > 0.0 && r.measurement_sigma_mgal.is_finite());
    }

    /// The committed 60-minute GPS-denied scenario (single source of truth for the params).
    fn gps_denied_cfg() -> GravityMapBenchmarkCfg {
        toml::from_str(include_str!("../scenarios/gps-denied-gravity-nav.toml"))
            .expect("60-min GPS-denied scenario parses")
    }

    #[test]
    fn scenario_runner_without_options_is_bit_identical_to_the_benchmark() {
        // `run_gravity_map_scenario` with neither `reference` nor `map_error_mc` must give
        // exactly the numbers of `run_gps_denied_gravity_nav`, bit for bit.
        let cfg = gps_denied_cfg();
        let a = run_gps_denied_gravity_nav(&cfg);
        let b = run_gravity_map_scenario(&cfg).expect("runs");
        assert!(b.reference.is_none() && b.map_error_mc.is_none());
        let bits = |r: &GravityMapNavResult| {
            [
                r.free_inertial_drift_m.to_bits(),
                r.map_matched_error_m.to_bits(),
                r.measurement_sigma_mgal.to_bits(),
            ]
        };
        assert_eq!(bits(&a), bits(&b.nav));
    }

    #[test]
    fn gps_denied_60min_recovers_position_within_500m() {
        // The headline alt-PNT result: one hour with no GNSS, the inertial solution drifts
        // to ≈ 70 km, and hierarchical gravity-map matching pulls it back under 500 m.
        let r = run_gps_denied_gravity_nav(&gps_denied_cfg());
        assert!(
            r.free_inertial_drift_m > 10_000.0,
            "free-inertial drift {} m must exceed 10 km",
            r.free_inertial_drift_m
        );
        assert!(
            r.map_matched_error_m < 500.0,
            "matched error {} m must beat the 500 m GPS-denied target",
            r.map_matched_error_m
        );
        // A genuine fix, not a marginal trim: at least a 100× cut over free-inertial drift.
        assert!(
            r.map_matched_error_m < r.free_inertial_drift_m / 100.0,
            "matched {} m vs drift {} m",
            r.map_matched_error_m,
            r.free_inertial_drift_m
        );
        assert!(r.measurement_sigma_mgal > 0.0 && r.measurement_sigma_mgal.is_finite());
    }

    #[test]
    fn hierarchical_refinement_is_what_breaks_the_500m_barrier() {
        // A single coarse grid (search_step ≈ 0.08° ≈ 9 km) cannot reach 500 m; the
        // coarse-to-fine refinement is what buys the sub-grid resolution. Same field, same
        // noise — only the stage count differs.
        let mut single = gps_denied_cfg();
        single.refine_stages = 1;
        let coarse = run_gps_denied_gravity_nav(&single);
        let refined = run_gps_denied_gravity_nav(&gps_denied_cfg());

        assert!(
            coarse.map_matched_error_m > 500.0,
            "single-stage {} m should NOT already meet the target",
            coarse.map_matched_error_m
        );
        assert!(refined.map_matched_error_m < 500.0);
        assert!(
            refined.map_matched_error_m < coarse.map_matched_error_m / 4.0,
            "refinement {} m must sharply beat single-stage {} m",
            refined.map_matched_error_m,
            coarse.map_matched_error_m
        );
    }

    #[test]
    fn gps_denied_recovery_is_stable_across_noise_realisations() {
        // The seeded gravimeter noise is reproducible per seed; across realisations the
        // matched error stays well under target and barely moves (the cold-atom floor is
        // far below the map error, so geometry — not noise — sets the result).
        let mut errs = Vec::new();
        for seed in 1..=5u64 {
            let mut cfg = gps_denied_cfg();
            cfg.noise_seed = seed;
            let r = run_gps_denied_gravity_nav(&cfg);
            assert!(
                r.map_matched_error_m < 500.0,
                "seed {seed}: matched {} m",
                r.map_matched_error_m
            );
            errs.push(r.map_matched_error_m);
        }
        let max = errs.iter().cloned().fold(f64::MIN, f64::max);
        let min = errs.iter().cloned().fold(f64::MAX, f64::min);
        assert!(max - min < 50.0, "spread {} m across seeds", max - min);
    }

    #[test]
    fn run_is_deterministic_for_a_fixed_seed() {
        let a = run_gps_denied_gravity_nav(&gps_denied_cfg());
        let b = run_gps_denied_gravity_nav(&gps_denied_cfg());
        assert_eq!(
            a.map_matched_error_m.to_bits(),
            b.map_matched_error_m.to_bits()
        );
    }

    #[test]
    fn committed_benchmark_scenario_loads_and_runs() {
        // The committed NAVISP benchmark spec parses and drives the same recovery.
        let cfg: GravityMapBenchmarkCfg =
            toml::from_str(include_str!("../scenarios/gravity-map-nav.toml"))
                .expect("benchmark scenario parses");
        let r = run_gravity_map_benchmark(&cfg);
        assert!(r.map_matched_error_m < 0.25 * r.free_inertial_drift_m);
        assert!(r.free_inertial_drift_m > 10_000.0);
    }
}