astrodynamics-gnss 0.14.0

//! Sequential RTK baseline filter — state ABI (slice 1+2 of the kernel migration).
//!
//! [`FilterState`] is the serializable value type a streaming processor owns: it
//! carries the baseline, the single-difference float ambiguities, the information
//! matrix (inverse covariance), and the held integer ambiguities epoch-to-epoch.
//! The eventual hot-path entry point is a pure `update(state, epoch) -> (state',
//! solution)`; this module establishes the state type and its prior-seeded,
//! dynamically-grown ambiguity columns. The measurement-update numerics (DD
//! assembly, information-form update, LAMBDA fix-and-hold) land on top of it.
//!
//! Parity reference: `Orbis.GNSS.RTK.run_sequential_baseline_filter` /
//! `sequential_initial_information`. The prior is a diagonal information matrix —
//! `1/σ²` on the baseline x/y/z and on each ambiguity diagonal, zero cross-terms.
//! Adding an ambiguity column on first sighting (a streaming filter cannot see
//! the future) is mathematically identical to the Elixir batch pre-sizing every
//! column at epoch 0: an unobserved ambiguity contributes no measurement
//! information, so it carries only its prior.

use std::collections::BTreeMap;

/// Schema version of the serialized filter state. Bump on any layout change so a
/// keyed stream processor can migrate persisted state.
///
/// v3: `reference_sat: String` became `references: BTreeMap<String, String>`
/// (per-system double-difference references, keyed by constellation letter).
pub const FILTER_STATE_VERSION: u16 = 3;

/// Sequential RTK baseline-filter state. The column order is the ABI: indices
/// 0..3 of the information matrix are baseline x/y/z, then one column per id in
/// `sd_ambiguity_ids`.
///
/// ABI forward-compatibility (decide-before-calcify; [`FILTER_STATE_VERSION`] is
/// the migration mechanism, and the update loop must keep these assumptions
/// LOCALIZED so they can be widened without a rewrite):
/// - **Multi-constellation:** DONE (v3). Double differences form within a GNSS
///   against that system's own reference; `references` carries one DD reference
///   per constellation letter. A single-entry map is the historical
///   single-system filter, bit-for-bit.
/// - **Kinematic (process noise):** baseline indices `0..3` may grow to `0..6`
///   (position+velocity). A time update with process noise is not expressible by
///   leaving the prior information unchanged — it runs in covariance (or
///   square-root) space (`Λ⁻¹ + Q`, re-invert), so the update loop must not
///   assume the carried information is propagated forward untouched.
#[derive(Debug, Clone, PartialEq)]
pub struct FilterState {
    /// Serialization schema version.
    pub version: u16,
    /// Per-system double-difference reference single-difference ambiguity ids,
    /// keyed by constellation letter (`"G" -> "G04"`). Each non-reference
    /// satellite differences against its own system's reference; there are no
    /// cross-system double differences.
    pub references: BTreeMap<String, String>,
    /// Single-difference ambiguity ids, in information-matrix column order.
    pub sd_ambiguity_ids: Vec<String>,
    /// Baseline estimate (metres, ECEF delta rover - base).
    pub baseline_m: [f64; 3],
    /// Float single-difference ambiguities (metres), parallel to `sd_ambiguity_ids`.
    pub sd_ambiguities_m: Vec<f64>,
    /// Row-major `n x n` information matrix, `n = 3 + sd_ambiguity_ids.len()`.
    pub information: Vec<f64>,
    /// Prior sigma (metres) applied to each new ambiguity column's diagonal.
    pub ambiguity_prior_sigma_m: f64,
    /// Number of epochs already incorporated into this state. This is the
    /// process-noise sentinel: pre-sized ambiguity columns mean "no ambiguity
    /// columns" no longer identifies the first epoch.
    pub epoch_count: usize,
    /// Held fixed integer double-difference ambiguities (id -> cycles).
    pub fixed_cycles: BTreeMap<String, i64>,
    /// Held fixed double-difference ambiguities (id -> metres). Redundant with
    /// `fixed_cycles` (metres = cycles·λ + offset); both are carried to mirror
    /// the Elixir filter state for 1:1 parity tracing. The metres form is the
    /// derived one — keep them consistent on every hold.
    pub fixed_m: BTreeMap<String, f64>,
}

impl FilterState {
    /// New filter seeded with a baseline guess and diagonal priors (mirrors the
    /// Elixir `sequential_initial_information`). `references` maps each
    /// constellation letter to its reference single-difference ambiguity id.
    pub fn new(
        references: BTreeMap<String, String>,
        baseline_m: [f64; 3],
        baseline_prior_sigma_m: f64,
        ambiguity_prior_sigma_m: f64,
    ) -> Self {
        let b = 1.0 / (baseline_prior_sigma_m * baseline_prior_sigma_m);
        let information = vec![b, 0.0, 0.0, 0.0, b, 0.0, 0.0, 0.0, b];
        Self {
            version: FILTER_STATE_VERSION,
            references,
            sd_ambiguity_ids: Vec::new(),
            baseline_m,
            sd_ambiguities_m: Vec::new(),
            information,
            ambiguity_prior_sigma_m,
            epoch_count: 0,
            fixed_cycles: BTreeMap::new(),
            fixed_m: BTreeMap::new(),
        }
    }

    /// State dimension `n = 3 + number of single-difference ambiguities`.
    pub fn dim(&self) -> usize {
        3 + self.sd_ambiguity_ids.len()
    }

    /// Information-matrix column index of an ambiguity id (`3 + position`), if tracked.
    pub fn index_of(&self, id: &str) -> Option<usize> {
        self.sd_ambiguity_ids
            .iter()
            .position(|x| x == id)
            .map(|p| 3 + p)
    }

    /// Read `information[i][j]`.
    pub fn info(&self, i: usize, j: usize) -> f64 {
        let n = self.dim();
        self.information[i * n + j]
    }

    /// Add a single-difference ambiguity column if absent, seeded with `initial_m`
    /// and the prior (`1/σ²` on its new diagonal, zero cross-terms). No-op if the
    /// id is already tracked.
    pub fn ensure_ambiguity(&mut self, id: &str, initial_m: f64) {
        if self.index_of(id).is_some() {
            return;
        }
        let old_n = self.dim();
        let new_n = old_n + 1;
        let mut grown = vec![0.0f64; new_n * new_n];
        for i in 0..old_n {
            for j in 0..old_n {
                grown[i * new_n + j] = self.information[i * old_n + j];
            }
        }
        let prior = 1.0 / (self.ambiguity_prior_sigma_m * self.ambiguity_prior_sigma_m);
        grown[(new_n - 1) * new_n + (new_n - 1)] = prior;
        self.information = grown;
        self.sd_ambiguity_ids.push(id.to_string());
        self.sd_ambiguities_m.push(initial_m);
    }
}

/// Fold one weighted measurement into an information system in place:
/// `Λ += weight·hhᵀ`, `η += weight·h·y`. `lambda` is the row-major `n x n`
/// information matrix, `eta` the length-`n` information vector, `h` the length-`n`
/// measurement row. This is the normal-equations accumulation shared by the
/// hold pseudo-measurements and single-row tests. Double-difference measurement
/// rows must use the block covariance fold path so shared-reference covariance
/// is preserved.
pub fn fold_measurement(lambda: &mut [f64], eta: &mut [f64], h: &[f64], weight: f64, y: f64) {
    let n = eta.len();
    for i in 0..n {
        let whi = weight * h[i];
        eta[i] += whi * y;
        let row = i * n;
        for j in 0..n {
            lambda[row + j] += whi * h[j];
        }
    }
}

// Op-for-op port of the Elixir `double_difference_inverse_covariance`
// (lib/orbis/gnss/rtk.ex): the equal-variance closed form when every row shares
// the first row's single-difference variance, otherwise the dense correlated
// covariance `R = D + σ_ref²·11ᵀ` inverted through the bit-identical
// `ils::invert`. NOT a Sherman-Morrison structured inverse — the kernel must
// reproduce the reference's exact floating-point order for the 0-ULP trace gate.
#[cfg(test)]
fn double_difference_inverse_covariance(rows: &[&DdRow]) -> Option<Vec<f64>> {
    let mut scratch = FoldBlockScratch::default();
    double_difference_inverse_covariance_into(rows, &mut scratch)?;
    Some(scratch.r_inv.clone())
}

#[cfg(test)]
#[allow(clippy::needless_range_loop)]
fn double_difference_inverse_covariance_into(
    rows: &[&DdRow],
    scratch: &mut FoldBlockScratch,
) -> Option<()> {
    let m = rows.len();
    if m == 0 {
        scratch.r_inv.clear();
        return Some(());
    }

    let first = rows[0].sd_variance_m2;
    let constant = rows
        .iter()
        .all(|r| r.sd_variance_m2 == first && r.ref_sd_variance_m2 == first);

    if constant {
        // Elixir `equal_double_difference_inverse_covariance/2`, exact op order:
        //   diagonal_scale = 1.0 / sd_var * (1.0 - 1.0 / (m + 1.0))
        //   off_diagonal   = -1.0 / (sd_var * (m + 1.0))
        let mf = m as f64;
        let diagonal_scale = 1.0 / first * (1.0 - 1.0 / (mf + 1.0));
        let off_diagonal = -1.0 / (first * (mf + 1.0));
        scratch.r_inv.resize(m * m, 0.0);
        for i in 0..m {
            for j in 0..m {
                scratch.r_inv[i * m + j] = if i == j { diagonal_scale } else { off_diagonal };
            }
        }
        Some(())
    } else {
        // Elixir `double_difference_covariance/1` + `invert_matrix/1`, but flat
        // and allocation-light. The Gaussian-elimination order mirrors
        // `ils::solve_linear` / `LinearAlgebra.solve_linear`.
        let ref_variance = rows[0].ref_sd_variance_m2;
        scratch.cov.resize(m * m, 0.0);
        for i in 0..m {
            for j in 0..m {
                scratch.cov[i * m + j] = if i == j {
                    rows[i].sd_variance_m2 + ref_variance
                } else {
                    ref_variance
                };
            }
        }
        invert_flat_into(&scratch.cov, m, &mut scratch.r_inv, &mut scratch.invert)
    }
}

fn invert_flat_into(
    a: &[f64],
    n: usize,
    out: &mut Vec<f64>,
    scratch: &mut InvertFlatScratch,
) -> Option<()> {
    out.resize(n * n, 0.0);
    scratch.rows.resize(n * (n + 1), 0.0);
    scratch.x.resize(n, 0.0);

    for col in 0..n {
        for i in 0..n {
            let src = i * n;
            let dst = i * (n + 1);
            scratch.rows[dst..(dst + n)].copy_from_slice(&a[src..(src + n)]);
            scratch.rows[dst + n] = if i == col { 1.0 } else { 0.0 };
        }
        solve_augmented_flat(&mut scratch.rows, n, &mut scratch.x)?;
        for i in 0..n {
            out[i * n + col] = scratch.x[i];
        }
    }

    Some(())
}

fn solve_augmented_flat(rows: &mut [f64], n: usize, x: &mut [f64]) -> Option<()> {
    let stride = n + 1;

    for col in 0..n {
        let mut pivot_row = col;
        let mut pivot_abs = rows[col * stride + col].abs();
        for idx in (col + 1)..n {
            let v = rows[idx * stride + col].abs();
            if v > pivot_abs {
                pivot_abs = v;
                pivot_row = idx;
            }
        }
        if pivot_abs <= 1.0e-12 {
            return None;
        }
        if pivot_row != col {
            for j in 0..=n {
                rows.swap(col * stride + j, pivot_row * stride + j);
            }
        }

        let pivot_value = rows[col * stride + col];
        for idx in (col + 1)..n {
            let factor = rows[idx * stride + col] / pivot_value;
            for j in 0..=n {
                rows[idx * stride + j] -= factor * rows[col * stride + j];
            }
        }
    }

    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += rows[i * stride + j] * x[j];
        }
        x[i] = (rows[i * stride + n] - known) / rows[i * stride + i];
    }

    Some(())
}

/// Fold one epoch/kind block of double-difference rows with the full correlated
/// measurement covariance. Rows in a block share the reference satellite's
/// single-difference variance, so `R = D + σ_ref²·11ᵀ`, not a diagonal matrix.
#[cfg(test)]
fn fold_measurement_block(lambda: &mut [f64], eta: &mut [f64], rows: &[&DdRow]) -> Option<()> {
    let mut scratch = FoldBlockScratch::default();
    fold_measurement_block_with_scratch(lambda, eta, rows, &mut scratch)
}

#[cfg(test)]
#[allow(clippy::needless_range_loop)]
fn fold_measurement_block_with_scratch(
    lambda: &mut [f64],
    eta: &mut [f64],
    rows: &[&DdRow],
    scratch: &mut FoldBlockScratch,
) -> Option<()> {
    if rows.is_empty() {
        return Some(());
    }

    let n = eta.len();
    let m = rows.len();
    double_difference_inverse_covariance_into(rows, scratch)?;
    let r_inv = &scratch.r_inv;

    // Op-for-op with the Elixir `block_normal_equations`:
    //   r_inv_y[a] = Σ_b R⁻¹[a][b]·y_b
    //   Λ[i][j]   += Σ_a h_a[i]·(Σ_b R⁻¹[a][b]·h_b[j])   (b-sum grouped first)
    //   η[i]      += Σ_a h_a[i]·r_inv_y[a]
    // The grouping (inner b-sum, then ·h_a[i], then accumulate over a) must match
    // exactly — the previous per-(a,b) form rounds differently.
    scratch.r_inv_y.resize(m, 0.0);
    for (a, rinvy_a) in scratch.r_inv_y.iter_mut().enumerate() {
        let mut s = 0.0;
        for b in 0..m {
            s += r_inv[a * m + b] * rows[b].y;
        }
        *rinvy_a = s;
    }

    for i in 0..n {
        let row = i * n;
        for j in 0..n {
            let mut acc = 0.0;
            for a in 0..m {
                let hi = rows[a].h[i];
                let mut row_sum = 0.0;
                for b in 0..m {
                    row_sum += r_inv[a * m + b] * rows[b].h[j];
                }
                acc += hi * row_sum;
            }
            lambda[row + j] += acc;
        }
    }

    for (i, e) in eta.iter_mut().enumerate() {
        let mut acc = 0.0;
        for a in 0..m {
            acc += rows[a].h[i] * scratch.r_inv_y[a];
        }
        *e += acc;
    }

    Some(())
}

#[allow(clippy::needless_range_loop)]
fn double_difference_inverse_covariance_indices(
    rows: &[DdRowScratch],
    indices: &[usize],
    scratch: &mut FoldBlockScratch,
) -> Option<()> {
    let m = indices.len();
    if m == 0 {
        scratch.r_inv.clear();
        return Some(());
    }

    let first = rows[indices[0]].sd_variance_m2;
    let constant = indices
        .iter()
        .all(|&idx| rows[idx].sd_variance_m2 == first && rows[idx].ref_sd_variance_m2 == first);

    if constant {
        let mf = m as f64;
        let diagonal_scale = 1.0 / first * (1.0 - 1.0 / (mf + 1.0));
        let off_diagonal = -1.0 / (first * (mf + 1.0));
        scratch.r_inv.resize(m * m, 0.0);
        for i in 0..m {
            for j in 0..m {
                scratch.r_inv[i * m + j] = if i == j { diagonal_scale } else { off_diagonal };
            }
        }
        Some(())
    } else {
        let ref_variance = rows[indices[0]].ref_sd_variance_m2;
        scratch.cov.resize(m * m, 0.0);
        for i in 0..m {
            for j in 0..m {
                scratch.cov[i * m + j] = if i == j {
                    rows[indices[i]].sd_variance_m2 + ref_variance
                } else {
                    ref_variance
                };
            }
        }
        invert_flat_into(&scratch.cov, m, &mut scratch.r_inv, &mut scratch.invert)
    }
}

#[allow(clippy::needless_range_loop)]
fn fold_measurement_block_indices(
    lambda: &mut [f64],
    eta: &mut [f64],
    rows: &[DdRowScratch],
    indices: &[usize],
    scratch: &mut FoldBlockScratch,
) -> Option<()> {
    if indices.is_empty() {
        return Some(());
    }

    let n = eta.len();
    let m = indices.len();
    double_difference_inverse_covariance_indices(rows, indices, scratch)?;
    let r_inv = &scratch.r_inv;

    scratch.r_inv_y.resize(m, 0.0);
    for (a, rinvy_a) in scratch.r_inv_y.iter_mut().enumerate() {
        let mut s = 0.0;
        for b in 0..m {
            s += r_inv[a * m + b] * rows[indices[b]].y;
        }
        *rinvy_a = s;
    }

    for i in 0..n {
        let row = i * n;
        for j in 0..n {
            let mut acc = 0.0;
            for a in 0..m {
                let hi = rows[indices[a]].h[i];
                let mut row_sum = 0.0;
                for b in 0..m {
                    row_sum += r_inv[a * m + b] * rows[indices[b]].h[j];
                }
                acc += hi * row_sum;
            }
            lambda[row + j] += acc;
        }
    }

    for (i, e) in eta.iter_mut().enumerate() {
        let mut acc = 0.0;
        for a in 0..m {
            acc += rows[indices[a]].h[i] * scratch.r_inv_y[a];
        }
        *e += acc;
    }

    Some(())
}

fn fold_hold_block_with_ambiguities(
    lambda: &mut [f64],
    eta: &mut [f64],
    state: &FilterState,
    sd_ambiguities_m: &[f64],
    held: &[Hold],
    hold_weight: f64,
) -> Option<()> {
    let n = state.dim();

    // Op-for-op with Elixir `sequential_hold_normal_equations/5`: build the hold
    // normal-equation block separately, using `weighted_h = h * weight`, then
    // accumulate `hi * weighted_hj`. Do not call `fold_measurement`: its
    // `(weight * hi) * hj` grouping is algebraically equivalent but not 0-ULP
    // equivalent, and Elixir adds the hold block after `(prior + measurement)`.
    for h in held {
        let sp = state.ambiguity_pos(&h.sat_sd_id)?;
        let rp = state.ambiguity_pos(&h.ref_sd_id)?;
        let sp_col = 3 + sp;
        let rp_col = 3 + rp;
        let current_dd = sd_ambiguities_m[sp] - sd_ambiguities_m[rp];
        let residual = h.fixed_m - current_dd;

        for i in 0..n {
            let hi = hold_design_value(i, sp_col, rp_col);
            let offset = i * n;
            for j in 0..n {
                let weighted_hj = hold_design_value(j, sp_col, rp_col) * hold_weight;
                lambda[offset + j] += hi * weighted_hj;
            }
        }

        for (i, eta_i) in eta.iter_mut().enumerate().take(n) {
            let weighted_hi = hold_design_value(i, sp_col, rp_col) * hold_weight;
            *eta_i += residual * weighted_hi;
        }
    }

    Some(())
}

fn hold_design_value(i: usize, sat_col: usize, ref_col: usize) -> f64 {
    if i == sat_col {
        1.0
    } else if i == ref_col {
        -1.0
    } else {
        0.0
    }
}

/// Solve the information system `Λ x = η` for the state correction, using flat
/// row-major Gaussian elimination (partial pivoting, singular guard). Returns
/// `None` if `Λ` is singular. `lambda` is row-major `n x n`.
pub fn solve_normal(lambda: &[f64], eta: &[f64]) -> Option<Vec<f64>> {
    let mut scratch = SolveNormalScratch::default();
    solve_normal_into(lambda, eta, &mut scratch).map(|x| x.to_vec())
}

fn solve_normal_into<'a>(
    lambda: &[f64],
    eta: &[f64],
    scratch: &'a mut SolveNormalScratch,
) -> Option<&'a [f64]> {
    let n = eta.len();
    if lambda.len() != n * n {
        return None;
    }

    scratch.a.resize(n * n, 0.0);
    scratch.a.copy_from_slice(lambda);
    scratch.b.resize(n, 0.0);
    scratch.b.copy_from_slice(eta);

    for k in 0..n {
        let mut pivot = k;
        let mut pivot_abs = scratch.a[k * n + k].abs();
        for i in (k + 1)..n {
            let candidate = scratch.a[i * n + k].abs();
            if candidate > pivot_abs {
                pivot = i;
                pivot_abs = candidate;
            }
        }
        // `<=` matches the bit-pinned ILS/Elixir singular guard (`@pivot_epsilon`).
        if pivot_abs <= 1.0e-12 {
            return None;
        }
        if pivot != k {
            for j in 0..n {
                scratch.a.swap(k * n + j, pivot * n + j);
            }
            scratch.b.swap(k, pivot);
        }

        let diag = scratch.a[k * n + k];
        for i in (k + 1)..n {
            let factor = scratch.a[i * n + k] / diag;
            scratch.a[i * n + k] = 0.0;
            for j in (k + 1)..n {
                scratch.a[i * n + j] -= factor * scratch.a[k * n + j];
            }
            scratch.b[i] -= factor * scratch.b[k];
        }
    }

    // Back-substitution in `ils::solve_linear`'s exact order: sum the known terms
    // first, THEN subtract from the rhs — `(b - Σ aᵢⱼ·xⱼ) / aᵢᵢ`, NOT a running
    // `rhs -= aᵢⱼ·xⱼ`. The two round differently; the 0-ULP gate requires this.
    scratch.x.resize(n, 0.0);
    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += scratch.a[i * n + j] * scratch.x[j];
        }
        scratch.x[i] = (scratch.b[i] - known) / scratch.a[i * n + i];
    }
    Some(&scratch.x)
}

// =========================================================================
// Double-difference measurement model (kernel slice 2a)
// -------------------------------------------------------------------------
// Port of the Elixir `build_epoch_sequential_baseline_rows` +
// `geometry_double_difference` + `design_*_baseline_row`. Per non-reference
// satellite it produces a code row and a phase row for the filter normal
// equations, linearized at the current baseline estimate (state.baseline_m).
//
// Geometry (matches the Elixir verbatim, RTKLIB first-order Sagnac):
//   range(sat, recv)       = |sat - recv|
//   geometric_range(...)   = range + ωe·(sx·ry - sy·rx)/c   (Sagnac, if enabled)
//   range_derivative(...)  = (recv - sat)/range              (LOS unit vector)
//   SD(sat)  = grange(sat_rover, rover) - grange(sat_base, base)
//   DD       = SD(sat) - SD(ref)
//   deriv_DD = LOS(rover->sat) - LOS(rover->ref)             (Euclidean, no Sagnac)
//   y_code   = obs_DD.code  - DD
//   y_phase  = obs_DD.phase - (DD + ambiguity_DD)
// where ambiguity_DD = state SD ambiguity (sat) - state SD ambiguity (ref).

use crate::constants::{C_M_S, OMEGA_E_DOT_RAD_S};

/// Code vs carrier-phase double-difference row. The `Ord` is the covariance
/// block sort order and must keep `Code < Phase` (Elixir sorts the block key
/// `{epoch, kind, ref}` with the atoms `:code < :phase`).
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, PartialOrd, Ord)]
pub enum RowKind {
    #[default]
    Code,
    Phase,
}

/// One satellite's base+rover code/phase observations and per-receiver
/// transmit-time ECEF positions at an epoch.
#[derive(Debug, Clone, PartialEq)]
pub struct SatMeas {
    pub sat: String,
    /// Single-difference ambiguity id (gap-segmentation already applied upstream).
    pub sd_ambiguity_id: String,
    pub base_code_m: f64,
    pub base_phase_m: f64,
    pub rover_code_m: f64,
    pub rover_phase_m: f64,
    /// Transmit-time satellite ECEF position (metres) for the base receiver.
    pub base_tx_pos: [f64; 3],
    /// Transmit-time satellite ECEF position (metres) for the rover receiver.
    pub rover_tx_pos: [f64; 3],
    /// Shared receive-time satellite ECEF position (metres). Used only for the
    /// elevation-dependent variance models: the Elixir reference computes
    /// elevation from `epoch.positions` (the shared map), NOT the per-receiver
    /// transmit-time maps. For synthetic fixtures this may equal the tx position.
    pub pos: [f64; 3],
}

/// One RTK epoch: the per-system double-difference reference satellites present
/// this epoch plus the non-reference satellites observed at both receivers.
/// Each non-reference satellite differences against the reference of its own
/// system (the first byte of the satellite id); a one-element `references` is
/// the historical single-system epoch.
#[derive(Debug, Clone, PartialEq)]
pub struct Epoch {
    pub references: Vec<SatMeas>,
    pub nonref: Vec<SatMeas>,
    /// Optional rover ECEF velocity in metres/second for the velocity-propagated
    /// predict branch. The value is an epoch input, not carried filter state.
    pub velocity_mps: Option<[f64; 3]>,
    /// Elapsed seconds since the previous filter epoch, used only when
    /// [`UpdateOpts::dynamics_model`] is [`DynamicsModel::VelocityPropagated`].
    pub dt_s: f64,
}

/// Constellation letter of a satellite (or single-difference ambiguity) id —
/// its first byte, mirroring the Elixir `satellite_system/1` (`String.first`).
fn satellite_system(id: &str) -> &str {
    id.get(..1).unwrap_or("")
}

/// Single-difference variance model, mirroring the Elixir `weights` options
/// (`stochastic_model` / `elevation_weighting`).
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum StochasticModel {
    /// `SD variance = 2σ²`; with `elevation_weighting`, σ is first scaled by
    /// `1/max(sin el, MIN_ELEVATION_SIN)`.
    Simple { elevation_weighting: bool },
    /// RTKLIB floor-plus-elevation: `SD variance = 2(σ² + σ²/sin²el)` with the
    /// same clamped `sin el`.
    Rtklib,
}

/// Floor on `sin(elevation)` in the variance models (Elixir `@min_elevation_sin`).
pub const MIN_ELEVATION_SIN: f64 = 0.05;

/// Measurement-model configuration.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct MeasModel {
    pub code_sigma_m: f64,
    pub phase_sigma_m: f64,
    pub sagnac: bool,
    pub stochastic: StochasticModel,
}

/// Selected receiver-antenna calibration for one frequency.
///
/// `noazi_pcv_m` entries are `(zenith_deg, value_m)`. `azi_pcv_m` entries are
/// `(azimuth_deg, zenith_deg, value_m)`.
#[derive(Debug, Clone, PartialEq)]
pub struct ReceiverAntennaCalibration {
    pub pco_neu_m: [f64; 3],
    pub noazi_pcv_m: Vec<(f64, f64)>,
    pub azi_pcv_m: Vec<(f64, f64, f64)>,
}

/// Base and rover receiver-antenna calibrations used by the DD row builder.
#[derive(Debug, Clone, PartialEq)]
pub struct ReceiverAntennaCorrections {
    pub base: ReceiverAntennaCalibration,
    pub rover: ReceiverAntennaCalibration,
}

/// Sine of the satellite elevation seen from `base`.
///
/// PARITY: the local-up here is GEOCENTRIC (`base/|base|`), exactly as the
/// Elixir `local_up/1` — NOT the geodetic ellipsoid normal. The two differ by
/// up to ~0.19°, which shifts low-elevation variances; do not "fix" this to
/// `geodetic_from_ecef_m` without changing the Elixir reference in lockstep.
fn elevation_sin(base: [f64; 3], sat_pos: [f64; 3]) -> f64 {
    let bn = norm3(base);
    let up = if bn > 0.0 {
        // Elixir `local_up` = `scale3(base, 1.0 / n)`: multiply by reciprocal.
        let inv = 1.0 / bn;
        [base[0] * inv, base[1] * inv, base[2] * inv]
    } else {
        [0.0, 0.0, 1.0]
    };
    let d = sub3(sat_pos, base);
    let dn = norm3(d);
    if dn > 0.0 {
        // Elixir op order: normalize the LOS first, THEN dot with up
        // (`unit3(sub3(sat, base))` = `scale3(_, 1.0 / n)`, then `dot3`).
        let inv = 1.0 / dn;
        let los = [d[0] * inv, d[1] * inv, d[2] * inv];
        los[0] * up[0] + los[1] * up[1] + los[2] * up[2]
    } else {
        -1.0
    }
}

/// Single-difference variance for one receiver-satellite pair (Elixir
/// `single_difference_variance/4`).
fn single_difference_variance(
    sigma_m: f64,
    stochastic: StochasticModel,
    base: [f64; 3],
    sat_pos: [f64; 3],
) -> f64 {
    match stochastic {
        StochasticModel::Simple {
            elevation_weighting: false,
        } => 2.0 * sigma_m * sigma_m,
        StochasticModel::Simple {
            elevation_weighting: true,
        } => {
            let sin_el = elevation_sin(base, sat_pos).max(MIN_ELEVATION_SIN);
            let scaled = sigma_m / sin_el;
            2.0 * scaled * scaled
        }
        StochasticModel::Rtklib => {
            let sin_el = elevation_sin(base, sat_pos).max(MIN_ELEVATION_SIN);
            2.0 * (sigma_m * sigma_m + sigma_m * sigma_m / (sin_el * sin_el))
        }
    }
}

/// A double-difference row: design vector `h` (length `state.dim()`), prefit
/// residual `y`, the satellite and reference single-difference variances, and
/// the diagonal information weight `1/(sd_variance + ref_sd_variance)` retained
/// for diagnostics/back-compat tests. Measurement updates use the full
/// block covariance in the epoch fold path.
#[derive(Debug, Clone, PartialEq)]
pub struct DdRow {
    pub kind: RowKind,
    pub sat: String,
    /// Reference satellite this row differenced against (its own system's
    /// reference). The covariance blocking key — rows correlate only through a
    /// shared reference single difference.
    pub ref_sat: String,
    pub sd_ambiguity_id: String,
    pub h: Vec<f64>,
    pub y: f64,
    pub sd_variance_m2: f64,
    pub ref_sd_variance_m2: f64,
    pub weight: f64,
}

#[derive(Debug, Default)]
struct DdRowScratch {
    kind: RowKind,
    sat: String,
    ref_sat: String,
    sd_ambiguity_id: String,
    h: Vec<f64>,
    y: f64,
    sd_variance_m2: f64,
    ref_sd_variance_m2: f64,
    weight: f64,
}

#[derive(Debug, Default)]
struct RefCtxScratch {
    system: String,
    sat: String,
    sd_ambiguity_id: String,
    pos: [f64; 3],
    sd_code: f64,
    sd_phase: f64,
    sd_geom: f64,
    los: [f64; 3],
    code_var: f64,
    phase_var: f64,
}

#[derive(Debug, Default)]
struct EpochRowsScratch {
    refs: Vec<RefCtxScratch>,
    refs_len: usize,
    rows: Vec<DdRowScratch>,
    rows_len: usize,
    receiver_antenna: ReceiverAntennaScratch,
}

#[derive(Debug, Default)]
struct InvertFlatScratch {
    rows: Vec<f64>,
    x: Vec<f64>,
}

#[derive(Debug, Default)]
struct FoldBlockScratch {
    r_inv: Vec<f64>,
    cov: Vec<f64>,
    r_inv_y: Vec<f64>,
    invert: InvertFlatScratch,
}

#[derive(Debug, Default)]
struct SolveNormalScratch {
    a: Vec<f64>,
    b: Vec<f64>,
    x: Vec<f64>,
}

fn assign_str(dst: &mut String, src: &str) {
    dst.clear();
    dst.push_str(src);
}

fn sub3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}

fn add3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}

fn scale3(v: [f64; 3], s: f64) -> [f64; 3] {
    [v[0] * s, v[1] * s, v[2] * s]
}

fn norm3(a: [f64; 3]) -> f64 {
    (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]).sqrt()
}

fn cross3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0],
    ]
}

fn dot3(a: [f64; 3], b: [f64; 3]) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}

fn unit3(v: [f64; 3]) -> Option<[f64; 3]> {
    match norm3(v) {
        n if n > 0.0 => Some(scale3(v, 1.0 / n)),
        _ => None,
    }
}

fn range_m(sat: [f64; 3], recv: [f64; 3]) -> f64 {
    norm3(sub3(sat, recv))
}

fn geometric_range_m(sat: [f64; 3], recv: [f64; 3], sagnac: bool) -> f64 {
    let r = range_m(sat, recv);
    if sagnac {
        r + OMEGA_E_DOT_RAD_S * (sat[0] * recv[1] - sat[1] * recv[0]) / C_M_S
    } else {
        r
    }
}

/// LOS unit vector `(recv - sat)/range` — the partial of range w.r.t. the
/// receiver position (Euclidean; Sagnac corrects the scalar range only).
fn range_derivative(recv: [f64; 3], sat: [f64; 3]) -> [f64; 3] {
    // Elixir `scale3(sub3(receiver, sat_pos), 1.0 / rho)`: multiply by reciprocal,
    // NOT component-wise division (they round differently — 0-ULP requires this).
    let rho = range_m(sat, recv);
    let inv = 1.0 / rho;
    let d = sub3(recv, sat);
    [d[0] * inv, d[1] * inv, d[2] * inv]
}

fn local_up(receiver_pos: [f64; 3]) -> [f64; 3] {
    match norm3(receiver_pos) {
        n if n > 0.0 => scale3(receiver_pos, 1.0 / n),
        _ => [0.0, 0.0, 1.0],
    }
}

fn east_unit_from_up(up: [f64; 3]) -> [f64; 3] {
    let z_axis = [0.0, 0.0, 1.0];
    let cross = cross3(z_axis, up);

    if cross == [0.0, 0.0, 0.0] {
        [1.0, 0.0, 0.0]
    } else {
        match norm3(cross) {
            n if n > 0.0 => scale3(cross, 1.0 / n),
            _ => [1.0, 0.0, 0.0],
        }
    }
}

fn north_unit_from_east_and_up(east: [f64; 3], up: [f64; 3]) -> [f64; 3] {
    unit3(cross3(east, up)).unwrap_or([0.0, 0.0, 1.0])
}

fn local_neu_basis(receiver_pos: [f64; 3]) -> ([f64; 3], [f64; 3], [f64; 3]) {
    let up = local_up(receiver_pos);
    let east = east_unit_from_up(up);
    let north = north_unit_from_east_and_up(east, up);
    (north, east, up)
}

fn los_projection(
    pco: [f64; 3],
    north_unit: [f64; 3],
    east_unit: [f64; 3],
    up_unit: [f64; 3],
    los: [f64; 3],
) -> f64 {
    let pco_ecef = add3(scale3(north_unit, pco[0]), scale3(east_unit, pco[1]));
    let pco_ecef = add3(pco_ecef, scale3(up_unit, pco[2]));
    dot3(pco_ecef, los)
}

fn los_zenith_azimuth_deg(
    los: [f64; 3],
    up: [f64; 3],
    north: [f64; 3],
    east: [f64; 3],
) -> (f64, f64) {
    let elevation_sin = dot3(los, up);
    let elevation_sin = (-1.0_f64).max(1.0_f64.min(elevation_sin));
    let zenith_deg = elevation_sin.acos() * 180.0 / std::f64::consts::PI;

    let azimuth_rad = dot3(los, east).atan2(dot3(los, north));
    let azimuth_deg = azimuth_rad * 180.0 / std::f64::consts::PI;
    let azimuth_deg = if azimuth_deg < 0.0 {
        azimuth_deg + 360.0
    } else {
        azimuth_deg
    };

    (zenith_deg, azimuth_deg)
}

fn normalize_azimuth(azimuth_deg: f64) -> f64 {
    let wrapped = azimuth_deg % 360.0;

    if wrapped < 0.0 {
        wrapped + 360.0
    } else {
        wrapped
    }
}

fn sorted_zenith_samples(samples: &[(f64, f64)], out: &mut Vec<(f64, f64)>) {
    out.clear();
    out.extend_from_slice(samples);
    out.sort_by(|a, b| a.0.partial_cmp(&b.0).unwrap_or(std::cmp::Ordering::Equal));
}

fn interpolate_samples(
    samples: &[(f64, f64)],
    zenith_deg: f64,
    scratch: &mut Vec<(f64, f64)>,
) -> Option<f64> {
    if samples.is_empty() {
        return None;
    }

    sorted_zenith_samples(samples, scratch);
    let first = scratch[0];
    let last = scratch[scratch.len() - 1];

    if zenith_deg <= first.0 {
        Some(first.1)
    } else if zenith_deg >= last.0 {
        Some(last.1)
    } else {
        let mut low = None;
        for &(z, v) in scratch.iter() {
            if z <= zenith_deg {
                low = Some((z, v));
            } else {
                break;
            }
        }

        let mut high = None;
        for &(z, v) in scratch.iter() {
            if z >= zenith_deg {
                high = Some((z, v));
                break;
            }
        }

        let (low_zen, low_value) = low?;
        let (high_zen, high_value) = high?;
        if high_zen == low_zen {
            Some(low_value)
        } else {
            let t = (zenith_deg - low_zen) / (high_zen - low_zen);
            Some(low_value + (high_value - low_value) * t)
        }
    }
}

#[derive(Debug, Default)]
struct ReceiverAntennaScratch {
    zenith_samples: Vec<(f64, f64)>,
    azimuths: Vec<f64>,
    low_samples: Vec<(f64, f64)>,
    high_samples: Vec<(f64, f64)>,
}

fn interpolate_azimuth_pcv(
    samples: &[(f64, f64, f64)],
    azimuth_deg: f64,
    zenith_deg: f64,
    scratch: &mut ReceiverAntennaScratch,
) -> Option<f64> {
    if samples.is_empty() {
        return None;
    }

    scratch.azimuths.clear();
    for &(az, _, _) in samples {
        if !scratch.azimuths.iter().any(|&existing| existing == az) {
            scratch.azimuths.push(az);
        }
    }
    scratch
        .azimuths
        .sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));

    let azimuth = normalize_azimuth(azimuth_deg);
    let first = *scratch.azimuths.first()?;
    let last = *scratch.azimuths.last()?;
    let low_deg = scratch
        .azimuths
        .iter()
        .rev()
        .find(|&&a| a <= azimuth)
        .copied();
    let high_deg = scratch.azimuths.iter().find(|&&a| a >= azimuth).copied();
    let (low_deg, high_deg) = match (low_deg, high_deg) {
        (None, _) => (last, first),
        (_, None) => (last, first),
        (Some(low), Some(high)) => (low, high),
    };

    scratch.low_samples.clear();
    scratch.high_samples.clear();
    for &(az, zen, value) in samples {
        if az == low_deg {
            scratch.low_samples.push((zen, value));
        }
        if az == high_deg {
            scratch.high_samples.push((zen, value));
        }
    }

    let low = low_deg;
    let high = if high_deg < low {
        high_deg + 360.0
    } else {
        high_deg
    };
    let target = if azimuth < low {
        azimuth + 360.0
    } else {
        azimuth
    };

    let low_value = interpolate_samples(
        &scratch.low_samples,
        zenith_deg,
        &mut scratch.zenith_samples,
    )?;
    let high_value = interpolate_samples(
        &scratch.high_samples,
        zenith_deg,
        &mut scratch.zenith_samples,
    )?;

    if high == low {
        Some(low_value)
    } else {
        let t = (target - low) / (high - low);
        Some(low_value + (high_value - low_value) * t)
    }
}

fn pcv_m(
    calibration: &ReceiverAntennaCalibration,
    zenith_deg: f64,
    azimuth_deg: f64,
    scratch: &mut ReceiverAntennaScratch,
) -> f64 {
    if calibration.azi_pcv_m.is_empty() {
        interpolate_samples(
            &calibration.noazi_pcv_m,
            zenith_deg,
            &mut scratch.zenith_samples,
        )
        .unwrap_or(0.0)
    } else {
        interpolate_azimuth_pcv(&calibration.azi_pcv_m, azimuth_deg, zenith_deg, scratch)
            .or_else(|| {
                interpolate_samples(
                    &calibration.noazi_pcv_m,
                    zenith_deg,
                    &mut scratch.zenith_samples,
                )
            })
            .unwrap_or(0.0)
    }
}

fn receiver_antenna_correction(
    sat_pos: [f64; 3],
    receiver_pos: [f64; 3],
    calibration: &ReceiverAntennaCalibration,
    scratch: &mut ReceiverAntennaScratch,
) -> f64 {
    let Some(los) = unit3(sub3(sat_pos, receiver_pos)) else {
        return 0.0;
    };
    let (north, east, up) = local_neu_basis(receiver_pos);
    let pco_projection = los_projection(calibration.pco_neu_m, north, east, up, los);
    let (zenith_deg, azimuth_deg) = los_zenith_azimuth_deg(los, up, north, east);
    let pcv = pcv_m(calibration, zenith_deg, azimuth_deg, scratch);
    pco_projection + pcv
}

fn double_difference_receiver_antenna_correction(
    sat_pos: [f64; 3],
    sat_base_pos: [f64; 3],
    sat_rover_pos: [f64; 3],
    ref_pos: [f64; 3],
    ref_base_pos: [f64; 3],
    ref_rover_pos: [f64; 3],
    corrections: Option<&ReceiverAntennaCorrections>,
    scratch: &mut ReceiverAntennaScratch,
) -> f64 {
    let Some(corrections) = corrections else {
        return 0.0;
    };

    let rover_sat_corr =
        receiver_antenna_correction(sat_pos, sat_rover_pos, &corrections.rover, scratch);
    let base_sat_corr =
        receiver_antenna_correction(sat_pos, sat_base_pos, &corrections.base, scratch);
    let rover_ref_corr =
        receiver_antenna_correction(ref_pos, ref_rover_pos, &corrections.rover, scratch);
    let base_ref_corr =
        receiver_antenna_correction(ref_pos, ref_base_pos, &corrections.base, scratch);

    rover_sat_corr - base_sat_corr - rover_ref_corr + base_ref_corr
}

impl FilterState {
    /// Position index (0..len) of an SD ambiguity within `sd_ambiguities_m`.
    fn ambiguity_pos(&self, id: &str) -> Option<usize> {
        self.sd_ambiguity_ids.iter().position(|x| x == id)
    }

    /// State double-difference ambiguity (metres): SD(sat) - SD(ref).
    fn dd_ambiguity_m(&self, sat_sd_id: &str, ref_sd_id: &str) -> Option<f64> {
        let s = self.sd_ambiguities_m[self.ambiguity_pos(sat_sd_id)?];
        let r = self.sd_ambiguities_m[self.ambiguity_pos(ref_sd_id)?];
        Some(s - r)
    }
}

/// Build the double-difference code+phase rows for an epoch, linearized at
/// `state.baseline_m`. `base` is the base-station ECEF; rover = base + baseline.
/// Each satellite contributes a code row then a phase row (Elixir order).
/// Returns `None` if a satellite's SD ambiguity column is not yet in the state
/// (caller must `ensure_ambiguity` first).
pub fn epoch_dd_rows(
    epoch: &Epoch,
    base: [f64; 3],
    state: &FilterState,
    model: &MeasModel,
) -> Option<Vec<DdRow>> {
    let mut scratch = EpochRowsScratch::default();
    let rows = epoch_dd_rows_into(
        epoch,
        base,
        state,
        state.baseline_m,
        &state.sd_ambiguities_m,
        model,
        None,
        &mut scratch,
    )?;
    Some(
        rows.iter()
            .map(|r| DdRow {
                kind: r.kind,
                sat: r.sat.clone(),
                ref_sat: r.ref_sat.clone(),
                sd_ambiguity_id: r.sd_ambiguity_id.clone(),
                h: r.h.clone(),
                y: r.y,
                sd_variance_m2: r.sd_variance_m2,
                ref_sd_variance_m2: r.ref_sd_variance_m2,
                weight: r.weight,
            })
            .collect(),
    )
}

fn epoch_dd_rows_into<'a>(
    epoch: &Epoch,
    base: [f64; 3],
    state: &FilterState,
    baseline_m: [f64; 3],
    sd_ambiguities_m: &[f64],
    model: &MeasModel,
    receiver_antenna_corrections: Option<&ReceiverAntennaCorrections>,
    scratch: &'a mut EpochRowsScratch,
) -> Option<&'a [DdRowScratch]> {
    let rover = [
        base[0] + baseline_m[0],
        base[1] + baseline_m[1],
        base[2] + baseline_m[2],
    ];
    let n = state.dim();

    // Per-system reference context, computed once per epoch (Elixir
    // `epoch_reference_data/2`): the reference's observation single differences,
    // SD geometry + LOS at the current linearization point, and SD variances
    // from the shared receive-time position (Elixir `row_variance/5`: sat and
    // reference evaluated separately).
    scratch.refs_len = 0;
    for r in &epoch.references {
        let system = satellite_system(&r.sat);
        let existing = (0..scratch.refs_len).find(|&i| scratch.refs[i].system == system);
        let idx = if let Some(i) = existing {
            i
        } else {
            if scratch.refs_len == scratch.refs.len() {
                scratch.refs.push(RefCtxScratch::default());
            }
            let i = scratch.refs_len;
            scratch.refs_len += 1;
            i
        };
        let rc = &mut scratch.refs[idx];
        assign_str(&mut rc.system, system);
        assign_str(&mut rc.sat, &r.sat);
        assign_str(&mut rc.sd_ambiguity_id, &r.sd_ambiguity_id);
        rc.pos = r.pos;
        rc.sd_code = r.rover_code_m - r.base_code_m;
        rc.sd_phase = r.rover_phase_m - r.base_phase_m;
        rc.sd_geom = geometric_range_m(r.rover_tx_pos, rover, model.sagnac)
            - geometric_range_m(r.base_tx_pos, base, model.sagnac);
        rc.los = range_derivative(rover, r.rover_tx_pos);
        rc.code_var = single_difference_variance(model.code_sigma_m, model.stochastic, base, r.pos);
        rc.phase_var =
            single_difference_variance(model.phase_sigma_m, model.stochastic, base, r.pos);
    }

    scratch.rows_len = 0;
    for m in &epoch.nonref {
        // Each non-reference satellite pairs with its OWN system's reference.
        let rc_idx =
            (0..scratch.refs_len).find(|&i| scratch.refs[i].system == satellite_system(&m.sat))?;
        let rc = &scratch.refs[rc_idx];
        let ref_sd_id = rc.sd_ambiguity_id.as_str();
        let ref_sd_code = rc.sd_code;
        let ref_sd_phase = rc.sd_phase;
        let ref_sd_geom = rc.sd_geom;
        let ref_pos = rc.pos;
        let ref_los = rc.los;
        let ref_code_var = rc.code_var;
        let ref_phase_var = rc.phase_var;
        let sat_sd_id = m.sd_ambiguity_id.as_str();
        let code_sd_var =
            single_difference_variance(model.code_sigma_m, model.stochastic, base, m.pos);
        let phase_sd_var =
            single_difference_variance(model.phase_sigma_m, model.stochastic, base, m.pos);

        let sat_sd_code = m.rover_code_m - m.base_code_m;
        let sat_sd_phase = m.rover_phase_m - m.base_phase_m;
        let obs_dd_code = sat_sd_code - ref_sd_code;
        let obs_dd_phase = sat_sd_phase - ref_sd_phase;

        let sat_sd_geom = geometric_range_m(m.rover_tx_pos, rover, model.sagnac)
            - geometric_range_m(m.base_tx_pos, base, model.sagnac);
        let geom_dd = sat_sd_geom - ref_sd_geom;
        let dd_receiver_correction = double_difference_receiver_antenna_correction(
            m.pos,
            base,
            rover,
            ref_pos,
            base,
            rover,
            receiver_antenna_corrections,
            &mut scratch.receiver_antenna,
        );
        let modeled_geom_dd = geom_dd - dd_receiver_correction;

        let sat_los = range_derivative(rover, m.rover_tx_pos);
        let dd_deriv = sub3(sat_los, ref_los); // LOS(sat) - LOS(ref)

        // Code design row: [dx, dy, dz | 0...] (code carries no ambiguity).
        if scratch.rows_len == scratch.rows.len() {
            scratch.rows.push(DdRowScratch::default());
        }
        let row = &mut scratch.rows[scratch.rows_len];
        scratch.rows_len += 1;
        row.kind = RowKind::Code;
        assign_str(&mut row.sat, &m.sat);
        assign_str(&mut row.ref_sat, &rc.sat);
        assign_str(&mut row.sd_ambiguity_id, &m.sd_ambiguity_id);
        row.h.resize(n, 0.0);
        row.h.fill(0.0);
        row.h[0] = dd_deriv[0];
        row.h[1] = dd_deriv[1];
        row.h[2] = dd_deriv[2];
        row.y = obs_dd_code - modeled_geom_dd;
        row.sd_variance_m2 = code_sd_var;
        row.ref_sd_variance_m2 = ref_code_var;
        row.weight = 1.0 / (code_sd_var + ref_code_var);

        // Phase design row: [dx, dy, dz | +1 at sat SD col, -1 at ref SD col].
        let sat_pos = state.ambiguity_pos(sat_sd_id)?;
        let ref_pos = state.ambiguity_pos(ref_sd_id)?;
        let ambiguity_dd = sd_ambiguities_m[sat_pos] - sd_ambiguities_m[ref_pos];
        if scratch.rows_len == scratch.rows.len() {
            scratch.rows.push(DdRowScratch::default());
        }
        let row = &mut scratch.rows[scratch.rows_len];
        scratch.rows_len += 1;
        row.kind = RowKind::Phase;
        assign_str(&mut row.sat, &m.sat);
        assign_str(&mut row.ref_sat, &rc.sat);
        assign_str(&mut row.sd_ambiguity_id, &m.sd_ambiguity_id);
        row.h.resize(n, 0.0);
        row.h.fill(0.0);
        row.h[0] = dd_deriv[0];
        row.h[1] = dd_deriv[1];
        row.h[2] = dd_deriv[2];
        row.h[3 + sat_pos] = 1.0;
        row.h[3 + ref_pos] = -1.0;
        row.y = obs_dd_phase - (modeled_geom_dd + ambiguity_dd);
        row.sd_variance_m2 = phase_sd_var;
        row.ref_sd_variance_m2 = ref_phase_var;
        row.weight = 1.0 / (phase_sd_var + ref_phase_var);
    }
    Some(&scratch.rows[..scratch.rows_len])
}

// =========================================================================
// Iterated information-filter measurement update (kernel slice 2b)
// -------------------------------------------------------------------------
// Port of the Elixir `iterate_sequential_filter_epoch` (Gauss-Newton iterated
// information filter). Per iteration: relinearize the DD geometry at the current
// iterate, accumulate the measurement normal equations (+ fix-and-hold pseudo-
// measurements), add the prior contribution `Λ_prior·(center - current)` to the
// rhs, solve δx, apply, and repeat until `‖δbaseline‖ ≤ pos_tol` and
// `max|δambiguity| ≤ amb_tol` (or `max_iterations`). The posterior information is
// `Λ_prior + Λ_measurement + Λ_hold` and is carried to the next epoch.

/// A held (fixed) double-difference ambiguity, as a hold pseudo-measurement:
/// constrains `SD(sat) - SD(ref)` toward `fixed_m`.
#[derive(Debug, Clone, PartialEq)]
pub struct Hold {
    pub sat_sd_id: String,
    pub ref_sd_id: String,
    pub fixed_m: f64,
}

/// Posterior of one epoch's iterated update.
#[derive(Debug, Clone, PartialEq)]
pub struct EpochPosterior {
    pub baseline_m: [f64; 3],
    pub sd_ambiguities_m: Vec<f64>,
    /// Posterior information matrix (row-major n×n), carried to the next epoch.
    pub information: Vec<f64>,
}

#[derive(Debug)]
struct EpochPosteriorRef<'a> {
    baseline_m: [f64; 3],
    sd_ambiguities_m: &'a [f64],
    information: &'a [f64],
}

#[derive(Debug, Default)]
struct IterateScratch {
    epoch_rows: EpochRowsScratch,
    fold_block: FoldBlockScratch,
    solve: SolveNormalScratch,
    prior_center: Vec<f64>,
    work_ambiguities: Vec<f64>,
    measurement_lambda: Vec<f64>,
    measurement_eta: Vec<f64>,
    hold_lambda: Vec<f64>,
    hold_eta: Vec<f64>,
    lambda: Vec<f64>,
    eta: Vec<f64>,
    delta_center: Vec<f64>,
    prior_rhs: Vec<f64>,
    block_indices: Vec<usize>,
    block_rows: Vec<usize>,
}

#[derive(Debug, Default)]
struct HoldPool {
    holds: Vec<Hold>,
    len: usize,
}

/// Reusable buffers for the RTK filter hot path.
///
/// Create one per arc/track and pass it to [`update_epoch_with_scratch`] to keep
/// row, normal-equation, and solver workspaces allocated across epochs. The
/// existing [`update_epoch`] entry point remains available for callers that do
/// not manage scratch explicitly.
#[derive(Debug, Default)]
pub struct RtkFilterScratch {
    iterate: IterateScratch,
    report_iterate: IterateScratch,
    screen_rows: EpochRowsScratch,
    screen_mask: Vec<bool>,
    held: HoldPool,
}

impl RtkFilterScratch {
    pub fn new() -> Self {
        Self::default()
    }
}

fn matvec_into(m: &[f64], v: &[f64], out: &mut [f64]) {
    let n = v.len();
    for (i, out_i) in out.iter_mut().enumerate().take(n) {
        let row = i * n;
        let mut acc = 0.0;
        for j in 0..n {
            acc += m[row + j] * v[j];
        }
        *out_i = acc;
    }
}

/// Iterated information-filter update for one epoch. `state` is the carried-in
/// prior (its `baseline_m`/`sd_ambiguities_m` are the prior center, `information`
/// the prior information). All ambiguity columns referenced by the epoch and the
/// holds must already be present in `state` (call `ensure_ambiguity` first).
/// `held` is empty for a pure float update.
///
/// PERF: clones the prior state once for relinearization and allocates the
/// per-iteration normal-equations buffers; fine for the correctness slice, to be
/// arena-reused before the benchmark gate (same note as `solve_normal`).
#[allow(clippy::too_many_arguments)]
pub fn iterate_epoch(
    state: &FilterState,
    epoch: &Epoch,
    base: [f64; 3],
    model: &MeasModel,
    held: &[Hold],
    hold_sigma_m: f64,
    position_tol_m: f64,
    ambiguity_tol_m: f64,
    max_iterations: usize,
) -> Option<EpochPosterior> {
    let mut scratch = IterateScratch::default();
    let posterior = iterate_epoch_into(
        state,
        epoch,
        base,
        model,
        held,
        hold_sigma_m,
        position_tol_m,
        ambiguity_tol_m,
        max_iterations,
        None,
        None,
        &mut scratch,
    )?;
    Some(EpochPosterior {
        baseline_m: posterior.baseline_m,
        sd_ambiguities_m: posterior.sd_ambiguities_m.to_vec(),
        information: posterior.information.to_vec(),
    })
}

#[allow(clippy::needless_range_loop, clippy::too_many_arguments)]
fn iterate_epoch_into<'a>(
    state: &FilterState,
    epoch: &Epoch,
    base: [f64; 3],
    model: &MeasModel,
    held: &[Hold],
    hold_sigma_m: f64,
    position_tol_m: f64,
    ambiguity_tol_m: f64,
    max_iterations: usize,
    receiver_antenna_corrections: Option<&ReceiverAntennaCorrections>,
    screen_mask: Option<&[bool]>,
    scratch: &'a mut IterateScratch,
) -> Option<EpochPosteriorRef<'a>> {
    let n = state.dim();
    let prior_information = &state.information;

    // Prior center (fixed across iterations): [bx, by, bz | sd ambiguities].
    scratch.prior_center.resize(n, 0.0);
    scratch.prior_center[..3].copy_from_slice(&state.baseline_m);
    scratch.prior_center[3..].copy_from_slice(&state.sd_ambiguities_m);

    // Current linearization point (starts at the prior center).
    let mut work_baseline = state.baseline_m;
    scratch
        .work_ambiguities
        .resize(state.sd_ambiguities_m.len(), 0.0);
    scratch
        .work_ambiguities
        .copy_from_slice(&state.sd_ambiguities_m);
    let hold_weight = 1.0 / (hold_sigma_m * hold_sigma_m);
    let nn = n * n;
    scratch.measurement_lambda.resize(nn, 0.0);
    scratch.measurement_eta.resize(n, 0.0);
    scratch.hold_lambda.resize(nn, 0.0);
    scratch.hold_eta.resize(n, 0.0);
    scratch.lambda.resize(nn, 0.0);
    scratch.eta.resize(n, 0.0);
    scratch.delta_center.resize(n, 0.0);
    scratch.prior_rhs.resize(n, 0.0);

    for iter in 1..=max_iterations.max(1) {
        let rows = epoch_dd_rows_into(
            epoch,
            base,
            state,
            work_baseline,
            &scratch.work_ambiguities,
            model,
            receiver_antenna_corrections,
            &mut scratch.epoch_rows,
        )?;

        scratch.measurement_lambda.fill(0.0);
        scratch.measurement_eta.fill(0.0);
        scratch.hold_lambda.fill(0.0);
        scratch.hold_eta.fill(0.0);

        // Measurement normal equations. Covariance blocks are keyed
        // {kind, reference satellite}: rows correlate only through their shared
        // reference single difference, so each system's code (and phase) double
        // differences form their own block with no cross-system correlation —
        // Elixir groups by `{epoch_idx, kind, ref_sat}` and folds the blocks in
        // sorted key order (`:code < :phase`, then reference id). With one
        // system the key is constant per kind, so this degenerates to the
        // historical two-block code-then-phase fold bit-for-bit. Within each
        // block, fold rows in satellite order — the Elixir reference sorts each
        // block (`Enum.sort_by(block_rows, & &1.sat)`) before assembling, so the
        // kernel must accumulate Σ_a Σ_b in the same order.
        scratch.block_indices.clear();
        match screen_mask {
            Some(mask) => scratch
                .block_indices
                .extend((0..rows.len()).filter(|&idx| mask.get(idx).copied().unwrap_or(false))),
            None => scratch.block_indices.extend(0..rows.len()),
        }
        scratch.block_indices.sort_by(|&a, &b| {
            (rows[a].kind, rows[a].ref_sat.as_str()).cmp(&(rows[b].kind, rows[b].ref_sat.as_str()))
        });
        let mut start = 0;
        while start < scratch.block_indices.len() {
            let first = scratch.block_indices[start];
            let kind = rows[first].kind;
            let ref_sat = rows[first].ref_sat.as_str();
            let mut end = start + 1;
            while end < scratch.block_indices.len() {
                let idx = scratch.block_indices[end];
                if rows[idx].kind != kind || rows[idx].ref_sat != ref_sat {
                    break;
                }
                end += 1;
            }
            scratch.block_rows.clear();
            scratch
                .block_rows
                .extend_from_slice(&scratch.block_indices[start..end]);
            scratch
                .block_rows
                .sort_by(|&a, &b| rows[a].sat.cmp(&rows[b].sat));
            fold_measurement_block_indices(
                &mut scratch.measurement_lambda,
                &mut scratch.measurement_eta,
                rows,
                &scratch.block_rows,
                &mut scratch.fold_block,
            )?;
            start = end;
        }

        // Fix-and-hold pseudo-measurements: SD(sat) - SD(ref) pulled to fixed_m.
        fold_hold_block_with_ambiguities(
            &mut scratch.hold_lambda,
            &mut scratch.hold_eta,
            state,
            &scratch.work_ambiguities,
            held,
            hold_weight,
        )?;

        // Prior contribution: rhs += Λ_prior·(center - current). Assemble the
        // matrix/vector in Elixir's exact grouping:
        //   information = (prior + measurement) + hold
        //   rhs         = (measurement + hold) + prior_rhs
        scratch.delta_center[0] = scratch.prior_center[0] - work_baseline[0];
        scratch.delta_center[1] = scratch.prior_center[1] - work_baseline[1];
        scratch.delta_center[2] = scratch.prior_center[2] - work_baseline[2];
        for i in 0..scratch.work_ambiguities.len() {
            scratch.delta_center[3 + i] = scratch.prior_center[3 + i] - scratch.work_ambiguities[i];
        }
        matvec_into(
            prior_information,
            &scratch.delta_center,
            &mut scratch.prior_rhs,
        );
        for i in 0..(n * n) {
            scratch.lambda[i] =
                (prior_information[i] + scratch.measurement_lambda[i]) + scratch.hold_lambda[i];
        }
        for i in 0..n {
            scratch.eta[i] =
                (scratch.measurement_eta[i] + scratch.hold_eta[i]) + scratch.prior_rhs[i];
        }

        // Per-system SD gauge fixing (op-for-op `apply_reference_sd_gauge/8`).
        // Every measurement row, hold, and reported quantity depends only on
        // within-system single-difference DIFFERENCES, so the common-mode level
        // of each system's SD ambiguity block is unobservable; once per-epoch
        // information accumulates, the 1/σ² initial prior carrying that gauge
        // direction falls below float precision and the pivot cancels exactly to
        // zero. Pin each per-system reference SD ambiguity at its prior-center
        // value with the hold weight — a pure gauge constraint (double
        // differences and the baseline are invariant). Applied to single-system
        // arcs too: the one system's reference SD is equally a gauge DOF and its
        // pivot cancels on a long tight-hold arc (the epoch-124 singularity)
        // without this. Applied AFTER the (prior + measurement) + hold assembly
        // and BEFORE the solve, exactly where the Elixir iterate folds it; the
        // gauge terms stay in `lambda`, so the carried posterior information
        // includes them.
        if !state.references.is_empty() {
            // Iterate the run-level references in sorted-system order (Elixir
            // `refs |> Enum.sort()`), skipping systems absent this epoch.
            for system in state.references.keys() {
                let Some(r) = epoch
                    .references
                    .iter()
                    .find(|m| satellite_system(&m.sat) == system)
                else {
                    continue;
                };
                let col = 3 + state.ambiguity_pos(&r.sd_ambiguity_id)?;
                let residual = scratch.prior_center[col] - scratch.work_ambiguities[col - 3];
                scratch.lambda[col * n + col] += hold_weight;
                scratch.eta[col] += hold_weight * residual;
            }
        }

        let dx = solve_normal_into(&scratch.lambda, &scratch.eta, &mut scratch.solve)?;

        // Apply: x += δx (baseline first, then ambiguities).
        for (k, b) in work_baseline.iter_mut().enumerate() {
            *b += dx[k];
        }
        for (k, a) in scratch.work_ambiguities.iter_mut().enumerate() {
            *a += dx[3 + k];
        }

        let baseline_step = (dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2]).sqrt();
        let ambiguity_step = dx[3..].iter().fold(0.0_f64, |m, &v| m.max(v.abs()));

        if (baseline_step <= position_tol_m && ambiguity_step <= ambiguity_tol_m)
            || iter >= max_iterations.max(1)
        {
            return Some(EpochPosteriorRef {
                baseline_m: work_baseline,
                sd_ambiguities_m: &scratch.work_ambiguities,
                information: &scratch.lambda,
            });
        }
    }
    None
}

// =========================================================================
// Ambiguity search inputs (kernel slice 2c-i): SD -> DD covariance in cycles
// -------------------------------------------------------------------------
// Port of the Elixir `dd_covariance_m_to_cycles` / `dd_covariance_m`. Given the
// single-difference ambiguity covariance block (metres²) from the inverted
// posterior information, build the double-difference ambiguity covariance in
// cycles for the LAMBDA search. For DD targets i,j whose SD positions are
// (sat_i, ref_i): the DD = sat_SD - ref_SD, so
//   DD_cov[i][j] = (C[si][sj] - C[si][rj] - C[ri][sj] + C[ri][rj]) / (λ_i λ_j).

/// Double-difference ambiguity covariance (cycles) from the `k x k` SD ambiguity
/// covariance block `sd_cov` (row-major, metres²). `dd[i] = (sat_pos, ref_pos)`
/// are SD-block indices; `wavelengths[i]` is the carrier wavelength (metres) of
/// DD target `i`.
#[allow(clippy::needless_range_loop)]
pub fn dd_covariance_cycles(
    sd_cov: &[f64],
    k: usize,
    dd: &[(usize, usize)],
    wavelengths: &[f64],
) -> Vec<f64> {
    let m = dd.len();
    let mut out = vec![0.0; m * m];
    for i in 0..m {
        let (si, ri) = dd[i];
        for j in 0..m {
            let (sj, rj) = dd[j];
            // Op-for-op with Elixir `dd_covariance_m/3`: two inner reductions
            // `(A - B)` and `(-C + D)`, then the outer addition. The flat
            // left-associated form `((A - B) - C) + D` is algebraically
            // equivalent but changes real-arc LAMBDA ratios at ~1e-11.
            let sat_terms = sd_cov[si * k + sj] + -sd_cov[si * k + rj];
            let ref_terms = -sd_cov[ri * k + sj] + sd_cov[ri * k + rj];
            let c = sat_terms + ref_terms;
            out[i * m + j] = c / (wavelengths[i] * wavelengths[j]);
        }
    }
    out
}

// =========================================================================
// Ambiguity search-and-hold (kernel slice 2c-ii)
// -------------------------------------------------------------------------
// Port of the Elixir `sequential_search_and_hold`. Invert the posterior
// information to covariance, take the SD ambiguity block, transform the
// not-yet-held DD ambiguities to cycles, compute the float DD cycles, run the
// LAMBDA kernel (ils.rs), and on a passing ratio test merge the accepted
// integers into the held fixes (each recording its (sat,ref) SD pair so the
// hold pseudo-measurement is correct even if the reference satellite changes).

/// Ratio-test options for the ambiguity search.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SearchOpts {
    pub ratio_threshold: f64,
}

/// Baseline dynamics used by the prediction step before each measurement update.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum DynamicsModel {
    /// Keep the carried baseline mean fixed. This is the historical RTK filter.
    ConstantPosition,
    /// Advance the baseline mean by the epoch velocity times elapsed seconds.
    /// Process-noise meaning is unchanged.
    VelocityPropagated,
}

/// Optional predicted-residual screen for one epoch update.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct InnovationScreenOpts {
    pub threshold_sigma: f64,
    pub min_rows: usize,
}

/// Diagnostics from the optional predicted-residual screen.
#[derive(Debug, Clone, PartialEq)]
pub struct InnovationScreen {
    pub threshold_sigma: f64,
    pub min_rows: usize,
    pub input_rows: usize,
    pub accepted_rows: usize,
    pub rejected_rows: usize,
    pub rejected_code_rows: usize,
    pub rejected_phase_rows: usize,
    pub max_abs_normalized_innovation: Option<f64>,
    pub max_rejected_abs_normalized_innovation: Option<f64>,
    pub coasted: bool,
}

/// Options for one streaming filter epoch update.
#[derive(Debug, Clone, PartialEq)]
pub struct UpdateOpts {
    pub hold_sigma_m: f64,
    pub position_tol_m: f64,
    pub ambiguity_tol_m: f64,
    pub max_iterations: usize,
    /// Kinematic process-noise sigma (metres) for the baseline. `0.0` (default)
    /// is the static filter: the carried information is propagated forward
    /// untouched. When `> 0`, a between-epoch predict step inflates the baseline
    /// covariance block by `sigma^2` (the `Λ⁻¹ + Q` round-trip), skipped on the
    /// first epoch. Mirrors the Elixir `:process_noise_baseline_sigma_m`.
    pub process_noise_baseline_sigma_m: f64,
    /// Prediction mean dynamics. Defaults to [`DynamicsModel::ConstantPosition`]
    /// in public callers.
    pub dynamics_model: DynamicsModel,
    /// Constellation letters whose double-difference ambiguities never enter
    /// the LAMBDA search set — they contribute float measurement rows only
    /// (GLONASS FDMA is the canonical use). Mirrors the Elixir
    /// `:float_only_systems`.
    pub float_only_systems: Vec<String>,
    /// Optional predicted-residual screen for the Rust kernel update.
    pub innovation_screen: Option<InnovationScreenOpts>,
    /// Optional receiver-antenna PCO/PCV corrections applied inside DD row construction.
    pub receiver_antenna_corrections: Option<ReceiverAntennaCorrections>,
    /// AR commitment arming gate (mirrors the Elixir `:ar_arming_sigma_m`).
    /// When set, the ambiguity search is attempted only once the baseline-block
    /// posterior standard deviation has converged to at most this many metres;
    /// below the gate the epoch carries its existing held set and stays float.
    /// `None` (default) keeps the always-armed behaviour.
    pub ar_arming_sigma_m: Option<f64>,
    pub search: SearchOpts,
}

/// Result of one streaming filter epoch update.
#[derive(Debug, Clone, PartialEq)]
pub struct EpochUpdate {
    /// Updated serializable filter state. Its `baseline_m` is the FLOAT posterior
    /// (the carried Kalman/information state); use [`Self::reported_baseline_m`]
    /// for the reported solution.
    pub state: FilterState,
    /// Ambiguity-conditioned ("fixed") baseline for this epoch's reported
    /// solution. When AR newly succeeds, this re-solves the epoch with the new
    /// integers held so the reported baseline reflects the fix in the same epoch
    /// (matching RTKLIB's fixed solution); otherwise it equals `state.baseline_m`.
    /// The carried `state` keeps the float baseline so the next epoch linearizes
    /// from the Kalman posterior, not the conditioned point.
    pub reported_baseline_m: [f64; 3],
    /// Single-difference ambiguity vector at the reported solution when it differs
    /// from the carried state, i.e. a same-epoch fixed report re-solve succeeded.
    /// `None` means the carried state's ambiguity vector is also the reported one.
    pub reported_sd_ambiguities_m: Option<Vec<f64>>,
    /// Integer ratio from this epoch's ambiguity search (`0.0` when all observed
    /// ambiguities were already held).
    pub integer_ratio: f64,
    /// Whether the state has any held integer ambiguity after this update.
    pub integer_fixed: bool,
    /// Ambiguity ids newly fixed during this epoch.
    pub newly_fixed: Vec<String>,
    /// All held ambiguity ids after this epoch.
    pub fixed_ids: Vec<String>,
    /// Optional predicted-residual screen metrics.
    pub innovation_screen: Option<InnovationScreen>,
}

/// Why one streaming RTK filter update could not complete.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum UpdateError {
    /// The carried filter state is tied to one reference ambiguity arc per
    /// system; applying held DDs to a different reference would reinterpret
    /// fixed integers.
    ReferenceChanged {
        system: String,
        expected: String,
        actual: String,
    },
    /// The epoch carries a reference for a constellation the state does not
    /// track (the per-system reference set is fixed at filter construction).
    UnknownReferenceSystem(String),
    /// A non-reference satellite's constellation has no reference this epoch
    /// (or a held ambiguity's constellation has no reference in the state).
    MissingSystemReference(String),
    /// An ambiguity needed by the epoch is not present in the state column set.
    MissingAmbiguityColumn(String),
    /// A target ambiguity has no wavelength entry.
    MissingWavelength(String),
    /// A target ambiguity has no metre offset entry.
    MissingOffset(String),
    /// The measurement/update normal equations or posterior covariance were singular.
    SingularGeometry,
    /// The integer least-squares search rejected the ambiguity covariance/input.
    Ils(crate::ils::IlsError),
}

fn prepare_innovation_screen(
    state: &FilterState,
    epoch: &Epoch,
    base: [f64; 3],
    model: &MeasModel,
    opts: &UpdateOpts,
    rows_scratch: &mut EpochRowsScratch,
    mask: &mut Vec<bool>,
) -> Result<Option<InnovationScreen>, UpdateError> {
    let Some(screen) = opts.innovation_screen else {
        mask.clear();
        return Ok(None);
    };

    let rows = epoch_dd_rows_into(
        epoch,
        base,
        state,
        state.baseline_m,
        &state.sd_ambiguities_m,
        model,
        opts.receiver_antenna_corrections.as_ref(),
        rows_scratch,
    )
    .ok_or(UpdateError::SingularGeometry)?;

    mask.clear();
    mask.reserve(rows.len());

    let mut accepted_rows = 0usize;
    let mut rejected_rows = 0usize;
    let mut rejected_code_rows = 0usize;
    let mut rejected_phase_rows = 0usize;
    let mut max_abs_normalized_innovation = None;
    let mut max_rejected_abs_normalized_innovation = None;

    for row in rows {
        let normalized = (row.y * row.weight).abs();
        max_abs_normalized_innovation = Some(
            max_abs_normalized_innovation
                .map_or(normalized, |current: f64| current.max(normalized)),
        );

        let rejected = normalized > screen.threshold_sigma;
        mask.push(!rejected);
        if rejected {
            rejected_rows += 1;
            match row.kind {
                RowKind::Code => rejected_code_rows += 1,
                RowKind::Phase => rejected_phase_rows += 1,
            }
            max_rejected_abs_normalized_innovation = Some(
                max_rejected_abs_normalized_innovation
                    .map_or(normalized, |current: f64| current.max(normalized)),
            );
        } else {
            accepted_rows += 1;
        }
    }

    Ok(Some(InnovationScreen {
        threshold_sigma: screen.threshold_sigma,
        min_rows: screen.min_rows,
        input_rows: rows.len(),
        accepted_rows,
        rejected_rows,
        rejected_code_rows,
        rejected_phase_rows,
        max_abs_normalized_innovation,
        max_rejected_abs_normalized_innovation,
        coasted: accepted_rows < screen.min_rows,
    }))
}

fn coasted_update(
    mut state: FilterState,
    innovation_screen: Option<InnovationScreen>,
) -> EpochUpdate {
    let fixed_ids: Vec<String> = state.fixed_cycles.keys().cloned().collect();
    state.epoch_count += 1;
    EpochUpdate {
        reported_baseline_m: state.baseline_m,
        reported_sd_ambiguities_m: None,
        integer_ratio: 0.0,
        integer_fixed: !fixed_ids.is_empty(),
        newly_fixed: Vec::new(),
        fixed_ids,
        innovation_screen,
        state,
    }
}

/// Run the LAMBDA search on the epoch's not-yet-held double differences and hold
/// any that fix. `posterior_information` is the `n x n` matrix from
/// [`iterate_epoch`]; `wavelengths`/`offsets` are keyed by the non-reference
/// satellite's SD ambiguity id. Double differences whose satellite belongs to a
/// system in `float_only_systems` never enter the search set (Elixir
/// `float_only_ambiguity_ids` rejection in `sequential_search_and_hold`).
/// Returns the updated held set and the ratio (`0.0` when there was nothing to
/// search).
#[allow(clippy::needless_range_loop, clippy::too_many_arguments)]
pub fn search_and_hold(
    state: &FilterState,
    posterior_information: &[f64],
    epoch: &Epoch,
    wavelengths: &BTreeMap<String, f64>,
    offsets: &BTreeMap<String, f64>,
    held: &[Hold],
    float_only_systems: &[String],
    ar_arming_sigma_m: Option<f64>,
    opts: &SearchOpts,
) -> Result<(Vec<Hold>, f64), UpdateError> {
    let n = state.dim();
    let held_sats: std::collections::BTreeSet<&str> =
        held.iter().map(|h| h.sat_sd_id.as_str()).collect();

    // DD candidates: non-reference sats observed this epoch, not already held,
    // not in a float-only system.
    let targets: Vec<&SatMeas> = epoch
        .nonref
        .iter()
        .filter(|m| {
            !held_sats.contains(m.sd_ambiguity_id.as_str())
                && !float_only_systems
                    .iter()
                    .any(|s| s == satellite_system(&m.sat))
        })
        .collect();
    if targets.is_empty() {
        return Ok((held.to_vec(), 0.0));
    }

    // Posterior covariance; take the SD ambiguity block (indices 3..n).
    let info_rows: Vec<Vec<f64>> = (0..n)
        .map(|i| posterior_information[i * n..i * n + n].to_vec())
        .collect();
    let cov = crate::ils::invert(&info_rows).map_err(|_| UpdateError::SingularGeometry)?;

    // AR commitment arming gate: skip the search (carry the held set, ratio 0.0)
    // while the baseline-block posterior sigma is above the threshold. Op order
    // matches the Elixir `ar_armed?` reference exactly.
    if let Some(threshold_m) = ar_arming_sigma_m {
        let base_sigma_m = (cov[0][0].max(0.0) + cov[1][1].max(0.0) + cov[2][2].max(0.0)).sqrt();

        if base_sigma_m > threshold_m {
            return Ok((held.to_vec(), 0.0));
        }
    }

    let k = n - 3;
    let mut sd_block = vec![0.0; k * k];
    for i in 0..k {
        for j in 0..k {
            sd_block[i * k + j] = cov[3 + i][3 + j];
        }
    }

    // DD target SD-block indices, wavelengths, and float DD cycles. Each DD
    // pairs the satellite's SD ambiguity with its OWN system's reference.
    let mut dd_idx = Vec::with_capacity(targets.len());
    let mut wl = Vec::with_capacity(targets.len());
    let mut float_cycles = Vec::with_capacity(targets.len());
    for m in &targets {
        let system = satellite_system(&m.sat);
        let ref_sd = state
            .references
            .get(system)
            .ok_or_else(|| UpdateError::MissingSystemReference(system.to_string()))?
            .as_str();
        let ref_pos = state
            .ambiguity_pos(ref_sd)
            .ok_or_else(|| UpdateError::MissingAmbiguityColumn(ref_sd.to_string()))?;
        let sat_pos = state
            .ambiguity_pos(&m.sd_ambiguity_id)
            .ok_or_else(|| UpdateError::MissingAmbiguityColumn(m.sd_ambiguity_id.clone()))?;
        dd_idx.push((sat_pos, ref_pos));
        let lambda = *wavelengths
            .get(&m.sd_ambiguity_id)
            .ok_or_else(|| UpdateError::MissingWavelength(m.sd_ambiguity_id.clone()))?;
        let offset = *offsets
            .get(&m.sd_ambiguity_id)
            .ok_or_else(|| UpdateError::MissingOffset(m.sd_ambiguity_id.clone()))?;
        wl.push(lambda);
        let amb_m = state
            .dd_ambiguity_m(&m.sd_ambiguity_id, ref_sd)
            .ok_or_else(|| UpdateError::MissingAmbiguityColumn(m.sd_ambiguity_id.clone()))?;
        float_cycles.push((amb_m - offset) / lambda);
    }

    let m_dd = targets.len();
    let dd_cov = dd_covariance_cycles(&sd_block, k, &dd_idx, &wl);
    let dd_cov_rows: Vec<Vec<f64>> = (0..m_dd)
        .map(|i| dd_cov[i * m_dd..i * m_dd + m_dd].to_vec())
        .collect();

    let result = crate::ils::lambda_ils_search(&float_cycles, &dd_cov_rows, opts.ratio_threshold)
        .map_err(UpdateError::Ils)?;

    if result.fixed_status {
        let mut updated = held.to_vec();
        for (t, &cycles) in targets.iter().zip(&result.fixed) {
            let lambda = *wavelengths
                .get(&t.sd_ambiguity_id)
                .ok_or_else(|| UpdateError::MissingWavelength(t.sd_ambiguity_id.clone()))?;
            let offset = *offsets
                .get(&t.sd_ambiguity_id)
                .ok_or_else(|| UpdateError::MissingOffset(t.sd_ambiguity_id.clone()))?;
            let system = satellite_system(&t.sat);
            let ref_sd = state
                .references
                .get(system)
                .ok_or_else(|| UpdateError::MissingSystemReference(system.to_string()))?;
            updated.push(Hold {
                sat_sd_id: t.sd_ambiguity_id.clone(),
                ref_sd_id: ref_sd.clone(),
                fixed_m: cycles as f64 * lambda + offset,
            });
        }
        Ok((updated, result.ratio))
    } else {
        Ok((held.to_vec(), result.ratio))
    }
}

fn phase_code_seed_m(meas: &SatMeas) -> f64 {
    (meas.rover_phase_m - meas.base_phase_m) - (meas.rover_code_m - meas.base_code_m)
}

fn held_from_state(state: &FilterState) -> Result<Vec<Hold>, UpdateError> {
    state
        .fixed_m
        .iter()
        .map(|(sat_sd_id, &fixed_m)| {
            let system = satellite_system(sat_sd_id);
            let ref_sd_id = state
                .references
                .get(system)
                .ok_or_else(|| UpdateError::MissingSystemReference(system.to_string()))?;
            Ok(Hold {
                sat_sd_id: sat_sd_id.clone(),
                ref_sd_id: ref_sd_id.clone(),
                fixed_m,
            })
        })
        .collect()
}

fn held_from_state_into<'a>(
    state: &FilterState,
    pool: &'a mut HoldPool,
) -> Result<&'a [Hold], UpdateError> {
    pool.len = 0;
    for (sat_sd_id, &fixed_m) in &state.fixed_m {
        let system = satellite_system(sat_sd_id);
        let ref_sd_id = state
            .references
            .get(system)
            .ok_or_else(|| UpdateError::MissingSystemReference(system.to_string()))?;
        if pool.len == pool.holds.len() {
            pool.holds.push(Hold {
                sat_sd_id: String::new(),
                ref_sd_id: String::new(),
                fixed_m: 0.0,
            });
        }
        let hold = &mut pool.holds[pool.len];
        pool.len += 1;
        assign_str(&mut hold.sat_sd_id, sat_sd_id);
        assign_str(&mut hold.ref_sd_id, ref_sd_id);
        hold.fixed_m = fixed_m;
    }
    Ok(&pool.holds[..pool.len])
}

fn held_contains_sat(held: &[Hold], sat_sd_id: &str) -> bool {
    held.iter().any(|h| h.sat_sd_id == sat_sd_id)
}

fn has_search_targets(epoch: &Epoch, held: &[Hold], float_only_systems: &[String]) -> bool {
    epoch.nonref.iter().any(|m| {
        !held_contains_sat(held, &m.sd_ambiguity_id)
            && !float_only_systems
                .iter()
                .any(|s| s == satellite_system(&m.sat))
    })
}

type FixedMaps = (BTreeMap<String, i64>, BTreeMap<String, f64>);

fn fixed_maps_from_holds(
    holds: &[Hold],
    wavelengths: &BTreeMap<String, f64>,
    offsets: &BTreeMap<String, f64>,
) -> Result<FixedMaps, UpdateError> {
    let mut fixed_cycles = BTreeMap::new();
    let mut fixed_m = BTreeMap::new();
    for hold in holds {
        let lambda = *wavelengths
            .get(&hold.sat_sd_id)
            .ok_or_else(|| UpdateError::MissingWavelength(hold.sat_sd_id.clone()))?;
        let offset = *offsets
            .get(&hold.sat_sd_id)
            .ok_or_else(|| UpdateError::MissingOffset(hold.sat_sd_id.clone()))?;
        let cycles = ((hold.fixed_m - offset) / lambda).round() as i64;
        fixed_cycles.insert(hold.sat_sd_id.clone(), cycles);
        fixed_m.insert(hold.sat_sd_id.clone(), hold.fixed_m);
    }
    Ok((fixed_cycles, fixed_m))
}

/// Kinematic predict: rank-3 (Woodbury) inflation of the baseline (indices 0..3)
/// covariance block of an `n x n` row-major information matrix by `q = sigma^2`:
///
///   Λ' = Λ - Λ[:,0..3] · (Q⁻¹ + Λ₀₀)⁻¹ · Λ[0..3,:],   Q = q·I₃
///
/// Mathematically equal to adding `q` to the baseline covariance diagonal and
/// re-inverting, but via a single 3×3 inverse instead of two full `n x n`
/// inversions. The double full-invert corrupts near-singular ambiguity
/// directions when `q` does not dominate (the filter then goes singular at small
/// process noise); this rank-3 form is stable for any `q` and preserves the
/// ambiguity block exactly. Returns `None` on a singular 3×3 system (caller keeps
/// the un-inflated prior). Same operation order as the Elixir
/// `inflate_baseline_information` for bit-for-bit trace parity.
fn time_update_information(information: &[f64], n: usize, sigma: f64) -> Option<Vec<f64>> {
    let inv_q = 1.0 / (sigma * sigma);
    // M = Q⁻¹ + Λ₀₀ (3×3 top-left block of the information matrix).
    let mut m = [[0.0f64; 3]; 3];
    for (i, mi) in m.iter_mut().enumerate() {
        for (j, mij) in mi.iter_mut().enumerate() {
            *mij = information[i * n + j] + if i == j { inv_q } else { 0.0 };
        }
    }
    let m_inv = invert_3x3(&m)?;
    // w = Λ[:,0..3] · M⁻¹ (n×3).
    let mut w = vec![[0.0f64; 3]; n];
    for (i, wi) in w.iter_mut().enumerate() {
        for (a, wia) in wi.iter_mut().enumerate() {
            let mut s = 0.0;
            for b in 0..3 {
                s += information[i * n + b] * m_inv[b][a];
            }
            *wia = s;
        }
    }
    // Λ' = Λ - w · Λ[0..3,:]  (Λ symmetric ⇒ Λ[0..3,:][a][j] = Λ[j][a]).
    let mut out = information.to_vec();
    for (i, wi) in w.iter().enumerate() {
        for j in 0..n {
            let mut corr = 0.0;
            for (a, &wia) in wi.iter().enumerate() {
                corr += wia * information[j * n + a];
            }
            out[i * n + j] -= corr;
        }
    }
    Some(out)
}

fn propagate_baseline_mean(state: &mut FilterState, epoch: &Epoch, opts: &UpdateOpts) {
    if state.epoch_count == 0 || opts.dynamics_model != DynamicsModel::VelocityPropagated {
        return;
    }

    let Some(velocity) = epoch.velocity_mps else {
        return;
    };

    let dt = epoch.dt_s;
    if !dt.is_finite() || dt <= 0.0 || !velocity.iter().all(|v| v.is_finite()) {
        return;
    }

    for (k, v) in velocity.iter().enumerate() {
        state.baseline_m[k] += v * dt;
    }
}

/// Closed-form 3×3 inverse (adjugate / determinant). `None` if `|det| <= 1e-12`.
/// Deterministic arithmetic matching the Elixir `invert_3x3` for trace parity.
fn invert_3x3(m: &[[f64; 3]; 3]) -> Option<[[f64; 3]; 3]> {
    let [[a, b, c], [d, e, f], [g, h, i]] = *m;
    let det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
    if det.abs() <= 1.0e-12 {
        return None;
    }
    let inv = 1.0 / det;
    Some([
        [
            (e * i - f * h) * inv,
            (c * h - b * i) * inv,
            (b * f - c * e) * inv,
        ],
        [
            (f * g - d * i) * inv,
            (a * i - c * g) * inv,
            (c * d - a * f) * inv,
        ],
        [
            (d * h - e * g) * inv,
            (b * g - a * h) * inv,
            (a * e - b * d) * inv,
        ],
    ])
}

/// Streaming RTK filter update for one epoch.
///
/// This is the crate-level `update(state, epoch) -> (state', solution)` entry
/// point for the kernel lane. It dynamically adds newly observed
/// single-difference ambiguity columns, seeds them from phase-code, runs the
/// iterated information update, searches unheld DD ambiguities with LAMBDA, and
/// persists accepted fixed ambiguities into the returned [`FilterState`].
pub fn update_epoch(
    state: FilterState,
    epoch: &Epoch,
    base: [f64; 3],
    model: &MeasModel,
    wavelengths: &BTreeMap<String, f64>,
    offsets: &BTreeMap<String, f64>,
    opts: &UpdateOpts,
) -> Result<EpochUpdate, UpdateError> {
    let mut scratch = RtkFilterScratch::default();
    update_epoch_with_scratch(
        state,
        epoch,
        base,
        model,
        wavelengths,
        offsets,
        opts,
        &mut scratch,
    )
}

/// Streaming RTK filter update using caller-owned reusable buffers.
#[allow(clippy::too_many_arguments)]
pub fn update_epoch_with_scratch(
    mut state: FilterState,
    epoch: &Epoch,
    base: [f64; 3],
    model: &MeasModel,
    wavelengths: &BTreeMap<String, f64>,
    offsets: &BTreeMap<String, f64>,
    opts: &UpdateOpts,
    scratch: &mut RtkFilterScratch,
) -> Result<EpochUpdate, UpdateError> {
    // Per-system reference guard: each epoch reference must match the state's
    // reference ambiguity arc for its constellation. A different arc (or an
    // untracked constellation) would reinterpret held integers.
    for r in &epoch.references {
        let system = satellite_system(&r.sat);
        match state.references.get(system) {
            None => return Err(UpdateError::UnknownReferenceSystem(system.to_string())),
            Some(expected) if expected != &r.sd_ambiguity_id => {
                return Err(UpdateError::ReferenceChanged {
                    system: system.to_string(),
                    expected: expected.clone(),
                    actual: r.sd_ambiguity_id.clone(),
                });
            }
            Some(_) => {}
        }
    }
    // Every non-reference satellite needs its own system's reference this epoch
    // (the Elixir per-system common invariant guarantees this upstream).
    for m in &epoch.nonref {
        let system = satellite_system(&m.sat);
        if !epoch
            .references
            .iter()
            .any(|r| satellite_system(&r.sat) == system)
        {
            return Err(UpdateError::MissingSystemReference(system.to_string()));
        }
    }

    // First epoch has no prior motion to propagate (mirrors the Elixir guard
    // `acc.epochs != []`). Do not infer this from ambiguity columns: the Orbis
    // trace path pre-sizes columns to match Elixir's global ordering.
    let first_epoch = state.epoch_count == 0;

    for r in &epoch.references {
        state.ensure_ambiguity(&r.sd_ambiguity_id, phase_code_seed_m(r));
    }
    for meas in &epoch.nonref {
        state.ensure_ambiguity(&meas.sd_ambiguity_id, phase_code_seed_m(meas));
    }

    // Predict step: optionally advance the baseline mean from an external
    // velocity, then inflate baseline covariance before the measurement update.
    // On a singular round-trip, keep the un-inflated prior (matches the Elixir
    // fallback). If the inflated prior later makes the measurement solve singular
    // at an unlucky sigma, retry the same epoch with the un-inflated prior; this
    // is the same fallback point extended to the first operation that can prove
    // the predicted information unusable.
    propagate_baseline_mean(&mut state, epoch, opts);

    let mut uninflated_state = None;
    if !first_epoch && opts.process_noise_baseline_sigma_m > 0.0 {
        if let Some(updated) = time_update_information(
            &state.information,
            state.dim(),
            opts.process_noise_baseline_sigma_m,
        ) {
            uninflated_state = Some(state.clone());
            state.information = updated;
        }
    }

    let mut innovation_screen = prepare_innovation_screen(
        &state,
        epoch,
        base,
        model,
        opts,
        &mut scratch.screen_rows,
        &mut scratch.screen_mask,
    )?;
    if innovation_screen
        .as_ref()
        .is_some_and(|screen| screen.coasted)
    {
        return Ok(coasted_update(state, innovation_screen));
    }
    let screen_mask = innovation_screen
        .as_ref()
        .map(|_| scratch.screen_mask.as_slice());

    let mut held = held_from_state_into(&state, &mut scratch.held)?;
    let mut posterior = iterate_epoch_into(
        &state,
        epoch,
        base,
        model,
        held,
        opts.hold_sigma_m,
        opts.position_tol_m,
        opts.ambiguity_tol_m,
        opts.max_iterations,
        opts.receiver_antenna_corrections.as_ref(),
        screen_mask,
        &mut scratch.iterate,
    );

    if posterior.is_none() {
        if let Some(fallback_state) = uninflated_state {
            state = fallback_state;
            innovation_screen = prepare_innovation_screen(
                &state,
                epoch,
                base,
                model,
                opts,
                &mut scratch.screen_rows,
                &mut scratch.screen_mask,
            )?;
            if innovation_screen
                .as_ref()
                .is_some_and(|screen| screen.coasted)
            {
                return Ok(coasted_update(state, innovation_screen));
            }
            let screen_mask = innovation_screen
                .as_ref()
                .map(|_| scratch.screen_mask.as_slice());
            held = held_from_state_into(&state, &mut scratch.held)?;
            posterior = iterate_epoch_into(
                &state,
                epoch,
                base,
                model,
                held,
                opts.hold_sigma_m,
                opts.position_tol_m,
                opts.ambiguity_tol_m,
                opts.max_iterations,
                opts.receiver_antenna_corrections.as_ref(),
                screen_mask,
                &mut scratch.iterate,
            );
        }
    }

    let has_targets = has_search_targets(epoch, held, &opts.float_only_systems);
    let prior_state_for_report = if has_targets {
        Some(state.clone())
    } else {
        None
    };

    {
        let posterior = posterior.ok_or(UpdateError::SingularGeometry)?;
        state.baseline_m = posterior.baseline_m;
        state
            .sd_ambiguities_m
            .copy_from_slice(posterior.sd_ambiguities_m);
        state.information.copy_from_slice(posterior.information);
    }

    if !has_targets {
        let fixed_ids: Vec<String> = state.fixed_cycles.keys().cloned().collect();
        state.epoch_count += 1;
        return Ok(EpochUpdate {
            reported_baseline_m: state.baseline_m,
            reported_sd_ambiguities_m: None,
            integer_ratio: 0.0,
            integer_fixed: !fixed_ids.is_empty(),
            newly_fixed: Vec::new(),
            fixed_ids,
            innovation_screen,
            state,
        });
    }

    let (updated_holds, integer_ratio) = search_and_hold(
        &state,
        &state.information,
        epoch,
        wavelengths,
        offsets,
        held,
        &opts.float_only_systems,
        opts.ar_arming_sigma_m,
        &opts.search,
    )?;

    let previous_fixed: std::collections::BTreeSet<String> =
        state.fixed_cycles.keys().cloned().collect();
    let (fixed_cycles, fixed_m) = fixed_maps_from_holds(&updated_holds, wavelengths, offsets)?;
    let fixed_ids: Vec<String> = fixed_cycles.keys().cloned().collect();
    let newly_fixed: Vec<String> = fixed_ids
        .iter()
        .filter(|id| !previous_fixed.contains(*id))
        .cloned()
        .collect();

    state.fixed_cycles = fixed_cycles;
    state.fixed_m = fixed_m;
    state.epoch_count += 1;

    // Reported (fixed) baseline: when AR newly succeeds, re-solve THIS epoch with
    // the new integers held so the reported baseline reflects the fix in the same
    // epoch. Re-uses the same prior (`state`) as the float iterate, swapping the
    // held set for the post-fix one. Without this the first fixed epoch reports
    // its float baseline while claiming a fix (the cold-start false confidence).
    let (reported_baseline_m, reported_sd_ambiguities_m) = if newly_fixed.is_empty() {
        (state.baseline_m, None)
    } else {
        let prior_state = prior_state_for_report.expect("search targets clone prior state");
        let new_held = held_from_state(&state)?;
        let screen_mask = innovation_screen
            .as_ref()
            .map(|_| scratch.screen_mask.as_slice());
        iterate_epoch_into(
            &prior_state,
            epoch,
            base,
            model,
            &new_held,
            opts.hold_sigma_m,
            opts.position_tol_m,
            opts.ambiguity_tol_m,
            opts.max_iterations,
            opts.receiver_antenna_corrections.as_ref(),
            screen_mask,
            &mut scratch.report_iterate,
        )
        .map(|conditioned| {
            (
                conditioned.baseline_m,
                Some(conditioned.sd_ambiguities_m.to_vec()),
            )
        })
        .unwrap_or((state.baseline_m, None))
    };

    Ok(EpochUpdate {
        state,
        reported_baseline_m,
        reported_sd_ambiguities_m,
        integer_ratio,
        integer_fixed: !fixed_ids.is_empty(),
        newly_fixed,
        fixed_ids,
        innovation_screen,
    })
}

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

    // Per-system reference map from reference satellite ids (system = first byte).
    fn refs_of(ids: &[&str]) -> BTreeMap<String, String> {
        ids.iter()
            .map(|id| (id[..1].to_string(), id.to_string()))
            .collect()
    }

    // Reference prior: diagonal information, 1/σ² on baseline (i<3) and each
    // ambiguity diagonal, zero off-diagonal (Elixir sequential_initial_information).
    fn presized(n: usize, baseline_sigma: f64, ambiguity_sigma: f64) -> Vec<f64> {
        let mut m = vec![0.0; n * n];
        for i in 0..n {
            m[i * n + i] = if i < 3 {
                1.0 / (baseline_sigma * baseline_sigma)
            } else {
                1.0 / (ambiguity_sigma * ambiguity_sigma)
            };
        }
        m
    }

    #[test]
    fn new_seeds_diagonal_baseline_information() {
        let s = FilterState::new(refs_of(&["G01"]), [1.0, 2.0, 3.0], 10.0, 5.7);
        assert_eq!(s.dim(), 3);
        assert_eq!(s.information, presized(3, 10.0, 5.7));
        assert_eq!(s.baseline_m, [1.0, 2.0, 3.0]);
        assert!(s.sd_ambiguity_ids.is_empty());
    }

    #[test]
    fn growing_columns_matches_presized_prior() {
        // Streaming add-on-first-sighting must equal batch pre-sizing.
        let mut s = FilterState::new(refs_of(&["G01"]), [0.0; 3], 10.0, 5.7);
        s.ensure_ambiguity("G02", 0.42);
        s.ensure_ambiguity("G05~ra1", -1.3);
        assert_eq!(s.dim(), 5);
        assert_eq!(s.information, presized(5, 10.0, 5.7));
        assert_eq!(s.sd_ambiguity_ids, vec!["G02", "G05~ra1"]);
        assert_eq!(s.sd_ambiguities_m, vec![0.42, -1.3]);
        assert_eq!(s.index_of("G02"), Some(3));
        assert_eq!(s.index_of("G05~ra1"), Some(4));
        assert_eq!(s.index_of("absent"), None);
    }

    #[test]
    fn ensure_ambiguity_is_idempotent() {
        let mut s = FilterState::new(refs_of(&["G01"]), [0.0; 3], 10.0, 5.7);
        s.ensure_ambiguity("G02", 0.42);
        s.ensure_ambiguity("G02", 99.0); // same id: no-op, value unchanged
        assert_eq!(s.dim(), 4);
        assert_eq!(s.sd_ambiguities_m, vec![0.42]);
    }

    #[test]
    fn normal_equations_fold_and_solve_recover_known_state() {
        // Two correlated measurements of a 2-state system:
        //   x0 + x1 = 10, x0 - x1 = 2  ->  x = [6, 4].
        let n = 2;
        let mut lambda = vec![0.0; n * n];
        let mut eta = vec![0.0; n];
        fold_measurement(&mut lambda, &mut eta, &[1.0, 1.0], 1.0, 10.0);
        fold_measurement(&mut lambda, &mut eta, &[1.0, -1.0], 1.0, 2.0);
        // Λ = [[2,0],[0,2]] (the off-diagonal +1 and -1 cancel), η = [12, 8].
        assert_eq!(lambda, vec![2.0, 0.0, 0.0, 2.0]);
        assert_eq!(eta, vec![12.0, 8.0]);
        let x = solve_normal(&lambda, &eta).unwrap();
        assert!((x[0] - 6.0).abs() < 1e-12);
        assert!((x[1] - 4.0).abs() < 1e-12);
    }

    #[test]
    fn solve_normal_reports_singular() {
        // Λ = [[1,2],[2,4]] is singular.
        assert!(solve_normal(&[1.0, 2.0, 2.0, 4.0], &[1.0, 2.0]).is_none());
    }

    #[test]
    fn weighted_measurement_recovers_weighted_mean() {
        // Scalar state, two measurements with weights 1 and 3 -> weighted mean.
        let mut lambda = vec![0.0];
        let mut eta = vec![0.0];
        fold_measurement(&mut lambda, &mut eta, &[1.0], 1.0, 10.0);
        fold_measurement(&mut lambda, &mut eta, &[1.0], 3.0, 12.0);
        let x = solve_normal(&lambda, &eta).unwrap();
        assert!((x[0] - 11.5).abs() < 1e-12); // (10 + 3*12) / 4
    }

    #[test]
    fn measurement_block_uses_shared_reference_covariance() {
        // Two DD measurements with equal single-difference variances v=2:
        // R = v*(I + J) = [[4, 2], [2, 4]], so
        // R^-1 = [[1/3, -1/6], [-1/6, 1/3]]. Diagonal row folding would produce
        // [[1/4, 0], [0, 1/4]] instead and miss the reference-satellite coupling.
        let row1 = DdRow {
            kind: RowKind::Code,
            sat: "G02".into(),
            ref_sat: "G01".into(),
            sd_ambiguity_id: "G02".into(),
            h: vec![1.0, 0.0],
            y: 10.0,
            sd_variance_m2: 2.0,
            ref_sd_variance_m2: 2.0,
            weight: 0.25,
        };
        let row2 = DdRow {
            kind: RowKind::Code,
            sat: "G03".into(),
            ref_sat: "G01".into(),
            sd_ambiguity_id: "G03".into(),
            h: vec![0.0, 1.0],
            y: 20.0,
            sd_variance_m2: 2.0,
            ref_sd_variance_m2: 2.0,
            weight: 0.25,
        };
        let rows = [&row1, &row2];

        let r_inv = double_difference_inverse_covariance(&rows).unwrap();
        let expected = vec![1.0 / 3.0, -1.0 / 6.0, -1.0 / 6.0, 1.0 / 3.0];
        for (got, want) in r_inv.iter().zip(&expected) {
            assert!((got - want).abs() < 1e-15, "{got} != {want}");
        }

        let mut lambda = vec![0.0; 4];
        let mut eta = vec![0.0; 2];
        fold_measurement_block(&mut lambda, &mut eta, &rows).unwrap();

        for (got, want) in lambda.iter().zip(&expected) {
            assert!((got - want).abs() < 1e-15, "{got} != {want}");
        }
        assert!(eta[0].abs() < 1e-14, "eta[0] = {}", eta[0]);
        assert!((eta[1] - 5.0).abs() < 1e-14, "eta[1] = {}", eta[1]);
    }

    // Reference values below are computed independently from the exact formulas
    // (geocentric up = base/|base|, sin el = LOS·up, clamp at 0.05) at full f64
    // precision — they pin the parity semantics, not Rust self-consistency.
    const VAR_BASE: [f64; 3] = [4_075_580.0, 931_854.0, 4_801_568.0];
    const SAT_HIGH: [f64; 3] = [15_000_000.0, 7_000_000.0, 21_000_000.0]; // sin el ≈ 0.982
    const SAT_LOW: [f64; 3] = [-12_000_000.0, 18_000_000.0, 19_000_000.0]; // sin el ≈ 0.106

    #[test]
    fn elevation_sin_is_geocentric_and_matches_reference() {
        let high = elevation_sin(VAR_BASE, SAT_HIGH);
        let low = elevation_sin(VAR_BASE, SAT_LOW);
        assert!((high - 0.9823712838936762).abs() < 1e-15, "{high}");
        assert!((low - 0.10636728642290873).abs() < 1e-15, "{low}");

        // Straight down (sat at base/2): LOS is -up, sin el = -1 (to FP rounding;
        // the dot of two opposite unit vectors can land an ulp outside [-1, 1]).
        let below = elevation_sin(VAR_BASE, [2_037_790.0, 465_927.0, 2_400_784.0]);
        assert!((below + 1.0).abs() < 1e-12, "{below}");
    }

    #[test]
    fn variance_models_match_reference_formulas() {
        let sigma = 0.3;
        let simple = StochasticModel::Simple {
            elevation_weighting: false,
        };
        let elev = StochasticModel::Simple {
            elevation_weighting: true,
        };

        // Simple: 2σ², elevation-independent.
        assert_eq!(
            single_difference_variance(sigma, simple, VAR_BASE, SAT_LOW),
            0.18
        );

        // Elevation-weighted: 2(σ/sin el)².
        let v = single_difference_variance(sigma, elev, VAR_BASE, SAT_HIGH);
        assert!((v - 0.18651818780129176).abs() < 1e-15, "{v}");
        let v = single_difference_variance(sigma, elev, VAR_BASE, SAT_LOW);
        assert!((v - 15.909493196935287).abs() < 1e-12, "{v}");

        // RTKLIB: 2(σ² + σ²/sin²el).
        let v = single_difference_variance(sigma, StochasticModel::Rtklib, VAR_BASE, SAT_HIGH);
        assert!((v - 0.36651818780129175).abs() < 1e-15, "{v}");
        let v = single_difference_variance(sigma, StochasticModel::Rtklib, VAR_BASE, SAT_LOW);
        assert!((v - 16.089493196935287).abs() < 1e-12, "{v}");

        // Below the horizon, sin el clamps to 0.05: elevation-weighted variance
        // is 2(0.3/0.05)² = 72 and RTKLIB 2(0.09 + 0.09/0.0025) = 72.18 (to FP
        // rounding; 0.3/0.05 is not exact in binary).
        let down = [2_037_790.0, 465_927.0, 2_400_784.0];
        let v = single_difference_variance(sigma, elev, VAR_BASE, down);
        assert!((v - 72.0).abs() < 1e-12, "{v}");
        let v = single_difference_variance(sigma, StochasticModel::Rtklib, VAR_BASE, down);
        assert!((v - 72.18).abs() < 1e-12, "{v}");
    }

    #[test]
    fn unequal_variance_block_matches_dense_inverse() {
        // RTKLIB variances at distinct elevations exercise the general
        // Sherman-Morrison path (unequal D) that the Elixir fast path lacks.
        // Reference R⁻¹ computed independently by dense inversion of
        // R = D + v_ref·11ᵀ with D = diag(16.089493…, 15.965508…), v_ref = 0.366518….
        let mk = |sat: &str, var: f64, ref_var: f64, h: Vec<f64>, y: f64| DdRow {
            kind: RowKind::Code,
            sat: sat.into(),
            ref_sat: "G01".into(),
            sd_ambiguity_id: sat.into(),
            h,
            y,
            sd_variance_m2: var,
            ref_sd_variance_m2: ref_var,
            weight: 1.0 / (var + ref_var),
        };
        let v_ref = 0.36651818780129175;
        let row1 = mk("G02", 16.089493196935287, v_ref, vec![1.0, 0.0], 1.0);
        let row2 = mk("G03", 15.965508421574619, v_ref, vec![0.0, 1.0], 1.0);
        let r_inv = double_difference_inverse_covariance(&[&row1, &row2]).unwrap();

        let expected = [
            0.06079845603340234,
            -0.0013644197661107447,
            -0.0013644197661107447,
            0.06126000823962085,
        ];
        for (got, want) in r_inv.iter().zip(&expected) {
            assert!((got - want).abs() < 1e-15, "{got} != {want}");
        }
    }

    #[test]
    fn baseline_block_is_preserved_when_growing() {
        // A non-trivial baseline information block must survive column growth.
        let mut s = FilterState::new(refs_of(&["G01"]), [0.0; 3], 10.0, 5.7);
        // Simulate accumulated baseline correlation.
        s.information[1] = 0.001; // [0][1]
        s.information[3] = 0.001; // [1][0]
        s.ensure_ambiguity("G02", 0.0);
        assert_eq!(s.info(0, 1), 0.001);
        assert_eq!(s.info(1, 0), 0.001);
        assert_eq!(s.info(3, 3), 1.0 / (5.7 * 5.7));
        assert_eq!(s.info(0, 3), 0.0);
    }

    #[test]
    fn dd_rows_match_perfect_synthetic_geometry() {
        let base = [4_075_580.0, 931_854.0, 4_801_568.0];
        let baseline = [1.2, -0.85, 0.91];
        let rover = [
            base[0] + baseline[0],
            base[1] + baseline[1],
            base[2] + baseline[2],
        ];
        let g01 = [15_000_000.0, 7_000_000.0, 21_000_000.0];
        let g02 = [-12_000_000.0, 18_000_000.0, 19_000_000.0];
        let amb_g01 = 3.0;
        let amb_g02 = -7.0;

        // Perfect synthetic observations: code = geometric range, phase = range +
        // ambiguity, zero receiver clocks (they cancel in the double difference).
        let mk = |sat: [f64; 3], id: &str, amb: f64| SatMeas {
            sat: id.into(),
            sd_ambiguity_id: id.into(),
            base_code_m: range_m(sat, base),
            base_phase_m: range_m(sat, base),
            rover_code_m: range_m(sat, rover),
            rover_phase_m: range_m(sat, rover) + amb,
            base_tx_pos: sat,
            rover_tx_pos: sat,
            pos: sat,
        };
        let epoch = Epoch {
            references: vec![mk(g01, "G01", amb_g01)],
            nonref: vec![mk(g02, "G02", amb_g02)],
            velocity_mps: None,
            dt_s: 0.0,
        };

        let mut state = FilterState::new(refs_of(&["G01"]), baseline, 10.0, 5.7);
        state.ensure_ambiguity("G01", amb_g01);
        state.ensure_ambiguity("G02", amb_g02);

        let model = MeasModel {
            code_sigma_m: 0.3,
            phase_sigma_m: 0.003,
            sagnac: false,
            stochastic: StochasticModel::Simple {
                elevation_weighting: false,
            },
        };
        let rows = epoch_dd_rows(&epoch, base, &state, &model).unwrap();

        assert_eq!(rows.len(), 2);
        let (code, phase) = (&rows[0], &rows[1]);
        assert_eq!(code.kind, RowKind::Code);
        assert_eq!(phase.kind, RowKind::Phase);

        // Perfect synthetic measurement -> ~zero prefit residual.
        assert!(code.y.abs() < 1e-6, "code y = {}", code.y);
        assert!(phase.y.abs() < 1e-6, "phase y = {}", phase.y);

        // Geometry block = LOS(rover->G02) - LOS(rover->G01).
        let expected = sub3(range_derivative(rover, g02), range_derivative(rover, g01));
        for (k, &e) in expected.iter().enumerate() {
            assert!((code.h[k] - e).abs() < 1e-12);
            assert!((phase.h[k] - e).abs() < 1e-12);
        }
        // Code carries no ambiguity columns; phase has +1 at the sat SD column
        // (G02 -> index 4) and -1 at the reference SD column (G01 -> index 3).
        assert_eq!(code.h[3], 0.0);
        assert_eq!(code.h[4], 0.0);
        assert_eq!(phase.h[3], -1.0);
        assert_eq!(phase.h[4], 1.0);

        // Information weights = 1/(4σ²) (simple model: SD var 2σ², DD = sum).
        assert!((code.weight - 1.0 / (4.0 * 0.3 * 0.3)).abs() < 1e-12);
        assert!((phase.weight - 1.0 / (4.0 * 0.003 * 0.003)).abs() < 1e-9);
    }

    #[test]
    fn receiver_antenna_pcv_interpolates_zenith_and_azimuth() {
        let cal = ReceiverAntennaCalibration {
            pco_neu_m: [0.0, 0.0, 0.0],
            noazi_pcv_m: vec![(0.0, 0.0), (10.0, 10.0)],
            azi_pcv_m: vec![
                (0.0, 0.0, 0.0),
                (0.0, 10.0, 10.0),
                (90.0, 0.0, 90.0),
                (90.0, 10.0, 100.0),
            ],
        };
        let mut scratch = ReceiverAntennaScratch::default();

        assert_eq!(pcv_m(&cal, 5.0, 45.0, &mut scratch), 50.0);
        assert_eq!(pcv_m(&cal, -1.0, 45.0, &mut scratch), 45.0);
        assert_eq!(pcv_m(&cal, 99.0, 45.0, &mut scratch), 55.0);
    }

    #[test]
    fn corrected_dd_rows_zero_synthetic_receiver_antenna_residuals() {
        let base = [4_075_580.0, 931_854.0, 4_801_568.0];
        let baseline = [1.2, -0.85, 0.91];
        let rover = [
            base[0] + baseline[0],
            base[1] + baseline[1],
            base[2] + baseline[2],
        ];
        let g01 = [15_000_000.0, 7_000_000.0, 21_000_000.0];
        let g02 = [-12_000_000.0, 18_000_000.0, 19_000_000.0];
        let base_cal = ReceiverAntennaCalibration {
            pco_neu_m: [0.0, 0.0, 0.10],
            noazi_pcv_m: Vec::new(),
            azi_pcv_m: Vec::new(),
        };
        let rover_cal = ReceiverAntennaCalibration {
            pco_neu_m: [0.0, 0.0, 0.05],
            noazi_pcv_m: Vec::new(),
            azi_pcv_m: Vec::new(),
        };
        let corrections = ReceiverAntennaCorrections {
            base: base_cal,
            rover: rover_cal,
        };
        let mut antenna_scratch = ReceiverAntennaScratch::default();
        let mk = |sat: [f64; 3],
                  id: &str,
                  corrections: &ReceiverAntennaCorrections,
                  scratch: &mut ReceiverAntennaScratch| {
            let base_corr = receiver_antenna_correction(sat, base, &corrections.base, scratch);
            let rover_corr = receiver_antenna_correction(sat, rover, &corrections.rover, scratch);
            SatMeas {
                sat: id.into(),
                sd_ambiguity_id: id.into(),
                base_code_m: range_m(sat, base) - base_corr,
                base_phase_m: range_m(sat, base) - base_corr,
                rover_code_m: range_m(sat, rover) - rover_corr,
                rover_phase_m: range_m(sat, rover) - rover_corr,
                base_tx_pos: sat,
                rover_tx_pos: sat,
                pos: sat,
            }
        };
        let epoch = Epoch {
            references: vec![mk(g01, "G01", &corrections, &mut antenna_scratch)],
            nonref: vec![mk(g02, "G02", &corrections, &mut antenna_scratch)],
            velocity_mps: None,
            dt_s: 0.0,
        };
        let mut state = FilterState::new(refs_of(&["G01"]), baseline, 10.0, 5.7);
        state.ensure_ambiguity("G01", 0.0);
        state.ensure_ambiguity("G02", 0.0);
        let model = MeasModel {
            code_sigma_m: 0.3,
            phase_sigma_m: 0.003,
            sagnac: false,
            stochastic: StochasticModel::Simple {
                elevation_weighting: false,
            },
        };
        let mut scratch = EpochRowsScratch::default();
        let rows = epoch_dd_rows_into(
            &epoch,
            base,
            &state,
            state.baseline_m,
            &state.sd_ambiguities_m,
            &model,
            Some(&corrections),
            &mut scratch,
        )
        .unwrap();

        assert_eq!(rows.len(), 2);
        assert_eq!(rows[0].kind, RowKind::Code);
        assert_eq!(rows[1].kind, RowKind::Phase);
        assert!(rows[0].y.abs() < 1.0e-8, "code y = {}", rows[0].y);
        assert!(rows[1].y.abs() < 1.0e-8, "phase y = {}", rows[1].y);
    }

    #[test]
    fn iterate_epoch_recovers_baseline_and_dd_ambiguities() {
        let base = [4_075_580.0, 931_854.0, 4_801_568.0];
        let truth = [1.2, -0.85, 0.91];
        let rover = [base[0] + truth[0], base[1] + truth[1], base[2] + truth[2]];
        // Reference G01 + 4 non-reference satellites; true SD ambiguities (m).
        let sats: [(&str, [f64; 3], f64); 5] = [
            ("G01", [15_000_000.0, 7_000_000.0, 21_000_000.0], 0.0),
            ("G02", [-12_000_000.0, 18_000_000.0, 19_000_000.0], 0.6),
            ("G03", [20_000_000.0, -10_000_000.0, 17_000_000.0], -1.4),
            ("G04", [-19_000_000.0, -13_000_000.0, 20_000_000.0], 1.0),
            ("G05", [9_000_000.0, 22_000_000.0, 16_000_000.0], -0.3),
        ];
        let mk = |pos: [f64; 3], id: &str, amb: f64| SatMeas {
            sat: id.into(),
            sd_ambiguity_id: id.into(),
            base_code_m: range_m(pos, base),
            base_phase_m: range_m(pos, base),
            rover_code_m: range_m(pos, rover),
            rover_phase_m: range_m(pos, rover) + amb,
            base_tx_pos: pos,
            rover_tx_pos: pos,
            pos,
        };
        let epoch = Epoch {
            references: vec![mk(sats[0].1, sats[0].0, sats[0].2)],
            nonref: sats[1..].iter().map(|&(id, p, a)| mk(p, id, a)).collect(),
            velocity_mps: None,
            dt_s: 0.0,
        };

        // Wrong initial baseline + very loose priors so the measurement dominates
        // (a finite prior pulling the wrong center leaves a bias ~ prior_info /
        // meas_info × offset; σ=1e4 makes it ~1e-7 m, negligible).
        let mut state = FilterState::new(refs_of(&["G01"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4);
        for &(id, _, _) in &sats {
            state.ensure_ambiguity(id, 0.0);
        }
        let model = MeasModel {
            code_sigma_m: 0.3,
            phase_sigma_m: 0.003,
            sagnac: false,
            stochastic: StochasticModel::Simple {
                elevation_weighting: false,
            },
        };

        let post = iterate_epoch(&state, &epoch, base, &model, &[], 1.0, 1e-3, 1e-6, 10).unwrap();

        // Baseline converges to truth.
        for (k, &t) in truth.iter().enumerate() {
            assert!(
                (post.baseline_m[k] - t).abs() < 1e-3,
                "baseline[{k}] = {} (truth {t})",
                post.baseline_m[k]
            );
        }
        // DD float ambiguities (sat - ref) converge to the true DDs.
        let pos = |id: &str| state.ambiguity_pos(id).unwrap();
        let ref_amb = post.sd_ambiguities_m[pos("G01")];
        for &(id, _, true_amb) in &sats[1..] {
            let dd = post.sd_ambiguities_m[pos(id)] - ref_amb;
            assert!(
                (dd - true_amb).abs() < 1e-3,
                "{id} dd = {dd} (truth {true_amb})"
            );
        }
    }

    #[test]
    fn dd_covariance_transform_matches_hand_computation() {
        // SD ambiguity covariance (A=0, B=1, reference C=2).
        #[rustfmt::skip]
        let sd = vec![
            4.0, 1.0, 0.5,
            1.0, 3.0, 0.5,
            0.5, 0.5, 2.0,
        ];
        // DD targets A-C and B-C; λ = 1 (cycles == metres).
        let dd = [(0usize, 2usize), (1usize, 2usize)];
        // DD_cov[i][j] = C[si][sj] - C[si][rj] - C[ri][sj] + C[ri][rj]:
        //   [0][0] = 4 - .5 - .5 + 2 = 5 ; [0][1]=[1][0] = 1 - .5 - .5 + 2 = 2
        //   [1][1] = 3 - .5 - .5 + 2 = 4
        assert_eq!(
            dd_covariance_cycles(&sd, 3, &dd, &[1.0, 1.0]),
            vec![5.0, 2.0, 2.0, 4.0]
        );
        // Wavelength scaling divides entry (i,j) by λ_i·λ_j.
        assert_eq!(
            dd_covariance_cycles(&sd, 3, &dd, &[2.0, 1.0]),
            vec![5.0 / 4.0, 2.0 / 2.0, 2.0 / 2.0, 4.0 / 1.0]
        );
    }

    #[test]
    fn search_and_hold_fixes_integer_double_differences() {
        let lambda = 299_792_458.0 / 1_575_420_000.0;
        let base = [4_075_580.0, 931_854.0, 4_801_568.0];
        let truth = [1.2, -0.85, 0.91];
        let rover = [base[0] + truth[0], base[1] + truth[1], base[2] + truth[2]];
        // Reference G01 + 4 non-reference; integer cycle ambiguities so the DD
        // floats land on integers and the ratio test passes.
        let sats: [(&str, [f64; 3], i64); 5] = [
            ("G01", [15_000_000.0, 7_000_000.0, 21_000_000.0], 0),
            ("G02", [-12_000_000.0, 18_000_000.0, 19_000_000.0], 3),
            ("G03", [20_000_000.0, -10_000_000.0, 17_000_000.0], -7),
            ("G04", [-19_000_000.0, -13_000_000.0, 20_000_000.0], 12),
            ("G05", [9_000_000.0, 22_000_000.0, 16_000_000.0], -4),
        ];
        let mk = |pos: [f64; 3], id: &str, ncyc: i64| SatMeas {
            sat: id.into(),
            sd_ambiguity_id: id.into(),
            base_code_m: range_m(pos, base),
            base_phase_m: range_m(pos, base),
            rover_code_m: range_m(pos, rover),
            rover_phase_m: range_m(pos, rover) + (ncyc as f64) * lambda,
            base_tx_pos: pos,
            rover_tx_pos: pos,
            pos,
        };
        let epoch = Epoch {
            references: vec![mk(sats[0].1, sats[0].0, sats[0].2)],
            nonref: sats[1..].iter().map(|&(id, p, n)| mk(p, id, n)).collect(),
            velocity_mps: None,
            dt_s: 0.0,
        };

        let mut state = FilterState::new(refs_of(&["G01"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4);
        for &(id, _, _) in &sats {
            state.ensure_ambiguity(id, 0.0);
        }
        let model = MeasModel {
            code_sigma_m: 0.3,
            phase_sigma_m: 0.003,
            sagnac: false,
            stochastic: StochasticModel::Simple {
                elevation_weighting: false,
            },
        };
        let post = iterate_epoch(&state, &epoch, base, &model, &[], 1.0, 1e-3, 1e-6, 10).unwrap();

        let mut state_post = state.clone();
        state_post.baseline_m = post.baseline_m;
        state_post.sd_ambiguities_m = post.sd_ambiguities_m.clone();

        let wl: BTreeMap<String, f64> = sats[1..]
            .iter()
            .map(|&(id, _, _)| (id.to_string(), lambda))
            .collect();
        let off: BTreeMap<String, f64> = sats[1..]
            .iter()
            .map(|&(id, _, _)| (id.to_string(), 0.0))
            .collect();

        let (holds, ratio) = search_and_hold(
            &state_post,
            &post.information,
            &epoch,
            &wl,
            &off,
            &[],
            &[],
            None,
            &SearchOpts {
                ratio_threshold: 3.0,
            },
        )
        .unwrap();

        assert_eq!(holds.len(), 4);
        assert!(ratio >= 3.0, "ratio = {ratio}");
        let by_sat: BTreeMap<&str, &Hold> =
            holds.iter().map(|h| (h.sat_sd_id.as_str(), h)).collect();
        for &(id, _, ncyc) in &sats[1..] {
            let h = by_sat[id];
            assert_eq!(h.ref_sd_id, "G01");
            assert_eq!((h.fixed_m / lambda).round() as i64, ncyc, "{id}");
        }

        // AR arming gate: a threshold below any achievable baseline posterior
        // sigma withholds the fix; the same call without the gate fixes (above).
        let (gated_holds, gated_ratio) = search_and_hold(
            &state_post,
            &post.information,
            &epoch,
            &wl,
            &off,
            &[],
            &[],
            Some(1.0e-9),
            &SearchOpts {
                ratio_threshold: 3.0,
            },
        )
        .unwrap();

        assert!(gated_holds.is_empty());
        assert_eq!(gated_ratio, 0.0);
    }

    #[test]
    fn update_epoch_grows_searches_and_carries_held_ambiguities() {
        let lambda = 299_792_458.0 / 1_575_420_000.0;
        let base = [4_075_580.0, 931_854.0, 4_801_568.0];
        let truth = [1.2, -0.85, 0.91];
        let rover = [base[0] + truth[0], base[1] + truth[1], base[2] + truth[2]];
        let sats: [(&str, [f64; 3], i64); 5] = [
            ("G01", [15_000_000.0, 7_000_000.0, 21_000_000.0], 0),
            ("G02", [-12_000_000.0, 18_000_000.0, 19_000_000.0], 3),
            ("G03", [20_000_000.0, -10_000_000.0, 17_000_000.0], -7),
            ("G04", [-19_000_000.0, -13_000_000.0, 20_000_000.0], 12),
            ("G05", [9_000_000.0, 22_000_000.0, 16_000_000.0], -4),
        ];
        let mk = |pos: [f64; 3], id: &str, ncyc: i64| SatMeas {
            sat: id.into(),
            sd_ambiguity_id: id.into(),
            base_code_m: range_m(pos, base),
            base_phase_m: range_m(pos, base),
            rover_code_m: range_m(pos, rover),
            rover_phase_m: range_m(pos, rover) + (ncyc as f64) * lambda,
            base_tx_pos: pos,
            rover_tx_pos: pos,
            pos,
        };
        let epoch = Epoch {
            references: vec![mk(sats[0].1, sats[0].0, sats[0].2)],
            nonref: sats[1..].iter().map(|&(id, p, n)| mk(p, id, n)).collect(),
            velocity_mps: None,
            dt_s: 0.0,
        };
        let model = MeasModel {
            code_sigma_m: 0.3,
            phase_sigma_m: 0.003,
            sagnac: false,
            stochastic: StochasticModel::Simple {
                elevation_weighting: false,
            },
        };
        let wavelengths: BTreeMap<String, f64> = sats[1..]
            .iter()
            .map(|&(id, _, _)| (id.to_string(), lambda))
            .collect();
        let offsets: BTreeMap<String, f64> = sats[1..]
            .iter()
            .map(|&(id, _, _)| (id.to_string(), 0.0))
            .collect();
        let opts = UpdateOpts {
            hold_sigma_m: 1.0,
            position_tol_m: 1e-3,
            ambiguity_tol_m: 1e-6,
            max_iterations: 10,
            process_noise_baseline_sigma_m: 0.0,
            dynamics_model: DynamicsModel::ConstantPosition,
            float_only_systems: vec![],
            innovation_screen: None,
            receiver_antenna_corrections: None,
            ar_arming_sigma_m: None,
            search: SearchOpts {
                ratio_threshold: 3.0,
            },
        };

        let state = FilterState::new(refs_of(&["G01"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4);

        assert_eq!(
            update_epoch(
                FilterState::new(refs_of(&["G99"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4),
                &epoch,
                base,
                &model,
                &wavelengths,
                &offsets,
                &opts,
            ),
            Err(UpdateError::ReferenceChanged {
                system: "G".into(),
                expected: "G99".into(),
                actual: "G01".into()
            })
        );

        assert_eq!(
            update_epoch(
                state.clone(),
                &epoch,
                base,
                &model,
                &BTreeMap::new(),
                &offsets,
                &opts,
            ),
            Err(UpdateError::MissingWavelength("G02".into()))
        );

        let mut presized = state.clone();
        for &(id, _, _) in &sats {
            presized.ensure_ambiguity(id, 0.0);
        }
        let mut noisy_opts = opts.clone();
        noisy_opts.process_noise_baseline_sigma_m = 30.0;
        let first_presized_static = update_epoch(
            presized.clone(),
            &epoch,
            base,
            &model,
            &wavelengths,
            &offsets,
            &opts,
        )
        .unwrap();
        let first_presized_kinematic = update_epoch(
            presized,
            &epoch,
            base,
            &model,
            &wavelengths,
            &offsets,
            &noisy_opts,
        )
        .unwrap();
        assert_eq!(first_presized_kinematic.state.epoch_count, 1);
        assert_eq!(
            first_presized_kinematic.state.baseline_m,
            first_presized_static.state.baseline_m
        );
        assert_eq!(
            first_presized_kinematic.reported_baseline_m,
            first_presized_static.reported_baseline_m
        );

        let first =
            update_epoch(state, &epoch, base, &model, &wavelengths, &offsets, &opts).unwrap();

        assert_eq!(first.state.epoch_count, 1);
        assert_eq!(
            first.state.sd_ambiguity_ids,
            vec!["G01", "G02", "G03", "G04", "G05"]
        );
        assert!(first.integer_fixed);
        assert!(
            first.integer_ratio >= 3.0,
            "ratio = {}",
            first.integer_ratio
        );
        assert_eq!(first.newly_fixed, vec!["G02", "G03", "G04", "G05"]);
        assert_eq!(first.fixed_ids, vec!["G02", "G03", "G04", "G05"]);
        for (k, &t) in truth.iter().enumerate() {
            assert!((first.state.baseline_m[k] - t).abs() < 1e-3);
        }
        for &(id, _, ncyc) in &sats[1..] {
            assert_eq!(first.state.fixed_cycles[id], ncyc);
            assert!((first.state.fixed_m[id] - (ncyc as f64) * lambda).abs() < 1e-9);
        }

        let second = update_epoch(
            first.state.clone(),
            &epoch,
            base,
            &model,
            &wavelengths,
            &offsets,
            &opts,
        )
        .unwrap();

        assert!(second.integer_fixed);
        assert_eq!(second.state.epoch_count, 2);
        assert_eq!(second.integer_ratio, 0.0);
        assert!(second.newly_fixed.is_empty());
        assert_eq!(second.fixed_ids, vec!["G02", "G03", "G04", "G05"]);
        assert_eq!(second.state.fixed_cycles, first.state.fixed_cycles);
    }

    #[test]
    fn velocity_prediction_moves_prior_after_first_epoch_only_when_enabled() {
        let epoch = Epoch {
            references: Vec::new(),
            nonref: Vec::new(),
            velocity_mps: Some([0.5, -1.0, 2.0]),
            dt_s: 4.0,
        };
        let mut opts = UpdateOpts {
            hold_sigma_m: 1.0,
            position_tol_m: 1e-3,
            ambiguity_tol_m: 1e-6,
            max_iterations: 10,
            process_noise_baseline_sigma_m: 0.0,
            dynamics_model: DynamicsModel::VelocityPropagated,
            float_only_systems: vec![],
            innovation_screen: None,
            receiver_antenna_corrections: None,
            ar_arming_sigma_m: None,
            search: SearchOpts {
                ratio_threshold: 3.0,
            },
        };

        let mut first = FilterState::new(refs_of(&["G01"]), [1.0, 2.0, 3.0], 10.0, 10.0);
        propagate_baseline_mean(&mut first, &epoch, &opts);
        assert_eq!(first.baseline_m, [1.0, 2.0, 3.0]);

        let mut later = FilterState::new(refs_of(&["G01"]), [1.0, 2.0, 3.0], 10.0, 10.0);
        later.epoch_count = 1;
        propagate_baseline_mean(&mut later, &epoch, &opts);
        assert_eq!(later.baseline_m, [3.0, -2.0, 11.0]);

        opts.dynamics_model = DynamicsModel::ConstantPosition;
        let mut default_path = FilterState::new(refs_of(&["G01"]), [1.0, 2.0, 3.0], 10.0, 10.0);
        default_path.epoch_count = 1;
        propagate_baseline_mean(&mut default_path, &epoch, &opts);
        assert_eq!(default_path.baseline_m, [1.0, 2.0, 3.0]);
    }

    // ---------------------------------------------------------------------
    // Multi-GNSS per-system references (Track B)
    // ---------------------------------------------------------------------

    const MG_BASE: [f64; 3] = [4_075_580.0, 931_854.0, 4_801_568.0];
    const MG_TRUTH: [f64; 3] = [1.2, -0.85, 0.91];
    // G ref + 3 G nonref, E ref + 2 E nonref; integer cycle ambiguities.
    const MG_SATS: [(&str, [f64; 3], i64); 7] = [
        ("G01", [15_000_000.0, 7_000_000.0, 21_000_000.0], 0),
        ("G02", [-12_000_000.0, 18_000_000.0, 19_000_000.0], 3),
        ("G03", [20_000_000.0, -10_000_000.0, 17_000_000.0], -7),
        ("G04", [-19_000_000.0, -13_000_000.0, 20_000_000.0], 12),
        ("E05", [9_000_000.0, 22_000_000.0, 16_000_000.0], -4),
        ("E07", [-6_000_000.0, -20_000_000.0, 18_000_000.0], 8),
        ("E11", [23_000_000.0, 2_000_000.0, 17_500_000.0], 5),
    ];

    fn mg_epoch(lambda: f64) -> Epoch {
        let rover = [
            MG_BASE[0] + MG_TRUTH[0],
            MG_BASE[1] + MG_TRUTH[1],
            MG_BASE[2] + MG_TRUTH[2],
        ];
        let mk = |pos: [f64; 3], id: &str, ncyc: i64| SatMeas {
            sat: id.into(),
            sd_ambiguity_id: id.into(),
            base_code_m: range_m(pos, MG_BASE),
            base_phase_m: range_m(pos, MG_BASE),
            rover_code_m: range_m(pos, rover),
            rover_phase_m: range_m(pos, rover) + (ncyc as f64) * lambda,
            base_tx_pos: pos,
            rover_tx_pos: pos,
            pos,
        };
        Epoch {
            references: vec![
                mk(MG_SATS[0].1, MG_SATS[0].0, MG_SATS[0].2),
                mk(MG_SATS[4].1, MG_SATS[4].0, MG_SATS[4].2),
            ],
            nonref: [1usize, 2, 3, 5, 6]
                .iter()
                .map(|&i| mk(MG_SATS[i].1, MG_SATS[i].0, MG_SATS[i].2))
                .collect(),
            velocity_mps: None,
            dt_s: 0.0,
        }
    }

    fn mg_state() -> FilterState {
        let mut state =
            FilterState::new(refs_of(&["G01", "E05"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4);
        for &(id, _, _) in &MG_SATS {
            state.ensure_ambiguity(id, 0.0);
        }
        state
    }

    fn mg_model() -> MeasModel {
        MeasModel {
            code_sigma_m: 0.3,
            phase_sigma_m: 0.003,
            sagnac: false,
            stochastic: StochasticModel::Simple {
                elevation_weighting: false,
            },
        }
    }

    fn mg_maps(lambda: f64) -> (BTreeMap<String, f64>, BTreeMap<String, f64>) {
        let nonref = ["G02", "G03", "G04", "E07", "E11"];
        (
            nonref.iter().map(|&id| (id.to_string(), lambda)).collect(),
            nonref.iter().map(|&id| (id.to_string(), 0.0)).collect(),
        )
    }

    fn mg_opts() -> UpdateOpts {
        UpdateOpts {
            hold_sigma_m: 1.0,
            position_tol_m: 1e-3,
            ambiguity_tol_m: 1e-6,
            max_iterations: 10,
            process_noise_baseline_sigma_m: 0.0,
            dynamics_model: DynamicsModel::ConstantPosition,
            float_only_systems: vec![],
            innovation_screen: None,
            receiver_antenna_corrections: None,
            ar_arming_sigma_m: None,
            search: SearchOpts {
                ratio_threshold: 3.0,
            },
        }
    }

    #[test]
    fn multi_system_rows_pair_with_their_own_reference() {
        let lambda = 299_792_458.0 / 1_575_420_000.0;
        let epoch = mg_epoch(lambda);
        let state = mg_state();
        let rows = epoch_dd_rows(&epoch, MG_BASE, &state, &mg_model()).unwrap();

        assert_eq!(rows.len(), 10);
        for row in &rows {
            let expected_ref = if row.sat.starts_with('G') {
                "G01"
            } else {
                "E05"
            };
            assert_eq!(row.ref_sat, expected_ref, "{}", row.sat);
            if row.kind == RowKind::Phase {
                // +1 at the sat SD column, -1 at the OWN system's reference column.
                let sat_col = 3 + state.ambiguity_pos(&row.sd_ambiguity_id).unwrap();
                let ref_col = 3 + state.ambiguity_pos(expected_ref).unwrap();
                assert_eq!(row.h[sat_col], 1.0);
                assert_eq!(row.h[ref_col], -1.0);
            }
        }

        // A non-reference satellite whose system has no reference makes the row
        // builder return None (update_epoch guards this with a typed error).
        let mut orphan = epoch.clone();
        orphan.references.retain(|r| r.sat != "E05");
        assert!(epoch_dd_rows(&orphan, MG_BASE, &state, &mg_model()).is_none());
    }

    #[test]
    fn multi_system_update_fixes_per_system_integers() {
        let lambda = 299_792_458.0 / 1_575_420_000.0;
        let epoch = mg_epoch(lambda);
        let (wl, off) = mg_maps(lambda);
        let update = update_epoch(
            mg_state(),
            &epoch,
            MG_BASE,
            &mg_model(),
            &wl,
            &off,
            &mg_opts(),
        )
        .unwrap();

        assert!(update.integer_fixed);
        assert_eq!(update.fixed_ids, vec!["E07", "E11", "G02", "G03", "G04"]);
        for (k, &t) in MG_TRUTH.iter().enumerate() {
            assert!((update.state.baseline_m[k] - t).abs() < 1e-3);
        }
        // Fixed integers are the per-system DDs (sat - own reference).
        let cycles: BTreeMap<&str, i64> = MG_SATS.iter().map(|&(id, _, n)| (id, n)).collect();
        for (id, &fixed) in &update.state.fixed_cycles {
            let ref_id = if id.starts_with('G') { "G01" } else { "E05" };
            assert_eq!(fixed, cycles[id.as_str()] - cycles[ref_id], "{id}");
        }
    }

    #[test]
    fn float_only_systems_stay_out_of_the_search_set() {
        let lambda = 299_792_458.0 / 1_575_420_000.0;
        let epoch = mg_epoch(lambda);
        let (wl, off) = mg_maps(lambda);
        let mut opts = mg_opts();
        opts.float_only_systems = vec!["E".to_string()];
        let update =
            update_epoch(mg_state(), &epoch, MG_BASE, &mg_model(), &wl, &off, &opts).unwrap();

        // Galileo never enters the integer search; GPS still fixes.
        assert!(update.integer_fixed);
        assert_eq!(update.fixed_ids, vec!["G02", "G03", "G04"]);
        assert!(!update
            .state
            .fixed_cycles
            .keys()
            .any(|id| id.starts_with('E')));
    }

    #[test]
    fn multi_system_reference_guards_are_typed() {
        let lambda = 299_792_458.0 / 1_575_420_000.0;
        let epoch = mg_epoch(lambda);
        let (wl, off) = mg_maps(lambda);

        // Untracked constellation.
        let g_only = FilterState::new(refs_of(&["G01"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4);
        assert_eq!(
            update_epoch(g_only, &epoch, MG_BASE, &mg_model(), &wl, &off, &mg_opts()),
            Err(UpdateError::UnknownReferenceSystem("E".into()))
        );

        // Reference ambiguity arc changed within a tracked constellation.
        let changed =
            FilterState::new(refs_of(&["G01", "E99"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4);
        assert_eq!(
            update_epoch(changed, &epoch, MG_BASE, &mg_model(), &wl, &off, &mg_opts()),
            Err(UpdateError::ReferenceChanged {
                system: "E".into(),
                expected: "E99".into(),
                actual: "E05".into()
            })
        );

        // Non-reference satellite with no same-system reference this epoch.
        let mut orphan = epoch.clone();
        orphan.references.retain(|r| r.sat != "E05");
        let state = FilterState::new(refs_of(&["G01", "E05"]), [-30.0, 25.0, -10.0], 1.0e4, 1.0e4);
        assert_eq!(
            update_epoch(state, &orphan, MG_BASE, &mg_model(), &wl, &off, &mg_opts()),
            Err(UpdateError::MissingSystemReference("E".into()))
        );
    }
}