astrodynamics-gnss 0.9.7

//! Bounded integer least squares — the integer-ambiguity-resolution kernel for
//! precise / RTK positioning.
//!
//! This is a **bit-identical** Rust port of the reference Elixir implementation
//! (`Orbis.GNSS.Core.IntegerLeastSquares` + `LinearAlgebra.invert_matrix`): the
//! same Gaussian elimination with partial pivoting (max-abs pivot, first-index
//! tie-break, `<= 1e-12` singular guard), the same `Δᵀ Q⁻¹ Δ` summation order
//! (i-outer, j-inner, left-associated products), the same lattice enumeration and
//! `{score, cycles}` candidate ordering. It therefore produces the same integer
//! selection, status, ratio, and scores as the Elixir reference — verified by a
//! parity test in orbis. All arithmetic is plain `*` / `-` / `+` (no FMA), per
//! the crate's 0-ULP reproducibility rule.
//!
//! It lives in Rust because the bounded search — and the partial-ambiguity subset
//! search built on top of it — is the compute hot path for multi-epoch RTK arcs.

use crate::{Error, Result};

/// Singular-pivot threshold (mirrors the Elixir `@pivot_epsilon`).
const PIVOT_EPSILON: f64 = 1.0e-12;

/// Why a bounded ILS search could not produce a result. Mapped by the NIF onto
/// the reference Elixir error tuples (`:singular_geometry`,
/// `{:no_integer_candidates, n}`, `{:too_many_integer_candidates, n, limit}`).
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum IlsError {
    /// The covariance matrix is singular (degenerate geometry).
    Singular,
    /// The lattice yielded no candidate (an empty search box).
    NoCandidates(usize),
    /// The lattice exceeded `candidate_limit`.
    TooManyCandidates { evaluated: usize, limit: usize },
    /// `float_cycles` was empty, or `covariance` was not exactly `n x n` for
    /// `n = float_cycles.len()` (`rows` is the offending row count, or the length
    /// of the first row that was not `n` wide).
    InvalidDimensions { n: usize, rows: usize },
    /// A `float_cycles` or `covariance` entry was NaN or infinite.
    NonFinite,
    /// The MLAMBDA search did not converge within `LAMBDA_LOOP_MAX` iterations
    /// (distinct from a singular/degenerate covariance).
    SearchLimitExceeded,
}

/// Validate inputs before any indexing or arithmetic: `covariance` must be a
/// square `n x n` matrix matching the number of float ambiguities (`n >= 1`),
/// and every value must be finite. Without the shape check an undersized
/// covariance indexes out of bounds (panic) and an oversized one is silently
/// truncated to a wrong-dimension submatrix; without the finite check NaN/Inf
/// propagate into a garbage "fix".
fn validate_inputs(
    float_cycles: &[f64],
    covariance: &[Vec<f64>],
) -> core::result::Result<(), IlsError> {
    let n = float_cycles.len();
    if n == 0 {
        return Err(IlsError::InvalidDimensions { n, rows: 0 });
    }
    if covariance.len() != n {
        return Err(IlsError::InvalidDimensions {
            n,
            rows: covariance.len(),
        });
    }
    for row in covariance {
        if row.len() != n {
            return Err(IlsError::InvalidDimensions { n, rows: row.len() });
        }
    }
    if float_cycles.iter().any(|v| !v.is_finite())
        || covariance.iter().flatten().any(|v| !v.is_finite())
    {
        return Err(IlsError::NonFinite);
    }
    Ok(())
}

/// Outcome of a bounded ILS search.
#[derive(Debug, Clone, PartialEq)]
pub struct IlsResult {
    /// Best integer vector, parallel to the input `float_cycles`.
    pub fixed: Vec<i64>,
    /// Whether the ratio test passes at the requested threshold.
    pub fixed_status: bool,
    /// Runner-up / best score ratio. `f64::INFINITY` when the best score is
    /// exactly zero with a positive runner-up; `0.0` when there is no runner-up.
    pub ratio: f64,
    /// Best (lowest) quadratic score `Δᵀ Q⁻¹ Δ`.
    pub best_score: f64,
    /// Runner-up score, if a second lattice point exists.
    pub second_best_score: Option<f64>,
    /// Number of lattice points evaluated.
    pub candidates_evaluated: usize,
    /// Symmetrized covariance actually used.
    pub covariance: Vec<Vec<f64>>,
    /// Symmetrized inverse covariance.
    pub covariance_inverse: Vec<Vec<f64>>,
}

/// Bounded integer least squares over the lattice within `radius` integers of
/// each rounded float ambiguity.
///
/// Returns the best integer vector and its ratio-test verdict, or an error when
/// the covariance is singular or the lattice exceeds `candidate_limit`.
pub fn bounded_ils_search(
    float_cycles: &[f64],
    covariance: &[Vec<f64>],
    radius: i64,
    candidate_limit: usize,
    ratio_threshold: f64,
) -> core::result::Result<IlsResult, IlsError> {
    validate_inputs(float_cycles, covariance)?;
    let q = symmetrize(covariance);
    let q_inv = symmetrize(&invert(&q).map_err(|_| IlsError::Singular)?);

    // Per-ambiguity candidate integers, ordered by |value - float| then value
    // (matches `integers_near/3`; the final top-two is order-independent, but we
    // mirror the reference exactly).
    let ranges: Vec<Vec<i64>> = float_cycles
        .iter()
        .map(|&f| {
            let center = f.round() as i64; // Elixir round/1: half away from zero
            integers_near(f, center - radius, center + radius)
        })
        .collect();

    let mut top: Vec<(f64, Vec<i64>)> = Vec::with_capacity(2);
    let mut evaluated: usize = 0;
    let mut current: Vec<i64> = Vec::with_capacity(float_cycles.len());

    enumerate(
        &ranges,
        0,
        &mut current,
        float_cycles,
        &q_inv,
        candidate_limit,
        &mut evaluated,
        &mut top,
    )?;

    let (best_score, fixed) = match top.first() {
        Some((s, c)) => (*s, c.clone()),
        None => return Err(IlsError::NoCandidates(evaluated)),
    };
    let second_best_score = top.get(1).map(|(s, _)| *s);
    let ratio = integer_ratio(best_score, second_best_score);

    Ok(IlsResult {
        fixed,
        fixed_status: ratio_pass(ratio, ratio_threshold),
        ratio,
        best_score,
        second_best_score,
        candidates_evaluated: evaluated,
        covariance: q,
        covariance_inverse: q_inv,
    })
}

// --- lattice enumeration -------------------------------------------------

#[allow(clippy::too_many_arguments)]
fn enumerate(
    ranges: &[Vec<i64>],
    depth: usize,
    current: &mut Vec<i64>,
    float_cycles: &[f64],
    q_inv: &[Vec<f64>],
    limit: usize,
    evaluated: &mut usize,
    top: &mut Vec<(f64, Vec<i64>)>,
) -> core::result::Result<(), IlsError> {
    if depth == ranges.len() {
        *evaluated += 1;
        if *evaluated > limit {
            return Err(IlsError::TooManyCandidates {
                evaluated: *evaluated,
                limit,
            });
        }
        let score = quadratic_score(float_cycles, current, q_inv);
        insert_top_two(top, score, current);
        return Ok(());
    }

    for &value in &ranges[depth] {
        current.push(value);
        enumerate(
            ranges,
            depth + 1,
            current,
            float_cycles,
            q_inv,
            limit,
            evaluated,
            top,
        )?;
        current.pop();
    }
    Ok(())
}

/// Keep the two lowest `(score, cycles)` candidates — same ordering as the
/// reference `integer_top_two/1` (score ascending, then cycles lexicographic).
fn insert_top_two(top: &mut Vec<(f64, Vec<i64>)>, score: f64, cycles: &[i64]) {
    top.push((score, cycles.to_vec()));
    top.sort_by(|(sa, ca), (sb, cb)| {
        sa.partial_cmp(sb)
            .unwrap_or(core::cmp::Ordering::Equal)
            .then_with(|| ca.cmp(cb))
    });
    top.truncate(2);
}

fn quadratic_score(float_cycles: &[f64], fixed: &[i64], q_inv: &[Vec<f64>]) -> f64 {
    let n = float_cycles.len();
    // delta = float - fixed, matching `a - z`.
    let deltas: Vec<f64> = (0..n).map(|i| float_cycles[i] - fixed[i] as f64).collect();

    // i-outer, j-inner, acc + delta[i] * q_inv[i][j] * delta[j] (left-assoc).
    let mut acc = 0.0;
    for i in 0..n {
        for j in 0..n {
            acc += deltas[i] * q_inv[i][j] * deltas[j];
        }
    }
    acc
}

fn integers_near(center: f64, low: i64, high: i64) -> Vec<i64> {
    let mut values: Vec<i64> = (low..=high).collect();
    values.sort_by(|&a, &b| {
        let da = (a as f64 - center).abs();
        let db = (b as f64 - center).abs();
        da.partial_cmp(&db)
            .unwrap_or(core::cmp::Ordering::Equal)
            .then_with(|| a.cmp(&b))
    });
    values
}

fn integer_ratio(best_score: f64, second_best_score: Option<f64>) -> f64 {
    match second_best_score {
        None => 0.0,
        Some(second) => {
            if best_score == 0.0 && second > 0.0 {
                f64::INFINITY
            } else if best_score == 0.0 {
                0.0
            } else {
                second / best_score
            }
        }
    }
}

fn ratio_pass(ratio: f64, threshold: f64) -> bool {
    ratio == f64::INFINITY || ratio >= threshold
}

// --- linear algebra (bit-identical to LinearAlgebra) ---------------------

fn symmetrize(m: &[Vec<f64>]) -> Vec<Vec<f64>> {
    let n = m.len();
    (0..n)
        .map(|i| (0..n).map(|j| (m[i][j] + m[j][i]) / 2.0).collect())
        .collect()
}

/// Invert by solving `A x = eᵢ` for each unit column, exactly as the reference
/// `invert_matrix/1`.
fn invert(a: &[Vec<f64>]) -> Result<Vec<Vec<f64>>> {
    let n = a.len();
    // columns[col] = the col-th column of A⁻¹.
    let mut columns: Vec<Vec<f64>> = Vec::with_capacity(n);
    for col in 0..n {
        let mut e = vec![0.0; n];
        e[col] = 1.0;
        columns.push(solve_linear(a, &e)?);
    }
    // A⁻¹[i][j] = columns[j][i].
    Ok((0..n)
        .map(|i| (0..n).map(|j| columns[j][i]).collect())
        .collect())
}

// Index-based loops mirror the reference Gaussian elimination (pivot scan and
// row updates index `rows` by position); an iterator form would obscure it.
#[allow(clippy::needless_range_loop)]
fn solve_linear(a: &[Vec<f64>], b: &[f64]) -> Result<Vec<f64>> {
    let n = b.len();
    // Augmented rows: each A row with bᵢ appended.
    let mut rows: Vec<Vec<f64>> = a
        .iter()
        .zip(b)
        .map(|(row, &bi)| {
            let mut r = row.clone();
            r.push(bi);
            r
        })
        .collect();

    // Forward elimination with partial pivoting.
    for col in 0..n {
        // Pivot: max |a[idx][col]| over idx in col..n, first index on a tie.
        let mut pivot_row = col;
        let mut pivot_abs = rows[col][col].abs();
        for idx in (col + 1)..n {
            let v = rows[idx][col].abs();
            if v > pivot_abs {
                pivot_abs = v;
                pivot_row = idx;
            }
        }
        if pivot_abs <= PIVOT_EPSILON {
            return Err(Error::InvalidInput("singular matrix".into()));
        }
        rows.swap(col, pivot_row);

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

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

// =========================================================================
// LAMBDA / MLAMBDA integer least squares (Teunissen 1995; Chang-Yang-Zhou 2005)
// -------------------------------------------------------------------------
// A faithful port of RTKLIB's `lambda()` (BSD-2, _tools/RTKLIB/src/lambda.c):
// LtDL factorization + integer-Gauss/permutation decorrelation reduction +
// modified-LAMBDA depth-first search. Unlike `bounded_ils_search` (a naive
// ±radius box that only finds the true ILS optimum when it lies within the box),
// this is a *correct* ILS solver for any positive-definite covariance — it is
// gated against RTKLIB's own committed reference vectors (incl. the strongly-
// correlated utest2 the box search cannot reach). Validation target is RTKLIB,
// not bit-identity; the algorithm differs, so agreement is to round-off.
//
// Matrices follow RTKLIB's COLUMN-MAJOR convention verbatim — element (row i,
// col j) of an n×n matrix is `flat[i + j*n]` — so the port reads line-for-line
// against lambda.c.

const LAMBDA_LOOP_MAX: usize = 10000;

#[inline]
fn lam_round(x: f64) -> f64 {
    (x + 0.5).floor() // RTKLIB ROUND(x) = floor(x+0.5)
}

#[inline]
fn lam_sgn(x: f64) -> f64 {
    if x <= 0.0 {
        -1.0
    } else {
        1.0
    }
}

/// LtDL factorization `Q = Lᵀ·diag(D)·L` (column-major). Returns `None` if Q is
/// not positive-definite (a pivot `D[i] <= 0`).
fn lam_ld(n: usize, q: &[f64]) -> Option<(Vec<f64>, Vec<f64>)> {
    let mut a = q.to_vec();
    let mut l = vec![0.0f64; n * n];
    let mut d = vec![0.0f64; n];
    for i in (0..n).rev() {
        d[i] = a[i + i * n];
        if d[i] <= 0.0 {
            return None;
        }
        let ai = d[i].sqrt();
        for j in 0..=i {
            l[i + j * n] = a[i + j * n] / ai;
        }
        for j in 0..i {
            for k in 0..=j {
                a[j + k * n] -= l[i + k * n] * l[i + j * n];
            }
        }
        for j in 0..=i {
            l[i + j * n] /= l[i + i * n];
        }
    }
    Some((l, d))
}

/// Integer Gauss transformation on column `j` using column `i`.
fn lam_gauss(n: usize, l: &mut [f64], z: &mut [f64], i: usize, j: usize) {
    let mu = lam_round(l[i + j * n]) as i64;
    if mu != 0 {
        let muf = mu as f64;
        for k in i..n {
            l[k + j * n] -= muf * l[k + i * n];
        }
        for k in 0..n {
            z[k + j * n] -= muf * z[k + i * n];
        }
    }
}

/// Permutation of adjacent ambiguities `j` and `j+1`.
fn lam_perm(n: usize, l: &mut [f64], d: &mut [f64], j: usize, del: f64, z: &mut [f64]) {
    let eta = d[j] / del;
    let lam = d[j + 1] * l[j + 1 + j * n] / del;
    d[j] = eta * d[j + 1];
    d[j + 1] = del;
    for k in 0..j {
        let a0 = l[j + k * n];
        let a1 = l[j + 1 + k * n];
        l[j + k * n] = -l[j + 1 + j * n] * a0 + a1;
        l[j + 1 + k * n] = eta * a0 + lam * a1;
    }
    l[j + 1 + j * n] = lam;
    for k in (j + 2)..n {
        l.swap(k + j * n, k + (j + 1) * n);
    }
    for k in 0..n {
        z.swap(k + j * n, k + (j + 1) * n);
    }
}

/// LAMBDA reduction: decorrelate via integer Gauss transformations + adjacent
/// permutations, accumulating the unimodular transform `Z`.
fn lam_reduction(n: usize, l: &mut [f64], d: &mut [f64], z: &mut [f64]) {
    let mut j: isize = n as isize - 2;
    let mut k: isize = n as isize - 2;
    while j >= 0 {
        let ju = j as usize;
        if j <= k {
            for i in (ju + 1)..n {
                lam_gauss(n, l, z, i, ju);
            }
        }
        let del = d[ju] + l[ju + 1 + ju * n] * l[ju + 1 + ju * n] * d[ju + 1];
        if del + 1.0e-6 < d[ju + 1] {
            lam_perm(n, l, d, ju, del, z);
            k = j;
            j = n as isize - 2;
        } else {
            j -= 1;
        }
    }
}

/// Modified-LAMBDA (mlambda) search for the `m` best integer vectors in the
/// decorrelated space. Returns `(zn, s)` where `zn` is `n*m` column-major
/// candidates and `s[k]` is the squared residual of candidate `k` (sorted
/// ascending). `None` on search-loop overflow.
fn lam_search(
    n: usize,
    m: usize,
    l: &[f64],
    d: &[f64],
    zs: &[f64],
) -> Option<(Vec<f64>, Vec<f64>)> {
    let mut s = vec![0.0f64; m];
    let mut zn = vec![0.0f64; n * m];
    let mut smat = vec![0.0f64; n * n];
    let mut dist = vec![0.0f64; n];
    let mut zb = vec![0.0f64; n];
    let mut z = vec![0.0f64; n];
    let mut step = vec![0.0f64; n];

    let mut nn: usize = 0;
    let mut imax: usize = 0;
    let mut maxdist = 1.0e99;

    let mut k: isize = n as isize - 1;
    let ku = k as usize;
    dist[ku] = 0.0;
    zb[ku] = zs[ku];
    z[ku] = lam_round(zb[ku]);
    let mut y = zb[ku] - z[ku];
    step[ku] = lam_sgn(y);

    let mut c = 0usize;
    while c < LAMBDA_LOOP_MAX {
        let kk = k as usize;
        let newdist = dist[kk] + y * y / d[kk];
        if newdist < maxdist {
            if k != 0 {
                k -= 1;
                let kk = k as usize;
                dist[kk] = newdist;
                for i in 0..=kk {
                    smat[kk + i * n] =
                        smat[kk + 1 + i * n] + (z[kk + 1] - zb[kk + 1]) * l[kk + 1 + i * n];
                }
                zb[kk] = zs[kk] + smat[kk + kk * n];
                z[kk] = lam_round(zb[kk]);
                y = zb[kk] - z[kk];
                step[kk] = lam_sgn(y);
            } else {
                if nn < m {
                    if nn == 0 || newdist > s[imax] {
                        imax = nn;
                    }
                    for i in 0..n {
                        zn[i + nn * n] = z[i];
                    }
                    s[nn] = newdist;
                    nn += 1;
                } else {
                    if newdist < s[imax] {
                        for i in 0..n {
                            zn[i + imax * n] = z[i];
                        }
                        s[imax] = newdist;
                        imax = 0;
                        for i in 0..m {
                            if s[imax] < s[i] {
                                imax = i;
                            }
                        }
                    }
                    maxdist = s[imax];
                }
                z[0] += step[0];
                y = zb[0] - z[0];
                step[0] = -step[0] - lam_sgn(step[0]);
            }
        } else {
            if k == n as isize - 1 {
                break;
            } else {
                k += 1;
                let kk = k as usize;
                z[kk] += step[kk];
                y = zb[kk] - z[kk];
                step[kk] = -step[kk] - lam_sgn(step[kk]);
            }
        }
        c += 1;
    }

    if c >= LAMBDA_LOOP_MAX {
        return None;
    }

    // Sort the m candidates by ascending residual (RTKLIB's selection sort).
    for i in 0..m.saturating_sub(1) {
        for j in (i + 1)..m {
            if s[i] < s[j] {
                continue;
            }
            s.swap(i, j);
            for k in 0..n {
                zn.swap(k + i * n, k + j * n);
            }
        }
    }
    Some((zn, s))
}

/// Correct integer-least-squares via the LAMBDA method (RTKLIB `lambda()` port).
///
/// Finds the true ILS optimum and runner-up for any positive-definite
/// covariance — no search box, no combinatorial blow-up. Returns the same
/// [`IlsResult`] shape as [`bounded_ils_search`] so it is a drop-in: in the
/// weakly-correlated regime both select the identical integer vector and ratio;
/// on strongly-correlated geometry only this one is correct.
pub fn lambda_ils_search(
    float_cycles: &[f64],
    covariance: &[Vec<f64>],
    ratio_threshold: f64,
) -> core::result::Result<IlsResult, IlsError> {
    validate_inputs(float_cycles, covariance)?;
    let n = float_cycles.len();
    let q = symmetrize(covariance);
    // Inverse is kept only for the diagnostic metadata (LAMBDA itself uses LtDL).
    let q_inv = symmetrize(&invert(&q).map_err(|_| IlsError::Singular)?);

    // Column-major copy of the symmetrized covariance for the RTKLIB port.
    let mut q_cm = vec![0.0f64; n * n];
    for i in 0..n {
        for j in 0..n {
            q_cm[i + j * n] = q[i][j];
        }
    }

    let (mut l, mut d) = lam_ld(n, &q_cm).ok_or(IlsError::Singular)?;
    let mut z = {
        // Z = identity (column-major).
        let mut e = vec![0.0f64; n * n];
        for i in 0..n {
            e[i + i * n] = 1.0;
        }
        e
    };
    lam_reduction(n, &mut l, &mut d, &mut z);

    // zs = Zᵀ·a.
    let mut zs = vec![0.0f64; n];
    for i in 0..n {
        let mut acc = 0.0;
        for k in 0..n {
            acc += z[k + i * n] * float_cycles[k];
        }
        zs[i] = acc;
    }

    let m = 2usize; // best + runner-up, for the ratio test
    // lam_ld already failed Singular above; a None here is search-loop overflow.
    let (zn, _s) = lam_search(n, m, &l, &d, &zs).ok_or(IlsError::SearchLimitExceeded)?;

    // Back-transform each decorrelated candidate: F = (Zᵀ)⁻¹·E (RTKLIB solve("T",Z,E)).
    // Z is unimodular, so the result is integer up to round-off.
    let mut zt = vec![vec![0.0f64; n]; n];
    for i in 0..n {
        for j in 0..n {
            zt[i][j] = z[j + i * n]; // (Zᵀ)[i][j] = Z[j][i]
        }
    }
    let mut fixed_candidates: Vec<Vec<i64>> = Vec::with_capacity(m);
    for col in 0..m {
        let b: Vec<f64> = (0..n).map(|i| zn[i + col * n]).collect();
        let x = solve_linear(&zt, &b).map_err(|_| IlsError::Singular)?;
        fixed_candidates.push(x.iter().map(|&v| lam_round(v) as i64).collect());
    }

    // LAMBDA's mlambda distance `s` is computed in the decorrelated LtDL space; to
    // keep the reported scores consistent with `bounded_ils_search` (and bit-exact
    // against the explicit `Δᵀ Q⁻¹ Δ` reference / numpy goldens), recompute each
    // candidate's score with the same quadratic form and order them the same way
    // (score ascending, then cycles lexicographic). LAMBDA's only job here is to
    // FIND the candidate set; scoring/ratio use the canonical formula.
    let mut scored: Vec<(f64, Vec<i64>)> = fixed_candidates
        .into_iter()
        .map(|c| (quadratic_score(float_cycles, &c, &q_inv), c))
        .collect();
    scored.sort_by(|(sa, ca), (sb, cb)| {
        sa.partial_cmp(sb)
            .unwrap_or(core::cmp::Ordering::Equal)
            .then_with(|| ca.cmp(cb))
    });

    let best_score = scored[0].0;
    let fixed = scored[0].1.clone();
    let second_best_score = scored.get(1).map(|(s, _)| *s);
    let ratio = integer_ratio(best_score, second_best_score);

    Ok(IlsResult {
        fixed,
        fixed_status: ratio_pass(ratio, ratio_threshold),
        ratio,
        best_score,
        second_best_score,
        // LAMBDA does not enumerate a box; report the number of candidate vectors.
        candidates_evaluated: m,
        covariance: q,
        covariance_inverse: q_inv,
    })
}

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

    #[test]
    fn inverts_a_known_matrix() {
        let a = vec![vec![4.0, 7.0], vec![2.0, 6.0]];
        let inv = invert(&a).unwrap();
        // [[0.6, -0.7], [-0.2, 0.4]]
        assert!((inv[0][0] - 0.6).abs() < 1e-12);
        assert!((inv[0][1] + 0.7).abs() < 1e-12);
        assert!((inv[1][0] + 0.2).abs() < 1e-12);
        assert!((inv[1][1] - 0.4).abs() < 1e-12);
    }

    #[test]
    fn rejects_a_singular_matrix() {
        let a = vec![vec![1.0, 2.0], vec![2.0, 4.0]];
        assert!(invert(&a).is_err());
    }

    #[test]
    fn fixes_a_well_separated_lattice_point() {
        // Float ambiguities very close to integers, tight diagonal covariance:
        // the nearest lattice point dominates and the ratio test passes.
        let float = vec![3.02, -1.98, 5.01];
        let cov = vec![
            vec![0.01, 0.0, 0.0],
            vec![0.0, 0.01, 0.0],
            vec![0.0, 0.0, 0.01],
        ];
        let r = bounded_ils_search(&float, &cov, 1, 200_000, 3.0).unwrap();
        assert_eq!(r.fixed, vec![3, -2, 5]);
        assert!(r.fixed_status);
        assert!(r.ratio > 3.0);
        assert_eq!(r.candidates_evaluated, 27); // 3^3
    }

    #[test]
    fn refuses_an_ambiguous_lattice() {
        // Half-integer floats: nearest points are equidistant -> low ratio.
        let float = vec![0.5, 0.5];
        let cov = vec![vec![1.0, 0.0], vec![0.0, 1.0]];
        let r = bounded_ils_search(&float, &cov, 1, 200_000, 3.0).unwrap();
        assert!(!r.fixed_status);
        assert!(r.ratio < 3.0);
    }

    #[test]
    fn errors_when_the_lattice_exceeds_the_candidate_limit() {
        let float = vec![0.0, 0.0, 0.0];
        let cov = vec![
            vec![1.0, 0.0, 0.0],
            vec![0.0, 1.0, 0.0],
            vec![0.0, 0.0, 1.0],
        ];
        // 3^3 = 27 lattice points, limit 10 -> error.
        assert!(bounded_ils_search(&float, &cov, 1, 10, 3.0).is_err());
    }

    // --- LAMBDA port vs RTKLIB's own committed reference vectors ----------
    // (t_lambda.c utest1/utest2; see parity/generator/lambda_ref). RTKLIB's
    // unit test tolerates 1e-4 on the residuals; we hold the same.

    fn full_matrix(flat: &[f64], n: usize) -> Vec<Vec<f64>> {
        (0..n)
            .map(|i| (0..n).map(|j| flat[i * n + j]).collect())
            .collect()
    }

    #[test]
    fn lambda_matches_rtklib_utest1() {
        let a = [
            1585184.171,
            -6716599.430,
            3915742.905,
            7627233.455,
            9565990.879,
            989457273.200,
        ];
        #[rustfmt::skip]
        let q = full_matrix(&[
            0.227134, 0.112202, 0.112202, 0.112202, 0.112202, 0.103473,
            0.112202, 0.227134, 0.112202, 0.112202, 0.112202, 0.103473,
            0.112202, 0.112202, 0.227134, 0.112202, 0.112202, 0.103473,
            0.112202, 0.112202, 0.112202, 0.227134, 0.112202, 0.103473,
            0.112202, 0.112202, 0.112202, 0.112202, 0.227134, 0.103473,
            0.103473, 0.103473, 0.103473, 0.103473, 0.103473, 0.434339,
        ], 6);

        let r = lambda_ils_search(&a, &q, 3.0).unwrap();
        assert_eq!(
            r.fixed,
            vec![1585184, -6716599, 3915743, 7627234, 9565991, 989457273]
        );
        assert!((r.best_score - 3.5079844392).abs() < 1e-4);
        assert!((r.second_best_score.unwrap() - 3.70845619249).abs() < 1e-4);
    }

    #[test]
    fn lambda_matches_rtklib_utest2_strongly_correlated() {
        // The case the bounded box search cannot solve: the ILS optimum is up
        // to 14 cycles from componentwise rounding. LAMBDA gets it exactly.
        let a = [
            -13324172.755747,
            -10668894.713608,
            -7157225.010770,
            -6149367.974367,
            -7454133.571066,
            -5969200.494550,
            8336734.058423,
            6186974.084502,
            -17549093.883655,
            -13970158.922370,
        ];
        #[rustfmt::skip]
        let q = full_matrix(&[
            0.446320,0.223160,0.223160,0.223160,0.223160,0.572775,0.286388,0.286388,0.286388,0.286388,
            0.223160,0.446320,0.223160,0.223160,0.223160,0.286388,0.572775,0.286388,0.286388,0.286388,
            0.223160,0.223160,0.446320,0.223160,0.223160,0.286388,0.286388,0.572775,0.286388,0.286388,
            0.223160,0.223160,0.223160,0.446320,0.223160,0.286388,0.286388,0.286388,0.572775,0.286388,
            0.223160,0.223160,0.223160,0.223160,0.446320,0.286388,0.286388,0.286388,0.286388,0.572775,
            0.572775,0.286388,0.286388,0.286388,0.286388,0.735063,0.367531,0.367531,0.367531,0.367531,
            0.286388,0.572775,0.286388,0.286388,0.286388,0.367531,0.735063,0.367531,0.367531,0.367531,
            0.286388,0.286388,0.572775,0.286388,0.286388,0.367531,0.367531,0.735063,0.367531,0.367531,
            0.286388,0.286388,0.286388,0.572775,0.286388,0.367531,0.367531,0.367531,0.735063,0.367531,
            0.286388,0.286388,0.286388,0.286388,0.572775,0.367531,0.367531,0.367531,0.367531,0.735063,
        ], 10);

        let r = lambda_ils_search(&a, &q, 3.0).unwrap();
        assert_eq!(
            r.fixed,
            vec![
                -13324188, -10668901, -7157236, -6149379, -7454143, -5969220, 8336726, 6186960,
                -17549108, -13970171
            ]
        );
        assert!((r.best_score - 1506.43578925).abs() < 1e-4);
        assert!((r.second_best_score.unwrap() - 1612.81176533).abs() < 1e-4);
    }

    #[test]
    fn lambda_agrees_with_box_search_in_regime() {
        // Weakly-correlated, ILS optimum near rounding: both kernels must agree.
        let a = vec![0.30, -0.40, 1.20];
        let q = vec![
            vec![0.50, 0.10, 0.05],
            vec![0.10, 0.50, 0.10],
            vec![0.05, 0.10, 0.50],
        ];
        let lam = lambda_ils_search(&a, &q, 3.0).unwrap();
        let box_ = bounded_ils_search(&a, &q, 1, 200_000, 3.0).unwrap();
        assert_eq!(lam.fixed, box_.fixed);
        assert!((lam.best_score - box_.best_score).abs() < 1e-9);
        assert!((lam.second_best_score.unwrap() - box_.second_best_score.unwrap()).abs() < 1e-9);
    }

    // --- input validation (both kernels reject malformed inputs cleanly) -----

    #[test]
    fn rejects_undersized_covariance() {
        // 2 ambiguities, 1x1 covariance — would index out of bounds without the guard.
        let a = vec![0.1, 0.2];
        let q = vec![vec![1.0]];
        assert_eq!(
            bounded_ils_search(&a, &q, 1, 200_000, 3.0),
            Err(IlsError::InvalidDimensions { n: 2, rows: 1 })
        );
        assert_eq!(
            lambda_ils_search(&a, &q, 3.0),
            Err(IlsError::InvalidDimensions { n: 2, rows: 1 })
        );
    }

    #[test]
    fn rejects_oversized_covariance() {
        // 1 ambiguity, 2x2 covariance — would silently use a submatrix.
        let a = vec![0.1];
        let q = vec![vec![1.0, 0.0], vec![0.0, 1.0]];
        assert_eq!(
            bounded_ils_search(&a, &q, 1, 200_000, 3.0),
            Err(IlsError::InvalidDimensions { n: 1, rows: 2 })
        );
        assert_eq!(
            lambda_ils_search(&a, &q, 3.0),
            Err(IlsError::InvalidDimensions { n: 1, rows: 2 })
        );
    }

    #[test]
    fn rejects_ragged_covariance() {
        // Square row count but a row of the wrong width.
        let a = vec![0.1, 0.2];
        let q = vec![vec![1.0, 0.0], vec![0.0]];
        assert_eq!(
            bounded_ils_search(&a, &q, 1, 200_000, 3.0),
            Err(IlsError::InvalidDimensions { n: 2, rows: 1 })
        );
        assert_eq!(
            lambda_ils_search(&a, &q, 3.0),
            Err(IlsError::InvalidDimensions { n: 2, rows: 1 })
        );
    }

    #[test]
    fn rejects_empty_input() {
        let a: Vec<f64> = vec![];
        let q: Vec<Vec<f64>> = vec![];
        assert_eq!(
            bounded_ils_search(&a, &q, 1, 200_000, 3.0),
            Err(IlsError::InvalidDimensions { n: 0, rows: 0 })
        );
        assert_eq!(
            lambda_ils_search(&a, &q, 3.0),
            Err(IlsError::InvalidDimensions { n: 0, rows: 0 })
        );
    }

    #[test]
    fn rejects_non_finite_input() {
        let q = vec![vec![1.0, 0.0], vec![0.0, 1.0]];
        assert_eq!(
            bounded_ils_search(&[f64::NAN, 0.2], &q, 1, 200_000, 3.0),
            Err(IlsError::NonFinite)
        );
        let q_inf = vec![vec![f64::INFINITY, 0.0], vec![0.0, 1.0]];
        assert_eq!(
            lambda_ils_search(&[0.1, 0.2], &q_inf, 3.0),
            Err(IlsError::NonFinite)
        );
    }
}