sidereon-core 0.8.0

//! Measurement-quality control for GNSS positioning.
//!
//! This module owns the language-independent RAIM/FDE decision logic and the
//! standard pseudorange weighting primitives used by Sidereon' QC surface.

use std::collections::{BTreeMap, BTreeSet};

use crate::constants::DEG_TO_RAD;
use crate::spp::{Observation, ReceiverSolution};
use crate::validate;

/// Default zenith-floor term for pseudorange variance, meters.
pub const DEFAULT_VARIANCE_A_M: f64 = 0.3;
/// Default elevation-scaled term for pseudorange variance, meters.
pub const DEFAULT_VARIANCE_B_M: f64 = 0.3;
/// Default false-alarm probability for RAIM.
pub const DEFAULT_P_FA: f64 = 1.0e-3;

/// Pseudorange variance model.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum PseudorangeVarianceModel {
    /// Elevation-only `a^2 + b^2 / sin(el)^2`.
    Elevation,
    /// Elevation plus a C/N0 variance contribution.
    ElevationCn0,
}

/// Options for [`pseudorange_variance`].
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PseudorangeVarianceOptions {
    /// Zenith-floor term, meters.
    pub a_m: f64,
    /// Elevation-scaled term, meters.
    pub b_m: f64,
    /// Selected variance model.
    pub model: PseudorangeVarianceModel,
    /// Carrier-to-noise density, dB-Hz, required by
    /// [`PseudorangeVarianceModel::ElevationCn0`].
    pub cn0_dbhz: Option<f64>,
    /// C/N0 variance scale, square meters.
    pub cn0_scale_m2: f64,
}

impl Default for PseudorangeVarianceOptions {
    fn default() -> Self {
        Self {
            a_m: DEFAULT_VARIANCE_A_M,
            b_m: DEFAULT_VARIANCE_B_M,
            model: PseudorangeVarianceModel::Elevation,
            cn0_dbhz: None,
            cn0_scale_m2: 1.0,
        }
    }
}

impl PseudorangeVarianceOptions {
    fn with_entry_cn0(self, cn0_dbhz: f64) -> Self {
        Self {
            model: PseudorangeVarianceModel::ElevationCn0,
            cn0_dbhz: Some(cn0_dbhz),
            ..self
        }
    }
}

/// One satellite/elevation entry used to build sigma or weight maps.
#[derive(Debug, Clone, PartialEq)]
pub struct WeightEntry {
    /// Satellite token at the binding boundary, e.g. `"G01"`.
    pub satellite_id: String,
    /// Topocentric elevation, degrees.
    pub elevation_deg: f64,
    /// Optional C/N0 for this observation. When present, it selects the C/N0
    /// model for this entry.
    pub cn0_dbhz: Option<f64>,
}

/// Error from quality-control primitives.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum QualityError {
    /// Elevation must be finite, inside `[-90, 90]`, and yield finite variance.
    InvalidElevation,
    /// The C/N0 model was selected without a C/N0 value.
    MissingCn0,
    /// Variance-model parameters must be finite and non-negative.
    InvalidParameter,
    /// Probability must be strictly inside `(0, 1)`.
    InvalidProbability,
    /// RAIM system-count override must be positive.
    InvalidSystemCount,
    /// Chi-square degrees of freedom must be positive.
    InvalidDof,
    /// RAIM weights must be positive finite values.
    InvalidWeight,
    /// RAIM residuals must be finite and aligned with used satellites.
    InvalidResiduals,
}

impl core::fmt::Display for QualityError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        match self {
            Self::InvalidElevation => write!(f, "invalid elevation"),
            Self::MissingCn0 => write!(f, "missing C/N0"),
            Self::InvalidParameter => write!(f, "invalid quality parameter"),
            Self::InvalidProbability => write!(f, "invalid probability"),
            Self::InvalidSystemCount => write!(f, "invalid RAIM system count"),
            Self::InvalidDof => write!(f, "invalid degrees of freedom"),
            Self::InvalidWeight => write!(f, "invalid RAIM weight"),
            Self::InvalidResiduals => write!(f, "invalid RAIM residuals"),
        }
    }
}

impl std::error::Error for QualityError {}

/// Pseudorange measurement variance, square meters.
pub fn pseudorange_variance(
    elevation_deg: f64,
    options: PseudorangeVarianceOptions,
) -> Result<f64, QualityError> {
    validate_elevation_deg(elevation_deg)?;
    validate_variance_options(options)?;

    let mut elevation_var = options.a_m * options.a_m;
    if options.b_m != 0.0 {
        let sin_el = (elevation_deg * DEG_TO_RAD).sin();
        let scaled = options.b_m * options.b_m / (sin_el * sin_el);
        if !scaled.is_finite() {
            return Err(QualityError::InvalidElevation);
        }
        elevation_var += scaled;
    }

    let variance = match options.model {
        PseudorangeVarianceModel::Elevation => elevation_var,
        PseudorangeVarianceModel::ElevationCn0 => {
            let Some(cn0) = options.cn0_dbhz else {
                return Err(QualityError::MissingCn0);
            };
            validate_nonneg_parameter(cn0, "cn0_dbhz")?;
            elevation_var + options.cn0_scale_m2 * 10.0_f64.powf(-cn0 / 10.0)
        }
    };

    validate_positive_variance(variance)?;
    Ok(variance)
}

fn validate_elevation_deg(elevation_deg: f64) -> Result<(), QualityError> {
    validate::finite(elevation_deg, "elevation_deg").map_err(|_| QualityError::InvalidElevation)?;
    if (-90.0..=90.0).contains(&elevation_deg) {
        Ok(())
    } else {
        Err(QualityError::InvalidElevation)
    }
}

fn validate_variance_options(options: PseudorangeVarianceOptions) -> Result<(), QualityError> {
    validate_nonneg_parameter(options.a_m, "variance a_m")?;
    validate_nonneg_parameter(options.b_m, "variance b_m")?;
    validate_nonneg_parameter(options.cn0_scale_m2, "variance cn0_scale_m2")
}

fn validate_nonneg_parameter(value: f64, field: &'static str) -> Result<(), QualityError> {
    validate::finite_nonneg(value, field)
        .map(|_| ())
        .map_err(map_parameter_error)
}

fn validate_positive_variance(value: f64) -> Result<(), QualityError> {
    validate::finite_positive(value, "pseudorange variance")
        .map(|_| ())
        .map_err(map_parameter_error)
}

fn map_parameter_error(_error: validate::FieldError) -> QualityError {
    QualityError::InvalidParameter
}

/// Build a satellite-to-sigma map. Entries whose variance cannot be computed are
/// dropped, matching the Sidereon public API.
pub fn sigmas(
    entries: &[WeightEntry],
    options: PseudorangeVarianceOptions,
) -> BTreeMap<String, f64> {
    entries
        .iter()
        .filter_map(|entry| {
            let opts = match entry.cn0_dbhz {
                Some(cn0) => options.with_entry_cn0(cn0),
                None => options,
            };
            pseudorange_variance(entry.elevation_deg, opts)
                .ok()
                .map(|var| (entry.satellite_id.clone(), var.sqrt()))
        })
        .collect()
}

/// Build a satellite-to-inverse-variance-weight map. Entries whose variance
/// cannot be computed are dropped, matching the Sidereon public API.
pub fn weight_vector(
    entries: &[WeightEntry],
    options: PseudorangeVarianceOptions,
) -> BTreeMap<String, f64> {
    entries
        .iter()
        .filter_map(|entry| {
            let opts = match entry.cn0_dbhz {
                Some(cn0) => options.with_entry_cn0(cn0),
                None => options,
            };
            pseudorange_variance(entry.elevation_deg, opts)
                .ok()
                .map(|var| (entry.satellite_id.clone(), 1.0 / var))
        })
        .collect()
}

/// RAIM weighting mode.
#[derive(Debug, Clone, PartialEq)]
pub enum RaimWeights {
    /// Unit weights, equivalent to sigma = 1 m for every satellite.
    Unit,
    /// Per-satellite inverse variance weights. Missing satellites default to
    /// unit weight.
    BySatellite(BTreeMap<String, f64>),
}

impl RaimWeights {
    fn validate(&self) -> Result<(), QualityError> {
        match self {
            Self::Unit => Ok(()),
            Self::BySatellite(weights) => weights
                .values()
                .try_for_each(|w| validate::finite_positive(*w, "raim weight").map(|_| ()))
                .map_err(|_| QualityError::InvalidWeight),
        }
    }

    fn weight_for(&self, satellite_id: &str) -> f64 {
        match self {
            Self::Unit => 1.0,
            Self::BySatellite(weights) => weights.get(satellite_id).copied().unwrap_or(1.0),
        }
    }
}

/// Options for [`raim`].
#[derive(Debug, Clone, PartialEq)]
pub struct RaimOptions {
    /// False-alarm probability.
    pub p_fa: f64,
    /// RAIM residual weights.
    pub weights: RaimWeights,
    /// Optional override for the number of distinct GNSS clock systems.
    pub n_systems: Option<isize>,
}

impl Default for RaimOptions {
    fn default() -> Self {
        Self {
            p_fa: DEFAULT_P_FA,
            weights: RaimWeights::Unit,
            n_systems: None,
        }
    }
}

/// Minimal solution view needed by RAIM.
#[derive(Debug, Clone, PartialEq)]
pub struct RaimInput {
    /// Used satellite tokens, in residual order.
    pub used_sats: Vec<String>,
    /// Post-fit pseudorange residuals, meters.
    pub residuals_m: Vec<f64>,
}

/// A solution that can feed the RAIM test.
pub trait RaimSolution {
    /// Used satellite tokens, in residual order.
    fn raim_used_sats(&self) -> Vec<String>;
    /// Post-fit residuals, meters, in used-satellite order.
    fn raim_residuals_m(&self) -> &[f64];
}

impl RaimSolution for ReceiverSolution {
    fn raim_used_sats(&self) -> Vec<String> {
        self.used_sats.iter().map(ToString::to_string).collect()
    }

    fn raim_residuals_m(&self) -> &[f64] {
        &self.residuals_m
    }
}

/// Result of a residual chi-square RAIM test.
#[derive(Debug, Clone, PartialEq)]
pub struct RaimResult {
    /// True when the test statistic exceeds the chi-square threshold.
    pub fault_detected: bool,
    /// Weighted residual sum of squares.
    pub test_statistic: f64,
    /// Chi-square threshold, absent when the geometry is not testable.
    pub threshold: Option<f64>,
    /// Degrees of freedom, `n_used - (3 + n_systems)`.
    pub dof: isize,
    /// False when `dof <= 0`.
    pub testable: bool,
    /// Per-satellite standardized residuals.
    pub normalized_residuals: BTreeMap<String, f64>,
    /// Satellite with the largest absolute standardized residual.
    pub worst_sat: Option<String>,
}

/// Run RAIM over a generic solution.
pub fn raim_for_solution<S: RaimSolution>(
    solution: &S,
    options: &RaimOptions,
) -> Result<RaimResult, QualityError> {
    raim(
        &RaimInput {
            used_sats: solution.raim_used_sats(),
            residuals_m: solution.raim_residuals_m().to_vec(),
        },
        options,
    )
}

/// Residual-based chi-square RAIM.
pub fn raim(input: &RaimInput, options: &RaimOptions) -> Result<RaimResult, QualityError> {
    validate_probability(options.p_fa)?;
    options.weights.validate()?;
    validate_raim_input(input)?;

    let n_used = input.used_sats.len() as isize;
    let n_systems = raim_system_count(input, options)?;
    let dof = n_used - (3 + n_systems);

    let mut test_statistic = 0.0;
    let mut normalized_residuals = BTreeMap::new();
    let mut worst_sat = None::<String>;
    let mut worst_abs = f64::NEG_INFINITY;

    for (satellite_id, residual_m) in input.used_sats.iter().zip(input.residuals_m.iter()) {
        let weight = options.weights.weight_for(satellite_id);
        let normalized = residual_m * weight.sqrt();
        test_statistic += residual_m * residual_m * weight;
        normalized_residuals.insert(satellite_id.clone(), normalized);
        let abs_normalized = normalized.abs();
        if abs_normalized > worst_abs {
            worst_abs = abs_normalized;
            worst_sat = Some(satellite_id.clone());
        }
    }

    if dof <= 0 {
        return Ok(RaimResult {
            fault_detected: false,
            test_statistic,
            threshold: None,
            dof,
            testable: false,
            normalized_residuals,
            worst_sat,
        });
    }

    let threshold = chi2_inv(1.0 - options.p_fa, dof as usize)?;
    Ok(RaimResult {
        fault_detected: test_statistic > threshold,
        test_statistic,
        threshold: Some(threshold),
        dof,
        testable: true,
        normalized_residuals,
        worst_sat,
    })
}

fn validate_probability(p: f64) -> Result<(), QualityError> {
    let p = validate::finite(p, "probability").map_err(|_| QualityError::InvalidProbability)?;
    if p > 0.0 && p < 1.0 {
        Ok(())
    } else {
        Err(QualityError::InvalidProbability)
    }
}

fn validate_raim_input(input: &RaimInput) -> Result<(), QualityError> {
    if input.used_sats.len() != input.residuals_m.len() {
        return Err(QualityError::InvalidResiduals);
    }
    validate::finite_slice(&input.residuals_m, "raim residuals")
        .map_err(|_| QualityError::InvalidResiduals)
}

fn raim_system_count(input: &RaimInput, options: &RaimOptions) -> Result<isize, QualityError> {
    match options.n_systems {
        Some(n_systems) if n_systems >= 1 => Ok(n_systems),
        Some(_) => Err(QualityError::InvalidSystemCount),
        None => Ok(distinct_systems(&input.used_sats)),
    }
}

fn distinct_systems(used_sats: &[String]) -> isize {
    used_sats
        .iter()
        .filter_map(|sat| sat.chars().next())
        .collect::<BTreeSet<_>>()
        .len() as isize
}

/// Result of a fault-detection-and-exclusion loop.
#[derive(Debug, Clone, PartialEq)]
pub struct FdeResult<S> {
    /// Final accepted solution.
    pub solution: S,
    /// Excluded satellites in exclusion order.
    pub excluded: Vec<String>,
    /// Number of exclusions performed.
    pub iterations: usize,
}

/// Error from [`fde`].
#[derive(Debug, Clone, PartialEq)]
pub enum FdeError<E> {
    /// RAIM still flagged the set when the exclusion budget was exhausted.
    FaultUnresolved(f64),
    /// The supplied solve callback failed.
    Solve(E),
    /// RAIM configuration was invalid.
    Raim(QualityError),
}

/// Options for [`fde`].
#[derive(Debug, Clone, PartialEq)]
pub struct FdeOptions {
    /// RAIM options used after each solve.
    pub raim: RaimOptions,
    /// Maximum number of exclusions to attempt.
    pub max_iterations: usize,
}

/// Fault detection and exclusion over a caller-supplied SPP solver.
pub fn fde<S, E, F>(
    observations: &[Observation],
    options: &FdeOptions,
    mut solve: F,
) -> Result<FdeResult<S>, FdeError<E>>
where
    S: RaimSolution,
    F: FnMut(&[Observation]) -> Result<S, E>,
{
    let mut remaining = observations.to_vec();
    let mut excluded = Vec::new();
    let mut iter = 0usize;

    loop {
        let solution = solve(&remaining).map_err(FdeError::Solve)?;
        let result = raim_for_solution(&solution, &options.raim).map_err(FdeError::Raim)?;

        if !result.fault_detected {
            return Ok(FdeResult {
                solution,
                excluded,
                iterations: iter,
            });
        }

        let Some(worst) = result.worst_sat else {
            return Err(FdeError::FaultUnresolved(result.test_statistic));
        };

        if iter >= options.max_iterations {
            return Err(FdeError::FaultUnresolved(result.test_statistic));
        }

        remaining.retain(|ob| ob.satellite_id.to_string() != worst);
        excluded.push(worst);
        iter += 1;
    }
}

/// Validation policy for receiver solutions returned by SPP.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SolutionValidationOptions {
    /// Optional PDOP ceiling.
    pub max_pdop: Option<f64>,
    /// Minimum plausible geocentric radius, meters.
    pub min_plausible_radius_m: f64,
    /// Maximum plausible geocentric radius, meters.
    pub max_plausible_radius_m: f64,
    /// Maximum plausible RMS for a solution flagged converged, meters.
    pub max_converged_residual_rms_m: f64,
}

impl Default for SolutionValidationOptions {
    fn default() -> Self {
        Self {
            max_pdop: None,
            min_plausible_radius_m: 6_344_752.0,
            max_plausible_radius_m: 8_378_137.0,
            max_converged_residual_rms_m: 1.0e4,
        }
    }
}

/// Error from [`validate_receiver_solution`].
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum SolutionValidationError {
    /// Validation gate options were malformed or degenerate.
    InvalidOptions {
        /// The invalid option field.
        field: &'static str,
        /// The validation failure category.
        reason: &'static str,
    },
    /// DOP could not be computed because the geometry was rank deficient.
    DegenerateGeometryRankDeficient,
    /// PDOP exceeded the caller's configured ceiling.
    DegenerateGeometryPdop(f64),
    /// Position geocentric radius was outside the physical receiver band.
    ImplausiblePosition(f64),
    /// Converged solution residuals were non-finite or produced non-finite RMS.
    InvalidResiduals,
    /// Converged solution had physically implausible post-fit residual RMS.
    NoConvergence(f64),
}

impl core::fmt::Display for SolutionValidationError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        match self {
            Self::InvalidOptions { field, reason } => {
                write!(f, "invalid receiver validation option {field}: {reason}")
            }
            Self::DegenerateGeometryRankDeficient => {
                write!(f, "receiver geometry is rank deficient")
            }
            Self::DegenerateGeometryPdop(pdop) => {
                write!(
                    f,
                    "receiver geometry PDOP {pdop} exceeds the configured limit"
                )
            }
            Self::ImplausiblePosition(radius_m) => write!(
                f,
                "receiver geocentric radius {radius_m} m is outside the plausible range"
            ),
            Self::InvalidResiduals => {
                write!(f, "converged solution residuals must be finite")
            }
            Self::NoConvergence(rms_m) => write!(
                f,
                "converged solution residual RMS {rms_m} m is implausibly large"
            ),
        }
    }
}

impl std::error::Error for SolutionValidationError {}

/// Apply the receiver-solution plausibility gates used by the Sidereon SPP API.
pub fn validate_receiver_solution(
    solution: &ReceiverSolution,
    options: SolutionValidationOptions,
) -> Result<(), SolutionValidationError> {
    validate_solution_validation_options(options)?;

    let Some(dop) = solution.dop else {
        return Err(SolutionValidationError::DegenerateGeometryRankDeficient);
    };

    if let Some(max_pdop) = options.max_pdop {
        if dop.pdop > max_pdop {
            return Err(SolutionValidationError::DegenerateGeometryPdop(dop.pdop));
        }
    }

    let p = solution.position.as_array();
    let radius_m = (p[0] * p[0] + p[1] * p[1] + p[2] * p[2]).sqrt();
    if radius_m < options.min_plausible_radius_m || radius_m > options.max_plausible_radius_m {
        return Err(SolutionValidationError::ImplausiblePosition(radius_m));
    }

    if solution.metadata.converged {
        if validate::finite_slice(&solution.residuals_m, "solution residuals").is_err() {
            return Err(SolutionValidationError::InvalidResiduals);
        }
        let rms = residual_rms(&solution.residuals_m);
        if !rms.is_finite() {
            return Err(SolutionValidationError::InvalidResiduals);
        }
        if rms > options.max_converged_residual_rms_m {
            return Err(SolutionValidationError::NoConvergence(rms));
        }
    }

    Ok(())
}

fn validate_solution_validation_options(
    options: SolutionValidationOptions,
) -> Result<(), SolutionValidationError> {
    if let Some(max_pdop) = options.max_pdop {
        validate::finite_positive(max_pdop, "max_pdop").map_err(validation_option_error)?;
    }
    validate::finite_positive(options.min_plausible_radius_m, "min_plausible_radius_m")
        .map_err(validation_option_error)?;
    validate::finite_positive(options.max_plausible_radius_m, "max_plausible_radius_m")
        .map_err(validation_option_error)?;
    if options.min_plausible_radius_m >= options.max_plausible_radius_m {
        return Err(invalid_validation_option(
            "plausible_radius_m",
            "must be increasing",
        ));
    }
    validate::finite_positive(
        options.max_converged_residual_rms_m,
        "max_converged_residual_rms_m",
    )
    .map_err(validation_option_error)?;
    Ok(())
}

fn validation_option_error(error: validate::FieldError) -> SolutionValidationError {
    invalid_validation_option(error.field(), error.reason())
}

fn invalid_validation_option(field: &'static str, reason: &'static str) -> SolutionValidationError {
    SolutionValidationError::InvalidOptions { field, reason }
}

fn residual_rms(residuals: &[f64]) -> f64 {
    if residuals.is_empty() {
        return 0.0;
    }
    let sum_sq = residuals.iter().map(|r| r * r).sum::<f64>();
    (sum_sq / residuals.len() as f64).sqrt()
}

/// Chi-square inverse CDF.
pub fn chi2_inv(p: f64, k: usize) -> Result<f64, QualityError> {
    validate_probability(p)?;
    if k == 0 {
        return Err(QualityError::InvalidDof);
    }
    let a = 0.5 * k as f64;
    let hi0 = (k as f64 + 10.0 * (2.0 * k as f64).sqrt()).max(1.0);
    let hi = chi2_bracket_hi(p, a, hi0);
    Ok(chi2_bisect(p, a, 0.0, hi, 0))
}

fn chi2_bracket_hi(p: f64, a: f64, hi: f64) -> f64 {
    if chi2_cdf(hi, a) >= p {
        hi
    } else {
        chi2_bracket_hi(p, a, hi * 2.0)
    }
}

fn chi2_bisect(p: f64, a: f64, lo: f64, hi: f64, iter: usize) -> f64 {
    if iter >= 120 {
        return 0.5 * (lo + hi);
    }
    let mid = 0.5 * (lo + hi);
    if chi2_cdf(mid, a) < p {
        chi2_bisect(p, a, mid, hi, iter + 1)
    } else {
        chi2_bisect(p, a, lo, mid, iter + 1)
    }
}

fn chi2_cdf(x: f64, a: f64) -> f64 {
    regularized_gamma_p(a, 0.5 * x)
}

const GAMMA_EPS: f64 = 1.0e-15;
const GAMMA_FPMIN: f64 = 1.0e-300;
const GAMMA_ITMAX: usize = 1_000;

fn regularized_gamma_p(a: f64, x: f64) -> f64 {
    if x <= 0.0 {
        return 0.0;
    }

    if x < a + 1.0 {
        let gln = log_gamma(a);
        let sum = gamma_series(x, 1.0 / a, 1.0 / a, a, 1);
        sum * (-x + a * x.ln() - gln).exp()
    } else {
        let gln = log_gamma(a);
        let q = gamma_continued_fraction(a, x) * (-x + a * x.ln() - gln).exp();
        1.0 - q
    }
}

fn gamma_series(x: f64, sum: f64, del: f64, ap: f64, n: usize) -> f64 {
    if n > GAMMA_ITMAX {
        return sum;
    }
    let ap = ap + 1.0;
    let del = del * x / ap;
    let sum = sum + del;
    if del.abs() < sum.abs() * GAMMA_EPS {
        sum
    } else {
        gamma_series(x, sum, del, ap, n + 1)
    }
}

fn gamma_continued_fraction(a: f64, x: f64) -> f64 {
    let b = x + 1.0 - a;
    let c = 1.0 / GAMMA_FPMIN;
    let d = 1.0 / safe_denominator(b);
    gamma_cf_iter(a, b, c, d, d, 1)
}

fn gamma_cf_iter(a: f64, b: f64, c: f64, d: f64, h: f64, n: usize) -> f64 {
    if n > GAMMA_ITMAX {
        return h;
    }

    let an = -(n as f64) * (n as f64 - a);
    let b = b + 2.0;
    let d = 1.0 / safe_denominator(an * d + b);
    let c = safe_denominator(b + an / c);
    let delta = d * c;
    let h = h * delta;

    if (delta - 1.0).abs() < GAMMA_EPS {
        h
    } else {
        gamma_cf_iter(a, b, c, d, h, n + 1)
    }
}

fn safe_denominator(x: f64) -> f64 {
    if x.abs() < GAMMA_FPMIN {
        GAMMA_FPMIN
    } else {
        x
    }
}

const LANCZOS: [f64; 9] = [
    0.9999999999998099,
    676.5203681218851,
    -1259.1392167224028,
    771.3234287776531,
    -176.6150291621406,
    12.507343278686905,
    -0.13857109526572012,
    9.984369578019572e-6,
    1.5056327351493116e-7,
];
const SQRT_2PI: f64 = 2.5066282746310002;

fn log_gamma(z: f64) -> f64 {
    if z < 0.5 {
        std::f64::consts::PI.ln() - (std::f64::consts::PI * z).sin().ln() - log_gamma(1.0 - z)
    } else {
        let z = z - 1.0;
        let mut x = LANCZOS[0];
        for (i, coef) in LANCZOS.iter().enumerate().skip(1) {
            x += coef / (z + i as f64);
        }
        let t = z + 7.5;
        SQRT_2PI.ln() + (z + 0.5) * t.ln() - t + x.ln()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{GnssSatelliteId, GnssSystem};

    #[derive(Debug, Clone)]
    struct TestSolution {
        used_sats: Vec<String>,
        residuals_m: Vec<f64>,
    }

    impl RaimSolution for TestSolution {
        fn raim_used_sats(&self) -> Vec<String> {
            self.used_sats.clone()
        }

        fn raim_residuals_m(&self) -> &[f64] {
            &self.residuals_m
        }
    }

    fn gps(prn: u8) -> GnssSatelliteId {
        GnssSatelliteId::new(GnssSystem::Gps, prn).expect("valid satellite id")
    }

    fn valid_receiver_solution() -> ReceiverSolution {
        ReceiverSolution {
            position: crate::frame::ItrfPositionM::new(6_378_137.0, 0.0, 0.0).unwrap(),
            geodetic: None,
            rx_clock_s: 0.0,
            system_clocks_s: vec![(GnssSystem::Gps, 0.0)],
            dop: Some(crate::dop::Dop {
                gdop: 2.5,
                pdop: 2.0,
                hdop: 1.5,
                vdop: 1.0,
                tdop: 0.5,
            }),
            residuals_m: vec![0.1, -0.1, 0.0, 0.05, -0.05],
            used_sats: (1..=5).map(gps).collect(),
            rejected_sats: Vec::new(),
            metadata: crate::spp::SolutionMetadata {
                iterations: 3,
                converged: true,
                status: crate::astro::math::least_squares::Status::StepTolerance,
                ionosphere_applied: false,
                troposphere_applied: false,
                outer_iterations: 0,
                final_robust_scale_m: None,
                used_count: 5,
                systems: vec![GnssSystem::Gps],
                redundancy: 1,
                raim_checkable: true,
            },
        }
    }

    #[test]
    fn pseudorange_variance_matches_elevation_model() {
        let opts = PseudorangeVarianceOptions::default();
        let variance = pseudorange_variance(30.0, opts).unwrap();
        assert!((variance - 0.45).abs() < 1.0e-15);
        assert_eq!(
            pseudorange_variance(0.0, opts),
            Err(QualityError::InvalidElevation)
        );
        let horizon_opts = PseudorangeVarianceOptions { b_m: 0.0, ..opts };
        assert_eq!(
            pseudorange_variance(0.0, horizon_opts),
            Ok(horizon_opts.a_m * horizon_opts.a_m)
        );
        assert_eq!(
            pseudorange_variance(-90.0, horizon_opts),
            Ok(horizon_opts.a_m * horizon_opts.a_m)
        );
        assert_eq!(
            pseudorange_variance(90.1, horizon_opts),
            Err(QualityError::InvalidElevation)
        );
        assert_eq!(
            pseudorange_variance(f64::NAN, opts),
            Err(QualityError::InvalidElevation)
        );
    }

    #[test]
    fn cn0_model_requires_cn0_and_adds_noise_term() {
        let opts = PseudorangeVarianceOptions {
            model: PseudorangeVarianceModel::ElevationCn0,
            cn0_dbhz: None,
            ..Default::default()
        };
        assert_eq!(
            pseudorange_variance(30.0, opts),
            Err(QualityError::MissingCn0)
        );

        let weak = pseudorange_variance(
            30.0,
            PseudorangeVarianceOptions {
                cn0_dbhz: Some(30.0),
                ..opts
            },
        )
        .unwrap();
        let strong = pseudorange_variance(
            30.0,
            PseudorangeVarianceOptions {
                cn0_dbhz: Some(50.0),
                ..opts
            },
        )
        .unwrap();
        assert!(strong < weak);
    }

    #[test]
    fn pseudorange_variance_rejects_nonfinite_and_negative_parameters() {
        let invalid_a = PseudorangeVarianceOptions {
            a_m: f64::NAN,
            ..Default::default()
        };
        assert_eq!(
            pseudorange_variance(30.0, invalid_a),
            Err(QualityError::InvalidParameter)
        );

        let invalid_b = PseudorangeVarianceOptions {
            b_m: -1.0,
            ..Default::default()
        };
        assert_eq!(
            pseudorange_variance(30.0, invalid_b),
            Err(QualityError::InvalidParameter)
        );

        let invalid_cn0_scale = PseudorangeVarianceOptions {
            cn0_scale_m2: f64::INFINITY,
            ..Default::default()
        };
        assert_eq!(
            pseudorange_variance(30.0, invalid_cn0_scale),
            Err(QualityError::InvalidParameter)
        );

        let invalid_cn0 = PseudorangeVarianceOptions {
            model: PseudorangeVarianceModel::ElevationCn0,
            cn0_dbhz: Some(f64::NAN),
            ..Default::default()
        };
        assert_eq!(
            pseudorange_variance(30.0, invalid_cn0),
            Err(QualityError::InvalidParameter)
        );
    }

    #[test]
    fn pseudorange_variance_rejects_zero_total_variance() {
        let zero_variance = PseudorangeVarianceOptions {
            a_m: 0.0,
            b_m: 0.0,
            ..Default::default()
        };
        assert_eq!(
            pseudorange_variance(30.0, zero_variance),
            Err(QualityError::InvalidParameter)
        );

        let entries = vec![WeightEntry {
            satellite_id: "G01".to_string(),
            elevation_deg: 30.0,
            cn0_dbhz: None,
        }];
        let weights = weight_vector(&entries, zero_variance);
        assert!(
            !weights.contains_key("G01"),
            "zero variance must not produce an infinite inverse-variance weight"
        );
    }

    #[test]
    fn sigma_and_weight_maps_drop_invalid_entries() {
        let entries = vec![
            WeightEntry {
                satellite_id: "G01".to_string(),
                elevation_deg: 90.0,
                cn0_dbhz: None,
            },
            WeightEntry {
                satellite_id: "G02".to_string(),
                elevation_deg: -91.0,
                cn0_dbhz: None,
            },
        ];
        let sigmas = sigmas(&entries, Default::default());
        let weights = weight_vector(&entries, Default::default());
        assert!(sigmas.contains_key("G01"));
        assert!(!sigmas.contains_key("G02"));
        assert_eq!(weights["G01"], 1.0 / (sigmas["G01"] * sigmas["G01"]));
    }

    #[test]
    fn sigma_and_weight_maps_retain_horizon_entries_without_elevation_term() {
        let entries = vec![
            WeightEntry {
                satellite_id: "G01".to_string(),
                elevation_deg: 0.0,
                cn0_dbhz: None,
            },
            WeightEntry {
                satellite_id: "G02".to_string(),
                elevation_deg: f64::NAN,
                cn0_dbhz: None,
            },
        ];
        let options = PseudorangeVarianceOptions {
            b_m: 0.0,
            ..Default::default()
        };
        let sigmas = sigmas(&entries, options);
        let weights = weight_vector(&entries, options);
        assert_eq!(sigmas["G01"], options.a_m);
        assert_eq!(weights["G01"], 1.0 / (options.a_m * options.a_m));
        assert!(!sigmas.contains_key("G02"));
        assert!(!weights.contains_key("G02"));
    }

    #[test]
    fn chi_square_inverse_matches_reference_values() {
        let refs = [
            (1, 10.828),
            (2, 13.816),
            (3, 16.266),
            (4, 18.467),
            (5, 20.515),
        ];
        for (dof, expected) in refs {
            let got = chi2_inv(0.999, dof).unwrap();
            assert!((got - expected).abs() < 1.0e-3);
        }
        assert_eq!(chi2_inv(1.0, 1), Err(QualityError::InvalidProbability));
        assert_eq!(chi2_inv(0.95, 0), Err(QualityError::InvalidDof));
    }

    #[test]
    fn raim_reports_fault_and_worst_satellite() {
        let input = RaimInput {
            used_sats: ["G01", "G02", "G03", "G04", "G05"]
                .into_iter()
                .map(str::to_string)
                .collect(),
            residuals_m: vec![0.0, 0.0, 0.0, 0.0, 5.0],
        };
        let result = raim(&input, &RaimOptions::default()).unwrap();
        assert!(result.fault_detected);
        assert!(result.testable);
        assert_eq!(result.dof, 1);
        assert_eq!(result.test_statistic, 25.0);
        assert_eq!(result.worst_sat.as_deref(), Some("G05"));
    }

    #[test]
    fn raim_dof_zero_is_not_testable() {
        let input = RaimInput {
            used_sats: ["G01", "G02", "G03", "G04"]
                .into_iter()
                .map(str::to_string)
                .collect(),
            residuals_m: vec![0.0, 0.0, 0.0, 0.0],
        };
        let result = raim(&input, &RaimOptions::default()).unwrap();
        assert!(!result.fault_detected);
        assert!(!result.testable);
        assert_eq!(result.threshold, None);
        assert_eq!(result.dof, 0);
    }

    #[test]
    fn raim_rejects_nonpositive_system_overrides() {
        let input = RaimInput {
            used_sats: ["G01", "G02", "G03", "G04", "G05"]
                .into_iter()
                .map(str::to_string)
                .collect(),
            residuals_m: vec![0.0; 5],
        };

        for n_systems in [0, -1] {
            let options = RaimOptions {
                n_systems: Some(n_systems),
                ..Default::default()
            };
            assert_eq!(
                raim(&input, &options),
                Err(QualityError::InvalidSystemCount)
            );
        }
    }

    #[test]
    fn raim_positive_system_override_controls_dof() {
        let input = RaimInput {
            used_sats: ["G01", "G02", "G03", "G04", "G05", "G06"]
                .into_iter()
                .map(str::to_string)
                .collect(),
            residuals_m: vec![0.0; 6],
        };
        let options = RaimOptions {
            n_systems: Some(2),
            ..Default::default()
        };

        let result = raim(&input, &options).unwrap();
        assert!(result.testable);
        assert_eq!(result.dof, 1);
    }

    #[test]
    fn raim_rejects_misaligned_or_nonfinite_residuals() {
        let input = RaimInput {
            used_sats: ["G01", "G02"].into_iter().map(str::to_string).collect(),
            residuals_m: vec![1.0],
        };
        assert_eq!(
            raim(&input, &RaimOptions::default()),
            Err(QualityError::InvalidResiduals)
        );

        let input = RaimInput {
            used_sats: ["G01", "G02"].into_iter().map(str::to_string).collect(),
            residuals_m: vec![1.0, f64::NAN],
        };
        assert_eq!(
            raim(&input, &RaimOptions::default()),
            Err(QualityError::InvalidResiduals)
        );
    }

    #[test]
    fn raim_rejects_nonfinite_weights_and_probability() {
        let input = RaimInput {
            used_sats: ["G01", "G02", "G03", "G04", "G05"]
                .into_iter()
                .map(str::to_string)
                .collect(),
            residuals_m: vec![0.0; 5],
        };
        let mut weights = BTreeMap::new();
        weights.insert("G01".to_string(), f64::NAN);
        let options = RaimOptions {
            weights: RaimWeights::BySatellite(weights),
            ..Default::default()
        };
        assert_eq!(raim(&input, &options), Err(QualityError::InvalidWeight));

        let options = RaimOptions {
            p_fa: f64::NAN,
            ..Default::default()
        };
        assert_eq!(
            raim(&input, &options),
            Err(QualityError::InvalidProbability)
        );
    }

    #[test]
    fn fde_excludes_largest_normalized_residual() {
        let observations: Vec<Observation> = (1..=5)
            .map(|prn| Observation {
                satellite_id: gps(prn),
                pseudorange_m: prn as f64,
            })
            .collect();

        let options = FdeOptions {
            raim: RaimOptions::default(),
            max_iterations: 1,
        };
        let result = fde(&observations, &options, |remaining| {
            let used_sats = remaining
                .iter()
                .map(|ob| ob.satellite_id.to_string())
                .collect::<Vec<_>>();
            let residuals_m = remaining
                .iter()
                .map(|ob| if ob.satellite_id == gps(5) { 5.0 } else { 0.0 })
                .collect::<Vec<_>>();
            Ok::<_, ()>(TestSolution {
                used_sats,
                residuals_m,
            })
        })
        .unwrap();

        assert_eq!(result.excluded, vec!["G05".to_string()]);
        assert_eq!(result.iterations, 1);
        assert_eq!(result.solution.used_sats.len(), 4);
    }

    #[test]
    fn fde_refuses_fault_when_budget_is_exhausted() {
        let observations: Vec<Observation> = (1..=5)
            .map(|prn| Observation {
                satellite_id: gps(prn),
                pseudorange_m: prn as f64,
            })
            .collect();
        let options = FdeOptions {
            raim: RaimOptions::default(),
            max_iterations: 0,
        };
        let err = fde(&observations, &options, |remaining| {
            Ok::<_, ()>(TestSolution {
                used_sats: remaining
                    .iter()
                    .map(|ob| ob.satellite_id.to_string())
                    .collect(),
                residuals_m: vec![0.0, 0.0, 0.0, 0.0, 5.0],
            })
        })
        .unwrap_err();

        assert_eq!(err, FdeError::FaultUnresolved(25.0));
    }

    #[test]
    fn receiver_solution_validation_rejects_invalid_gate_options() {
        let solution = valid_receiver_solution();
        for (options, field, reason) in [
            (
                SolutionValidationOptions {
                    max_pdop: Some(f64::NAN),
                    ..Default::default()
                },
                "max_pdop",
                "not finite",
            ),
            (
                SolutionValidationOptions {
                    max_pdop: Some(0.0),
                    ..Default::default()
                },
                "max_pdop",
                "not positive",
            ),
            (
                SolutionValidationOptions {
                    min_plausible_radius_m: 0.0,
                    ..Default::default()
                },
                "min_plausible_radius_m",
                "not positive",
            ),
            (
                SolutionValidationOptions {
                    max_plausible_radius_m: f64::INFINITY,
                    ..Default::default()
                },
                "max_plausible_radius_m",
                "not finite",
            ),
            (
                SolutionValidationOptions {
                    max_converged_residual_rms_m: f64::NAN,
                    ..Default::default()
                },
                "max_converged_residual_rms_m",
                "not finite",
            ),
        ] {
            assert_eq!(
                validate_receiver_solution(&solution, options),
                Err(SolutionValidationError::InvalidOptions { field, reason })
            );
        }

        let inverted_radius = SolutionValidationOptions {
            min_plausible_radius_m: 8_000_000.0,
            max_plausible_radius_m: 7_000_000.0,
            ..Default::default()
        };
        assert_eq!(
            validate_receiver_solution(&solution, inverted_radius),
            Err(SolutionValidationError::InvalidOptions {
                field: "plausible_radius_m",
                reason: "must be increasing",
            })
        );
    }

    #[test]
    fn receiver_solution_validation_rejects_nonfinite_residuals() {
        let mut solution = valid_receiver_solution();
        solution.residuals_m[1] = f64::NAN;
        assert_eq!(
            validate_receiver_solution(&solution, SolutionValidationOptions::default()),
            Err(SolutionValidationError::InvalidResiduals)
        );
    }
}