astrodynamics-gnss 0.8.0

//! Single-point positioning (SPP).
//!
//! Recovers a receiver ECEF position and clock bias from a set of pseudoranges,
//! a satellite ephemeris source (a precise SP3 product or a broadcast navigation
//! message, via the [`EphemerisSource`] trait), and the Klobuchar ionosphere /
//! Saastamoinen-Niell troposphere correction models. GPS L1 C/A, Galileo E1, and
//! BeiDou B1I are supported; the broadcast Klobuchar ionosphere delay is computed
//! on L1 and scaled to each satellite's carrier by the dispersive `(f_L1 / f)^2`
//! factor, so GPS L1 / Galileo E1 (both at `f_L1`) are unscaled and BeiDou B1I is
//! scaled to its frequency. A system without a modeled single-frequency carrier
//! is rejected when the ionosphere correction is requested.
//!
//! The state vector is `[x_m, y_m, z_m, clk_0, clk_1, ...]`: three ECEF position
//! components (meters) followed by one receiver clock per distinct GNSS in the
//! solve, expressed as a length (meters). A single-system solve reduces to the
//! classic `[x_m, y_m, z_m, b_m]`; a multi-system solve adds an inter-system
//! bias parameter for each additional constellation. The seconds value
//! `rx_clock_s = clk_0 / c` (the reference system) and the per-system clocks are
//! reported only at the API boundary.
//!
//! The per-satellite predicted pseudorange is built in a pinned operation order:
//! a fixed-count transmit-time iteration (receive time minus geometric range
//! over `c`) locates the satellite ephemeris at transmission, an Earth-rotation
//! (Sagnac) closed-form rotation brings the satellite into the receive-time
//! frame, the geometric range and the line-of-sight azimuth/elevation follow,
//! then the ionosphere and troposphere delays are added to the predicted range
//! left-to-right. The residual the solver sees is `sqrt(w) * (P_meas - P_hat)`
//! with an elevation-based weight evaluated once at the frozen initial-guess
//! geometry.
//!
//! The geometric/clock/correction substrate and its 2-point finite-difference
//! Jacobian are arithmetic over the libm-bound model functions and are a
//! bit-exact (0-ULP) parity target against the reference recipe. The converged
//! position is produced by the trust-region least-squares solver in the
//! `astrodynamics` core, whose linear-algebra step is not bit-reproducible
//! across BLAS builds; the converged solution is therefore a sub-micron
//! solver-agreement result, not a 0-ULP claim.
//!
//! The bit-exact claim depends on the fused-multiply-add policy matching the
//! reference exactly. The substrate uses no contracted `a*b+c` anywhere the
//! reference computes the two roundings separately; the single deliberate
//! exception is the 3x3-by-vector rotation primitive, which uses `mul_add` to
//! reproduce the reference's rounding of that product. The certified target
//! pins `target-cpu`/features so the compiler neither introduces nor drops a
//! contraction; on a host that auto-contracts these expressions the last bit
//! can differ and the goldens are not expected to hold.

use astrodynamics::math::least_squares::{
    self, solve_trf, LeastSquaresProblem, SolveOptions, Status,
};
use nalgebra::DVector;

use crate::dop::{dop, dop_multi, Dop, LineOfSight};
use crate::frame::{ItrfPositionM, Wgs84Geodetic};
use crate::id::{GnssSatelliteId, GnssSystem};
use crate::ionex::{klobuchar_native, KlobucharParams};
use crate::sp3::Sp3;
use crate::tropo::slant_components;

/// Speed of light in vacuum (m/s), IS-GPS-200 / matches the correction recipes.
pub const C_M_S: f64 = 299_792_458.0;
/// GPS L1 carrier frequency (Hz). Galileo E1 shares this frequency.
pub const F_L1_HZ: f64 = 1_575.42e6;
/// BeiDou B1I carrier frequency (Hz).
pub const F_B1I_HZ: f64 = 1_561.098e6;

/// The single-frequency carrier (Hz) the ionosphere correction is reported on
/// for a constellation, or `None` for a system with no modeled single-frequency
/// signal. GPS L1 C/A and Galileo E1 are both at [`F_L1_HZ`]; BeiDou uses B1I.
/// The broadcast Klobuchar delay is computed on L1 and scaled to this carrier by
/// the dispersive `(f_L1 / f)^2` factor (see [`crate::ionex::klobuchar_native`]).
pub(crate) const fn carrier_frequency_hz(system: GnssSystem) -> Option<f64> {
    match system {
        GnssSystem::Gps | GnssSystem::Galileo => Some(F_L1_HZ),
        GnssSystem::BeiDou => Some(F_B1I_HZ),
        _ => None,
    }
}
/// WGS84 Earth rotation rate (rad/s), used by the Sagnac correction.
pub const OMEGA_E_DOT_RAD_S: f64 = 7.2921151467e-5;

// WGS84 + AU constants replicating the core itrs_to_geodetic_compute operation
// tree (Skyfield's AU-internal 3-iteration algorithm) so the meters/radians
// geodetic helper is bit-exact at the boundary against that function.
const AU_KM: f64 = 149_597_870.700;
const WGS84_A_KM: f64 = 6378.137;
const WGS84_F: f64 = 1.0 / 298.257223563;
const WGS84_E2: f64 = 2.0 * WGS84_F - WGS84_F * WGS84_F;
const PI: f64 = std::f64::consts::PI;
const TAU: f64 = std::f64::consts::TAU;

/// Transmit-time iteration count (fixed, no inner convergence test).
pub const TRANSMIT_TIME_ITERATIONS: usize = 2;
/// Elevation mask in radians (10 degrees); a satellite is excluded iff its
/// elevation is strictly below this value.
pub const ELEVATION_MASK_RAD: f64 = 10.0 * PI / 180.0;
/// Base measurement standard deviation (m) for the elevation weight model.
pub const SIGMA0_M: f64 = 1.0;

/// A single GPS L1 pseudorange observation.
///
/// The input boundary of the pipeline is the pseudorange; raw observation
/// formation (RINEX decoding, code tracking) is out of scope. The receive epoch
/// and the time-of-day / day-of-year arguments are common to all observations
/// in one solve and are carried on [`SolveInputs`], not here.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Observation {
    /// The transmitting satellite.
    pub satellite_id: GnssSatelliteId,
    /// Measured pseudorange in meters.
    pub pseudorange_m: f64,
}

/// Why a satellite was excluded from the solve, in pinned priority order.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum RejectionReason {
    /// The SP3 product has no usable position or clock for the satellite at the
    /// transmit epoch.
    NoEphemeris,
    /// The satellite is below the elevation mask at the frozen geometry.
    LowElevation,
}

/// A rejected satellite paired with its rejection reason.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct RejectedSat {
    /// The excluded satellite.
    pub satellite_id: GnssSatelliteId,
    /// The first matching rejection reason.
    pub reason: RejectionReason,
}

/// Models and convergence detail describing how a solution was produced.
#[derive(Debug, Clone, PartialEq)]
pub struct SolutionMetadata {
    /// Number of accepted solver iterations.
    pub iterations: usize,
    /// Whether the solver reached a convergence stopping criterion (as opposed
    /// to exhausting its evaluation budget).
    pub converged: bool,
    /// The solver's termination status.
    pub status: Status,
    /// Whether the ionosphere correction was applied.
    pub ionosphere_applied: bool,
    /// Whether the troposphere correction was applied.
    pub troposphere_applied: bool,
}

/// A receiver position/clock solution with its geometry diagnostics.
#[derive(Debug, Clone)]
pub struct ReceiverSolution {
    /// Converged receiver position, ITRF/IGS ECEF meters.
    pub position: ItrfPositionM,
    /// The geodetic form of the position, if the conversion was requested.
    pub geodetic: Option<Wgs84Geodetic>,
    /// Receiver clock bias in seconds (`clk_0 / c`) for the reference GNSS — the
    /// first entry of `system_clocks_s`. For a single-system solve this is the
    /// only clock; for a multi-system solve the other systems' absolute clocks
    /// are in `system_clocks_s`.
    pub rx_clock_s: f64,
    /// The absolute receiver clock for each GNSS in the solve, in ascending
    /// system order, in seconds. One entry for a single-system solve; one per
    /// constellation for a multi-system solve. The first entry equals
    /// `rx_clock_s`; the inter-system bias for any other system is *its clock
    /// minus that reference* (these are absolute per-system clocks, not biases).
    pub system_clocks_s: Vec<(GnssSystem, f64)>,
    /// Dilution-of-precision scalars from the converged geometry. A
    /// single-system solve uses the 0-ULP four-state cofactor; a multi-system
    /// solve uses the general inverse with one clock column per constellation (a
    /// deterministic diagnostic, not a 0-ULP target). `None` only if the
    /// converged geometry is rank-deficient.
    pub dop: Option<Dop>,
    /// Post-fit residuals in meters, in `used_sats` order (unweighted
    /// `P_meas - P_hat`).
    pub residuals_m: Vec<f64>,
    /// The satellites that contributed to the solve, ascending id order.
    pub used_sats: Vec<GnssSatelliteId>,
    /// The excluded satellites, each with its reason.
    pub rejected_sats: Vec<RejectedSat>,
    /// Iteration / convergence / model metadata.
    pub metadata: SolutionMetadata,
}

/// Which correction terms a solve applies, building up incrementally.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Corrections {
    /// Apply the Klobuchar L1 ionosphere delay.
    pub ionosphere: bool,
    /// Apply the Saastamoinen/Niell troposphere delay.
    pub troposphere: bool,
}

impl Corrections {
    /// No atmospheric corrections (geometry + clock + Sagnac only).
    pub const NONE: Self = Self {
        ionosphere: false,
        troposphere: false,
    };
    /// Ionosphere only.
    pub const IONO: Self = Self {
        ionosphere: true,
        troposphere: false,
    };
    /// Ionosphere and troposphere.
    pub const IONO_TROPO: Self = Self {
        ionosphere: true,
        troposphere: true,
    };
}

/// Broadcast Klobuchar coefficients for the ionosphere term.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct KlobucharCoeffs {
    /// Cosine-amplitude polynomial coefficients (a0..a3).
    pub alpha: [f64; 4],
    /// Period polynomial coefficients (b0..b3).
    pub beta: [f64; 4],
}

/// Surface meteorology for the troposphere term.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SurfaceMet {
    /// Total pressure (hPa).
    pub pressure_hpa: f64,
    /// Temperature (K).
    pub temperature_k: f64,
    /// Relative humidity, fraction in `[0, 1]`.
    pub relative_humidity: f64,
}

/// Everything one SPP solve needs besides the SP3 product itself.
///
/// The receive epoch is carried as seconds-since-J2000 (`t_rx_j2000_s`), the
/// argument the transmit-time iteration differences against the geometric range
/// to land the satellite ephemeris at transmission, with no Julian-date
/// round-trip inside the loop. The Klobuchar diurnal argument
/// (`t_rx_second_of_day_s`) and the Niell seasonal argument (`day_of_year`) are
/// supplied directly so the correction kernels run in their bit-exact native
/// units.
#[derive(Debug, Clone)]
pub struct SolveInputs {
    /// The pseudorange observations (any order; the solve sorts them).
    pub observations: Vec<Observation>,
    /// Receive epoch, seconds since J2000 in the SP3 product's time scale.
    pub t_rx_j2000_s: f64,
    /// GPS second-of-day of the receive epoch (Klobuchar diurnal argument).
    pub t_rx_second_of_day_s: f64,
    /// Fractional day-of-year of the receive epoch (Niell seasonal argument).
    pub day_of_year: f64,
    /// Initial guess `[x_m, y_m, z_m, b_m]`.
    pub initial_guess: [f64; 4],
    /// The correction terms to apply.
    pub corrections: Corrections,
    /// Broadcast Klobuchar coefficients (used iff `corrections.ionosphere`).
    /// Applied to every system unless `beidou_klobuchar` overrides BeiDou.
    pub klobuchar: KlobucharCoeffs,
    /// Optional BeiDou-specific Klobuchar coefficients (the broadcast `BDSA`/
    /// `BDSB` set). When present, BeiDou satellites use these instead of
    /// [`klobuchar`](Self::klobuchar); both feed the same model, frequency-scaled
    /// to B1I. `None` falls back to `klobuchar` for BeiDou too.
    pub beidou_klobuchar: Option<KlobucharCoeffs>,
    /// Surface meteorology (used iff `corrections.troposphere`).
    pub met: SurfaceMet,
}

/// Error from [`solve`].
#[derive(Debug, Clone)]
pub enum SppError {
    /// Fewer usable satellites survived rejection than the solve has parameters
    /// (`3 + n_systems`: three position components plus one receiver clock per
    /// GNSS), so the solve is underdetermined.
    TooFewSatellites {
        /// The number of satellites that survived rejection.
        used: usize,
        /// The number of satellites required (`3 + n_systems`).
        required: usize,
    },
    /// The trust-region step hit a rank-deficient Jacobian (degenerate geometry).
    Singular(least_squares::SolveError),
    /// The same satellite appears in more than one observation. One pseudorange
    /// per satellite is required, so the input is rejected rather than silently
    /// picking one (which would make the result depend on observation order).
    DuplicateObservation {
        /// The satellite that was observed more than once.
        satellite: GnssSatelliteId,
    },
    /// A satellite that survived the frozen selection had no usable SP3
    /// position/clock at a transmit epoch reached during the solve. Returned
    /// instead of panicking; normally precluded by the selection step.
    EphemerisLost {
        /// The satellite whose ephemeris became unavailable during the solve.
        satellite: GnssSatelliteId,
    },
    /// The ionosphere correction was requested but an observed satellite is on a
    /// system with no modeled single-frequency carrier, so the L1 Klobuchar delay
    /// cannot be scaled to it. GPS L1, Galileo E1, and BeiDou B1I are covered;
    /// a system such as GLONASS (FDMA, per-satellite frequency) is rejected rather
    /// than corrected with an undefined frequency.
    IonosphereUnsupported {
        /// The satellite the ionosphere model does not cover.
        satellite: GnssSatelliteId,
    },
}

impl core::fmt::Display for SppError {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        match self {
            SppError::TooFewSatellites { used, required } => write!(
                f,
                "only {used} usable satellites; need at least {required} \
                 (3 position + 1 clock per GNSS)"
            ),
            SppError::Singular(e) => write!(f, "degenerate geometry: {e}"),
            SppError::DuplicateObservation { satellite } => {
                write!(f, "satellite {satellite} observed more than once")
            }
            SppError::EphemerisLost { satellite } => {
                write!(f, "satellite {satellite} lost ephemeris during the solve")
            }
            SppError::IonosphereUnsupported { satellite } => write!(
                f,
                "ionosphere correction has no modeled carrier frequency for {satellite}"
            ),
        }
    }
}

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

impl From<least_squares::SolveError> for SppError {
    fn from(e: least_squares::SolveError) -> Self {
        SppError::Singular(e)
    }
}

// ---------------------------------------------------------------------------
// Geodetic (ECEF meters -> lat/lon radians, height meters).
//
// Replicates the core itrs_to_geodetic_compute operation tree (Skyfield's
// AU-internal 3-iteration algorithm), taking meters in and radians+meters out,
// so no degree boundary appears inside the SPP loop. Cross-checked bit-for-bit
// against the core km/deg function at the boundary in the parity tests.
// ---------------------------------------------------------------------------
pub(crate) fn geodetic_from_ecef_m(x_m: f64, y_m: f64, z_m: f64) -> Wgs84Geodetic {
    let x = x_m / 1000.0;
    let y = y_m / 1000.0;
    let z = z_m / 1000.0;

    let x_au = x / AU_KM;
    let y_au = y / AU_KM;
    let z_au = z / AU_KM;

    let a_au = WGS84_A_KM / AU_KM;
    let r_xy = (x_au * x_au + y_au * y_au).sqrt();

    let lon_raw = y_au.atan2(x_au);
    let mut lon_shifted = (lon_raw - PI) % TAU;
    if lon_shifted < 0.0 {
        lon_shifted += TAU;
    }
    let lon = lon_shifted - PI;

    let mut lat = z_au.atan2(r_xy);
    let mut a_c = 0.0;
    let mut hyp = 0.0;
    for _ in 0..3 {
        let sin_lat = lat.sin();
        let e2_sin_lat = WGS84_E2 * sin_lat;
        a_c = a_au / (1.0 - e2_sin_lat * sin_lat).sqrt();
        hyp = z_au + a_c * e2_sin_lat;
        lat = hyp.atan2(r_xy);
    }

    let height_au = (hyp * hyp + r_xy * r_xy).sqrt() - a_c;
    let height_m = height_au * AU_KM * 1000.0;

    Wgs84Geodetic {
        lat_rad: lat,
        lon_rad: lon,
        height_m,
    }
}

/// Azimuth / elevation of a satellite from the receiver, ECEF meters in,
/// radians out, plus the receiver geodetic recomputed from the current state.
pub(crate) struct AzEl {
    pub geodetic: Wgs84Geodetic,
    pub az_rad: f64,
    pub el_rad: f64,
}

pub(crate) fn az_el_from_ecef(rx_ecef_m: [f64; 3], sat_ecef_m: [f64; 3]) -> AzEl {
    let dx = sat_ecef_m[0] - rx_ecef_m[0];
    let dy = sat_ecef_m[1] - rx_ecef_m[1];
    let dz = sat_ecef_m[2] - rx_ecef_m[2];

    let geo = geodetic_from_ecef_m(rx_ecef_m[0], rx_ecef_m[1], rx_ecef_m[2]);
    let sin_lat = geo.lat_rad.sin();
    let cos_lat = geo.lat_rad.cos();
    let sin_lon = geo.lon_rad.sin();
    let cos_lon = geo.lon_rad.cos();

    let e = -sin_lon * dx + cos_lon * dy;
    let n = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz;
    let u = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz;

    let rng = (e * e + n * n + u * u).sqrt();
    let el = (u / rng).asin();
    let mut az = e.atan2(n);
    if az < 0.0 {
        az += TAU;
    }

    AzEl {
        geodetic: geo,
        az_rad: az,
        el_rad: el,
    }
}

/// Per-satellite model intermediates, recorded for the 0-ULP parity track.
///
/// The solve path reads only the predicted range, elevation, and rotated
/// position; the remaining intermediates exist so the trace-replay parity test
/// can assert each named value (transmit time, satellite ECEF, Sagnac angle,
/// ionosphere, troposphere) bit-for-bit against the reference recipe.
#[derive(Debug, Clone, Copy)]
#[allow(dead_code)]
pub(crate) struct SatModel {
    pub tau_s: f64,
    pub t_tx_j2000_s: f64,
    pub sat_ecef_m: [f64; 3],
    pub dt_sat_s: f64,
    pub theta_rad: f64,
    pub sat_rot_ecef_m: [f64; 3],
    pub rho_m: f64,
    pub az_rad: f64,
    pub el_rad: f64,
    pub iono_m: f64,
    pub tropo_m: f64,
    pub p_hat_m: f64,
}

/// A source of satellite position and clock at a transmit epoch.
///
/// The SPP pipeline is written against this trait rather than a concrete product
/// so it can run on either a precise SP3 ephemeris or a broadcast navigation
/// message. The contract is exactly what the transmit-time iteration needs: the
/// ECEF position (meters) and the satellite clock offset (seconds) at a given
/// J2000 second, or `None` if the source has no usable ephemeris for that
/// satellite at that instant.
pub trait EphemerisSource {
    /// ECEF position (m) and satellite clock offset (s) for `sat` at `t_j2000_s`.
    fn position_clock_at_j2000_s(
        &self,
        sat: GnssSatelliteId,
        t_j2000_s: f64,
    ) -> Option<([f64; 3], f64)>;
}

impl EphemerisSource for Sp3 {
    fn position_clock_at_j2000_s(
        &self,
        sat: GnssSatelliteId,
        t_j2000_s: f64,
    ) -> Option<([f64; 3], f64)> {
        let state = self.position_at_j2000_seconds(sat, t_j2000_s).ok()?;
        let clk = state.clock_s?;
        Some((state.position.as_array(), clk))
    }
}

/// The Klobuchar coefficients a satellite's system uses for the ionosphere
/// correction: BeiDou prefers its own `beidou_klobuchar` (the broadcast
/// `BDSA`/`BDSB` set) when present; every other system, and BeiDou when no
/// BeiDou-specific set was supplied, uses the shared [`SolveInputs::klobuchar`].
fn klobuchar_for(system: GnssSystem, inputs: &SolveInputs) -> &KlobucharCoeffs {
    match (system, &inputs.beidou_klobuchar) {
        (GnssSystem::BeiDou, Some(bds)) => bds,
        _ => &inputs.klobuchar,
    }
}

/// Build the per-satellite predicted pseudorange and all named intermediates in
/// the pinned operation order. Returns `None` if the ephemeris source has no
/// usable position/clock for the satellite at the transmit epoch.
#[allow(clippy::too_many_arguments)]
pub(crate) fn sat_model(
    eph: &dyn EphemerisSource,
    sat: GnssSatelliteId,
    rx_ecef_m: [f64; 3],
    b_m: f64,
    p_meas_m: f64,
    t_rx_j2000_s: f64,
    t_rx_second_of_day_s: f64,
    day_of_year: f64,
    corrections: Corrections,
    klobuchar: &KlobucharCoeffs,
    met: &SurfaceMet,
) -> Option<SatModel> {
    // Transmit-time iteration: fixed count, no inner convergence test.
    let mut tau = p_meas_m / C_M_S;
    let mut t_tx = t_rx_j2000_s - tau;
    let mut sat_pos = [0.0f64; 3];
    let mut dt_sat = 0.0f64;
    for _ in 0..TRANSMIT_TIME_ITERATIONS {
        let (pos, clk) = eph.position_clock_at_j2000_s(sat, t_tx)?;
        sat_pos = pos;
        dt_sat = clk;
        let d0x = sat_pos[0] - rx_ecef_m[0];
        let d0y = sat_pos[1] - rx_ecef_m[1];
        let d0z = sat_pos[2] - rx_ecef_m[2];
        let rho0 = (d0x * d0x + d0y * d0y + d0z * d0z).sqrt();
        tau = rho0 / C_M_S;
        t_tx = t_rx_j2000_s - tau;
    }

    // Sagnac / Earth-rotation closed-form +theta Z-rotation over the flight time.
    let theta = OMEGA_E_DOT_RAD_S * tau;
    let cos_t = theta.cos();
    let sin_t = theta.sin();
    let sat_rot = [
        cos_t * sat_pos[0] + sin_t * sat_pos[1],
        -sin_t * sat_pos[0] + cos_t * sat_pos[1],
        sat_pos[2],
    ];

    // Geometric range (post-Sagnac).
    let drx = sat_rot[0] - rx_ecef_m[0];
    let dry = sat_rot[1] - rx_ecef_m[1];
    let drz = sat_rot[2] - rx_ecef_m[2];
    let rho = (drx * drx + dry * dry + drz * drz).sqrt();

    // Geometry for corrections: az/el from rx and the Sagnac-rotated satellite.
    let g = az_el_from_ecef(rx_ecef_m, sat_rot);

    let mut iono_m = 0.0;
    let mut tropo_m = 0.0;
    if corrections.ionosphere {
        let lat_deg = g.geodetic.lat_rad * 180.0 / PI;
        let lon_deg = g.geodetic.lon_rad * 180.0 / PI;
        let az_deg = g.az_rad * 180.0 / PI;
        let el_deg = g.el_rad * 180.0 / PI;
        // Klobuchar L1 delay scaled to the satellite's carrier by `(f_L1 / f)^2`.
        // GPS L1 / Galileo E1 are at f_L1 so the factor is exactly 1.0 (the L1
        // delay is unchanged bit-for-bit); BeiDou B1I is scaled to its frequency.
        // A used satellite always has a modeled carrier here (the solve rejects
        // an ionosphere request for any system that does not), so the fallback is
        // unreachable.
        let freq_hz = carrier_frequency_hz(sat.system).unwrap_or(F_L1_HZ);
        iono_m = klobuchar_native(
            &KlobucharParams {
                alpha: klobuchar.alpha,
                beta: klobuchar.beta,
            },
            lat_deg,
            lon_deg,
            az_deg,
            el_deg,
            t_rx_second_of_day_s,
            freq_hz,
        );
    }
    if corrections.troposphere {
        tropo_m = slant_components(
            g.el_rad,
            g.geodetic.lat_rad,
            g.geodetic.lon_rad,
            g.geodetic.height_m,
            met.pressure_hpa,
            met.temperature_k,
            met.relative_humidity,
            day_of_year,
        )
        .slant_m;
    }

    // Predicted pseudorange, left-to-right; c*dt_sat is a single multiply.
    let p_hat = rho + b_m - C_M_S * dt_sat + iono_m + tropo_m;

    Some(SatModel {
        tau_s: tau,
        t_tx_j2000_s: t_tx,
        sat_ecef_m: sat_pos,
        dt_sat_s: dt_sat,
        theta_rad: theta,
        sat_rot_ecef_m: sat_rot,
        rho_m: rho,
        az_rad: g.az_rad,
        el_rad: g.el_rad,
        iono_m,
        tropo_m,
        p_hat_m: p_hat,
    })
}

/// The frozen-geometry selection: used satellites (ascending id), rejected
/// satellites with reason, and the per-used-sat weight from the elevation at
/// the initial-guess geometry.
pub(crate) struct Selection {
    pub used: Vec<GnssSatelliteId>,
    pub rejected: Vec<RejectedSat>,
    /// `sqrt(weight)` per used satellite, index-aligned to `used`. Used by the
    /// trace-replay parity track to weight the residual the FD Jacobian sees.
    #[allow(dead_code)]
    pub sqrt_weights: Vec<f64>,
    /// `weight` per used satellite, index-aligned to `used`.
    pub weights: Vec<f64>,
}

pub(crate) fn select_sats(eph: &dyn EphemerisSource, inputs: &SolveInputs) -> Selection {
    let rx0 = [
        inputs.initial_guess[0],
        inputs.initial_guess[1],
        inputs.initial_guess[2],
    ];
    let b0 = inputs.initial_guess[3];

    // Ascending satellite-id order, never observation order.
    let mut obs: Vec<&Observation> = inputs.observations.iter().collect();
    obs.sort_by_key(|o| o.satellite_id);

    let mut used = Vec::new();
    let mut rejected = Vec::new();
    let mut sqrt_weights = Vec::new();
    let mut weights = Vec::new();

    for ob in obs {
        let model = sat_model(
            eph,
            ob.satellite_id,
            rx0,
            b0,
            ob.pseudorange_m,
            inputs.t_rx_j2000_s,
            inputs.t_rx_second_of_day_s,
            inputs.day_of_year,
            inputs.corrections,
            klobuchar_for(ob.satellite_id.system, inputs),
            &inputs.met,
        );
        let model = match model {
            Some(m) => m,
            None => {
                rejected.push(RejectedSat {
                    satellite_id: ob.satellite_id,
                    reason: RejectionReason::NoEphemeris,
                });
                continue;
            }
        };
        if model.el_rad < ELEVATION_MASK_RAD {
            rejected.push(RejectedSat {
                satellite_id: ob.satellite_id,
                reason: RejectionReason::LowElevation,
            });
            continue;
        }
        let sin_el = model.el_rad.sin();
        let weight = (sin_el * sin_el) / (SIGMA0_M * SIGMA0_M);
        used.push(ob.satellite_id);
        weights.push(weight);
        sqrt_weights.push(weight.sqrt());
    }

    Selection {
        used,
        rejected,
        sqrt_weights,
        weights,
    }
}

/// The distinct GNSS present in `used`, in ascending system order.
///
/// The receiver-clock part of the state has one entry per system, each the
/// *absolute* receiver clock for that system (not a bias); the first is the
/// reference clock and a system's inter-system bias is its clock minus that
/// reference. For a single-system solve this is one element and the state is the
/// classic `[x, y, z, b]`.
pub(crate) fn clock_systems(used: &[GnssSatelliteId]) -> Vec<GnssSystem> {
    let mut systems: Vec<GnssSystem> = used.iter().map(|s| s.system).collect();
    systems.sort_unstable();
    systems.dedup();
    systems
}

/// The unweighted residual vector `P_meas - P_hat` at state `x`, in `used` order.
///
/// The state is `[x, y, z, clk_0, clk_1, ...]` where `clk_i` is the absolute
/// receiver clock for the i-th system returned by [`clock_systems`] (in meters).
/// Each satellite's residual uses its own system's clock, so a multi-GNSS set is
/// solved with one absolute receiver clock per system (a system's inter-system
/// bias is its clock minus the reference `clk_0`). A single-system set reduces to
/// `[x, y, z, b]` and `clk_0 = x[3]`.
///
/// Returns `Err(satellite)` if a used satellite has no observation or no usable
/// ephemeris at `x` (the frozen used set is fixed, but a finite-difference probe
/// could in principle reach an epoch off the ephemeris coverage). The caller
/// turns that into an [`SppError`] rather than panicking.
#[allow(clippy::too_many_arguments)]
pub(crate) fn residual_unweighted(
    eph: &dyn EphemerisSource,
    used: &[GnssSatelliteId],
    obs_by_id: &[(GnssSatelliteId, f64)],
    x: &[f64],
    inputs: &SolveInputs,
) -> Result<Vec<f64>, GnssSatelliteId> {
    let rx = [x[0], x[1], x[2]];
    let systems = clock_systems(used);
    let mut out = Vec::with_capacity(used.len());
    for &sat in used {
        let p_meas = obs_by_id
            .iter()
            .find(|(id, _)| *id == sat)
            .map(|(_, p)| *p)
            .ok_or(sat)?;
        // The clock for this satellite's system (index 0 = reference clock).
        let sys_idx = systems.iter().position(|s| *s == sat.system).unwrap_or(0);
        let b = x[3 + sys_idx];
        let m = sat_model(
            eph,
            sat,
            rx,
            b,
            p_meas,
            inputs.t_rx_j2000_s,
            inputs.t_rx_second_of_day_s,
            inputs.day_of_year,
            inputs.corrections,
            klobuchar_for(sat.system, inputs),
            &inputs.met,
        )
        .ok_or(sat)?;
        out.push(p_meas - m.p_hat_m);
    }
    Ok(out)
}

/// Run the SPP solve from synthesized/measured pseudoranges.
///
/// Uses the core trust-region weighted least-squares solver over the
/// `sqrt(w) * (P_meas - P_hat)` residual. The converged position/clock is a
/// sub-micron solver-agreement result (the linear-algebra step is not
/// bit-reproducible across BLAS builds), not a 0-ULP claim. The residual /
/// Jacobian substrate evaluated at recorded states is the 0-ULP target and is
/// exercised by the trace-replay parity test, not by this entry point.
pub fn solve(
    eph: &dyn EphemerisSource,
    inputs: &SolveInputs,
    with_geodetic: bool,
) -> Result<ReceiverSolution, SppError> {
    // One pseudorange per satellite. Reject duplicates deterministically (by
    // the smallest repeated id) so the result can never depend on observation
    // order and the >= 4 check below counts distinct satellites.
    let mut ids: Vec<GnssSatelliteId> =
        inputs.observations.iter().map(|o| o.satellite_id).collect();
    ids.sort_unstable();
    if let Some(w) = ids.windows(2).find(|w| w[0] == w[1]) {
        return Err(SppError::DuplicateObservation { satellite: w[0] });
    }

    // The broadcast Klobuchar delay is computed on L1 and scaled to each
    // satellite's carrier by `(f_L1 / f)^2`, which covers GPS L1, Galileo E1, and
    // BeiDou B1I. A system with no modeled single-frequency carrier (e.g. GLONASS
    // FDMA) cannot be scaled, so reject an ionosphere-corrected solve that
    // includes such an observation rather than apply an undefined correction.
    // This runs before selection so the model is never evaluated for it
    // (`select_sats` would otherwise call `sat_model` with the correction for
    // every observation).
    if inputs.corrections.ionosphere {
        if let Some(sat) = ids
            .iter()
            .find(|s| carrier_frequency_hz(s.system).is_none())
        {
            return Err(SppError::IonosphereUnsupported { satellite: *sat });
        }
    }

    let sel = select_sats(eph, inputs);

    // One receiver-clock parameter per distinct GNSS (a reference clock plus an
    // inter-system bias for each additional system), so the state has
    // `3 + n_systems` parameters and needs at least that many usable satellites.
    // Floor the clock count at one: the minimum solve is the four-parameter
    // single-system form even when no satellite survives selection.
    let systems = clock_systems(&sel.used);
    let n_clocks = systems.len();
    let n_params = 3 + n_clocks.max(1);
    if sel.used.len() < n_params {
        return Err(SppError::TooFewSatellites {
            used: sel.used.len(),
            required: n_params,
        });
    }

    let obs_by_id: Vec<(GnssSatelliteId, f64)> = inputs
        .observations
        .iter()
        .map(|o| (o.satellite_id, o.pseudorange_m))
        .collect();

    let used = sel.used.clone();
    let inputs_ref = inputs.clone();
    let obs_ref = obs_by_id.clone();
    let eph_ref = eph;
    let n_used = used.len();

    // The least-squares solver's residual closure cannot return an error, so an
    // ephemeris loss during a probe is recorded here and surfaced as an
    // SppError after the solve (rather than panicking inside the closure).
    let lost = std::rc::Rc::new(std::cell::Cell::new(None::<GnssSatelliteId>));
    let lost_in = lost.clone();
    let residual = move |x: &DVector<f64>| -> DVector<f64> {
        match residual_unweighted(eph_ref, &used, &obs_ref, x.as_slice(), &inputs_ref) {
            Ok(r) => DVector::from_vec(r),
            Err(sat) => {
                lost_in.set(Some(sat));
                DVector::from_vec(vec![0.0; n_used])
            }
        }
    };

    // Extend the 4-element initial guess `[x, y, z, b_ref]` with a zero starting
    // value for each additional system's inter-system bias.
    let mut x0v = inputs.initial_guess.to_vec();
    x0v.extend(std::iter::repeat_n(0.0, n_clocks - 1));
    let x0 = DVector::from_vec(x0v);
    let weights = DVector::from_row_slice(&sel.weights);
    let problem = LeastSquaresProblem::with_weights(residual, x0, weights);
    // Agreement-track tolerances: drive the independent solver to the true fixed
    // point of this (noise-free, by-construction-zero-residual) problem so the
    // converged position agrees with the reference solution to the documented
    // sub-micron bound. These are the independent solver's own stopping
    // thresholds, not the parity target's pinned scipy options.
    let opts = SolveOptions {
        gtol: 1e-14,
        ftol: 1e-15,
        xtol: 1e-14,
        max_nfev: 400,
    };
    // Check for an ephemeris loss recorded by the residual closure BEFORE
    // propagating a solver error: a lost satellite zeroes its residual row,
    // which can itself make the Jacobian singular, and EphemerisLost is the
    // more specific, actionable cause.
    let report_result = solve_trf(&problem, &opts);
    if let Some(satellite) = lost.get() {
        return Err(SppError::EphemerisLost { satellite });
    }
    let report = report_result?;

    let xs = &report.x;
    let position = ItrfPositionM::new(xs[0], xs[1], xs[2]);
    let rx_clock_s = xs[3] / C_M_S;
    // One receiver clock (seconds) per system, in the same order as the state's
    // clock parameters. The first equals `rx_clock_s` (the reference system).
    let system_clocks_s: Vec<(GnssSystem, f64)> = systems
        .iter()
        .enumerate()
        .map(|(i, &sys)| (sys, xs[3 + i] / C_M_S))
        .collect();
    let geodetic = if with_geodetic {
        Some(geodetic_from_ecef_m(xs[0], xs[1], xs[2]))
    } else {
        None
    };

    // Post-fit unweighted residuals in used order.
    let residuals_m = residual_unweighted(eph, &sel.used, &obs_by_id, xs.as_slice(), inputs)
        .map_err(|satellite| SppError::EphemerisLost { satellite })?;

    // DOP from the converged geometry: line-of-sight unit vectors to the
    // Sagnac-rotated satellite positions, with the frozen weights. A
    // single-system solve uses the 0-ULP four-state cofactor inverse; a
    // multi-system solve uses the general (3 + n_systems) inverse with one clock
    // column per GNSS (a deterministic geometry diagnostic, not a 0-ULP target).
    // The receiver-clock argument does not affect the line of sight, so the
    // reference clock is passed for every satellite.
    let rx_ecef = [xs[0], xs[1], xs[2]];
    let geo = geodetic_from_ecef_m(xs[0], xs[1], xs[2]);
    let mut los = Vec::with_capacity(sel.used.len());
    let mut clock_index = Vec::with_capacity(sel.used.len());
    for &sat in &sel.used {
        let p_meas = obs_by_id
            .iter()
            .find(|(id, _)| *id == sat)
            .map(|(_, p)| *p)
            .ok_or(SppError::EphemerisLost { satellite: sat })?;
        let m = sat_model(
            eph,
            sat,
            rx_ecef,
            xs[3],
            p_meas,
            inputs.t_rx_j2000_s,
            inputs.t_rx_second_of_day_s,
            inputs.day_of_year,
            inputs.corrections,
            klobuchar_for(sat.system, inputs),
            &inputs.met,
        )
        .ok_or(SppError::EphemerisLost { satellite: sat })?;
        let dx = m.sat_rot_ecef_m[0] - rx_ecef[0];
        let dy = m.sat_rot_ecef_m[1] - rx_ecef[1];
        let dz = m.sat_rot_ecef_m[2] - rx_ecef[2];
        let n = (dx * dx + dy * dy + dz * dz).sqrt();
        los.push(LineOfSight::new(dx / n, dy / n, dz / n));
        let idx = systems.iter().position(|s| *s == sat.system).unwrap_or(0);
        clock_index.push(idx);
    }
    let dop_result = if n_clocks == 1 {
        dop(&los, &sel.weights, geo).ok()
    } else {
        dop_multi(&los, &clock_index, n_clocks, &sel.weights, geo).ok()
    };

    let converged = matches!(
        report.status,
        Status::GradientTolerance | Status::CostTolerance | Status::StepTolerance
    );

    Ok(ReceiverSolution {
        position,
        geodetic,
        rx_clock_s,
        system_clocks_s,
        dop: dop_result,
        residuals_m,
        used_sats: sel.used,
        rejected_sats: sel.rejected,
        metadata: SolutionMetadata {
            iterations: report.iterations,
            converged,
            status: report.status,
            ionosphere_applied: inputs.corrections.ionosphere,
            troposphere_applied: inputs.corrections.troposphere,
        },
    })
}

/// The core km/deg geodetic recipe, for the boundary cross-check against the
/// meters-native helper.
#[cfg(test)]
pub(crate) mod test_support {
    use super::*;

    pub fn geodetic_from_ecef_m_for_test(x_m: f64, y_m: f64, z_m: f64) -> Wgs84Geodetic {
        geodetic_from_ecef_m(x_m, y_m, z_m)
    }

    pub fn sat_model_for_test(
        eph: &dyn EphemerisSource,
        sat: GnssSatelliteId,
        rx: [f64; 3],
        b_m: f64,
        p_meas: f64,
        t_rx_j2000_s: f64,
        sod: f64,
        doy: f64,
        corrections: Corrections,
        klobuchar: &KlobucharCoeffs,
        met: &SurfaceMet,
    ) -> Option<SatModel> {
        sat_model(
            eph,
            sat,
            rx,
            b_m,
            p_meas,
            t_rx_j2000_s,
            sod,
            doy,
            corrections,
            klobuchar,
            met,
        )
    }

    /// The core km/deg geodetic recipe (Skyfield AU-internal), returning the
    /// public `(lat_deg, lon_deg, alt_km)`, for the boundary cross-check.
    pub fn itrs_to_geodetic_core_km(x_km: f64, y_km: f64, z_km: f64) -> (f64, f64, f64) {
        astrodynamics::frames::transforms::itrs_to_geodetic_compute(x_km, y_km, z_km)
    }
}

#[cfg(test)]
mod tests;