astrodynamics-gnss 0.9.5

//! Broadcast-ephemeris orbit and clock evaluation (GPS LNAV, Galileo I/NAV,
//! BeiDou D1/D2).
//!
//! Evaluates a broadcast navigation message into an ECEF satellite position and
//! a satellite clock offset, by the standard Keplerian construction of
//! IS-GPS-200 (Section 20.3.3.4.3.1, Table 20-IV) and the equivalent Galileo OS
//! and BeiDou SIS ICD sections. The constellations share the algorithm and
//! differ in the gravitational constant, the Earth-rotation rate, and the
//! relativistic clock constant ([`ConstellationConstants`]). BeiDou
//! geostationary satellites additionally take a custom-frame-to-ECEF rotation
//! (the `is_geo` path of [`satellite_position_ecef`]); GPS, Galileo, and BeiDou
//! MEO/IGSO satellites use the direct rotation.
//!
//! This is a 0-ULP parity target: the operation order reproduces the canonical
//! reference recipe (`parity/generator/broadcast_eval.py`) bit-for-bit. The
//! transcendentals are the libm scalar `sin`/`cos`/`sqrt`/`atan2` (matching the
//! recipe's CPython `math` calls on the pinned Apple-libm target); separate
//! `.sin()` and `.cos()` calls are used deliberately (never `.sin_cos()`, whose
//! fused evaluation can differ in the last bit). Integer powers are explicit
//! repeated multiplies and there is no fused multiply-add (Rust does not
//! auto-contract `a * b + c`).

use crate::frame::ItrfPositionM;

/// Half a week, the fold threshold for a time difference against `toe`/`toc`.
pub use crate::constants::HALF_WEEK_S;
/// Seconds in one GPS/Galileo week.
pub use crate::constants::SECONDS_PER_WEEK;

/// Eccentric-anomaly fixed-point convergence threshold (radians).
pub const KEPLER_TOL: f64 = 1.0e-12;
/// Maximum eccentric-anomaly fixed-point iterations.
pub const KEPLER_MAX_ITER: usize = 30;
/// Satellite-clock time-argument refinement count (RTKLIB `eph2clk` convention).
pub const CLOCK_MAX_ITER: usize = 2;

/// Per-constellation physical constants used by the broadcast evaluation.
///
/// The literals match the values the broadcast reference recipe and the `rinex`
/// crate use, so the Python and Rust sides share identical `f64` bit patterns.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ConstellationConstants {
    /// Gravitational constant GM (m^3 / s^2).
    pub gm_m3_s2: f64,
    /// Earth rotation rate used by the longitude-of-node (Sagnac) term (rad/s).
    pub omega_e_rad_s: f64,
    /// Relativistic clock constant `F = -2 * sqrt(GM) / c^2` (s / sqrt(m)).
    pub dtr_f: f64,
}

impl ConstellationConstants {
    /// GPS constants (IS-GPS-200).
    pub const GPS: Self = Self {
        gm_m3_s2: 3.9860050E14,
        omega_e_rad_s: 7.2921151467E-5,
        dtr_f: -0.000000000444280763339306,
    };
    /// Galileo constants (OS SIS ICD); shares the GPS rotation rate.
    pub const GALILEO: Self = Self {
        gm_m3_s2: 3.986004418E14,
        omega_e_rad_s: 7.2921151467E-5,
        dtr_f: -0.00000000044428073090439775,
    };
    /// BeiDou constants (BDS-SIS-ICD); its own Earth-rotation rate.
    pub const BEIDOU: Self = Self {
        gm_m3_s2: 3.986004418E14,
        omega_e_rad_s: 7.292115E-5,
        dtr_f: -0.00000000044428073090439775,
    };
}

/// Broadcast Keplerian orbital elements (SI units; angles in radians; `toe_sow`
/// in seconds of the constellation's week).
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct KeplerianElements {
    /// Square root of the semi-major axis (sqrt(m)).
    pub sqrt_a: f64,
    /// Eccentricity (dimensionless).
    pub e: f64,
    /// Mean anomaly at reference time (rad).
    pub m0: f64,
    /// Mean motion difference from computed value (rad/s).
    pub delta_n: f64,
    /// Longitude of ascending node at weekly epoch (rad).
    pub omega0: f64,
    /// Inclination at reference time (rad).
    pub i0: f64,
    /// Argument of perigee (rad).
    pub omega: f64,
    /// Rate of right ascension (rad/s).
    pub omega_dot: f64,
    /// Rate of inclination (rad/s).
    pub idot: f64,
    /// Latitude argument cosine correction (rad).
    pub cuc: f64,
    /// Latitude argument sine correction (rad).
    pub cus: f64,
    /// Orbit radius cosine correction (m).
    pub crc: f64,
    /// Orbit radius sine correction (m).
    pub crs: f64,
    /// Inclination cosine correction (rad).
    pub cic: f64,
    /// Inclination sine correction (rad).
    pub cis: f64,
    /// Ephemeris reference time, seconds of week.
    pub toe_sow: f64,
}

/// Broadcast satellite-clock polynomial about `toc_sow`.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ClockPolynomial {
    /// Clock bias (s).
    pub af0: f64,
    /// Clock drift (s/s).
    pub af1: f64,
    /// Clock drift rate (s/s^2).
    pub af2: f64,
    /// Clock reference time, seconds of week.
    pub toc_sow: f64,
}

/// A solved eccentric anomaly and the iteration count that produced it.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct EccentricAnomaly {
    /// Eccentric anomaly (rad).
    pub value: f64,
    /// Fixed-point iterations performed.
    pub iterations: usize,
}

/// The full intermediate substrate of a broadcast orbit evaluation.
///
/// Every field is exposed so a 0-ULP parity test can localize a mismatch to a
/// single operation. [`OrbitState::position`] returns the ECEF position.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct OrbitState {
    /// Semi-major axis (m).
    pub a: f64,
    /// Computed mean motion (rad/s).
    pub n0: f64,
    /// Corrected mean motion (rad/s).
    pub n: f64,
    /// Time from ephemeris reference epoch, half-week folded (s).
    pub tk: f64,
    /// Mean anomaly (rad).
    pub mk: f64,
    /// Eccentric anomaly (rad).
    pub eccentric_anomaly: f64,
    /// Number of Kepler iterations.
    pub kepler_iterations: usize,
    /// sin(E).
    pub sin_e: f64,
    /// cos(E).
    pub cos_e: f64,
    /// True anomaly (rad).
    pub nu: f64,
    /// Argument of latitude before correction (rad).
    pub phi: f64,
    /// sin(2*phi).
    pub s2: f64,
    /// cos(2*phi).
    pub c2: f64,
    /// Argument-of-latitude correction (rad).
    pub du: f64,
    /// Radius correction (m).
    pub dr: f64,
    /// Inclination correction (rad).
    pub di: f64,
    /// Corrected argument of latitude (rad).
    pub u: f64,
    /// Corrected radius (m).
    pub r: f64,
    /// Corrected inclination (rad).
    pub i: f64,
    /// Orbital-plane x (m).
    pub xp: f64,
    /// Orbital-plane y (m).
    pub yp: f64,
    /// Corrected longitude of ascending node (rad).
    pub omega_k: f64,
    /// ECEF x (m).
    pub x_m: f64,
    /// ECEF y (m).
    pub y_m: f64,
    /// ECEF z (m).
    pub z_m: f64,
}

impl OrbitState {
    /// The Earth-fixed (ITRF/ECEF) satellite position in meters.
    pub const fn position(&self) -> ItrfPositionM {
        ItrfPositionM::new(self.x_m, self.y_m, self.z_m)
    }
}

/// The satellite clock offset, split into its components.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct ClockOffset {
    /// Polynomial term (s).
    pub dt_clock_poly_s: f64,
    /// Relativistic eccentricity term (s).
    pub dt_rel_s: f64,
    /// Group delay subtracted for the single-frequency user (s).
    pub tgd_s: f64,
    /// Total satellite clock offset (s).
    pub dt_clock_total_s: f64,
}

/// Time difference `t - t_ref` (seconds), folded into the +/- half-week range.
///
/// Used for both `tk = t - toe` (orbit) and `t - toc` (clock); the fold handles
/// a query that straddles the week rollover relative to the reference.
pub fn time_from_reference_s(t_sow_s: f64, t_ref_sow_s: f64) -> f64 {
    let mut dt = t_sow_s - t_ref_sow_s;
    if dt > HALF_WEEK_S {
        dt -= SECONDS_PER_WEEK;
    }
    if dt < -HALF_WEEK_S {
        dt += SECONDS_PER_WEEK;
    }
    dt
}

/// Solve Kepler's equation by fixed-point iteration `E = M + e*sin(E)`, seeded
/// at `E = M`; stops on `|dE| <= KEPLER_TOL` or after `KEPLER_MAX_ITER` steps.
pub fn eccentric_anomaly(mean_anomaly_rad: f64, eccentricity: f64) -> EccentricAnomaly {
    let mut e_k = mean_anomaly_rad;
    let mut iterations = 0usize;
    while iterations < KEPLER_MAX_ITER {
        let e_prev = e_k;
        e_k = mean_anomaly_rad + eccentricity * e_prev.sin();
        iterations += 1;
        let delta = (e_k - e_prev).abs();
        if delta <= KEPLER_TOL {
            break;
        }
    }
    EccentricAnomaly {
        value: e_k,
        iterations,
    }
}

/// Evaluate the broadcast Keplerian orbit at `t_sow_s` (seconds of week).
///
/// `is_geo` selects the BeiDou geostationary path (the node omits the
/// Earth-rotation-during-`tk` term and the position is rotated to ECEF by
/// `Rz(omega_e*tk) . Rx(-5deg)`); GPS, Galileo, and BeiDou MEO/IGSO use `false`.
/// The statement order reproduces `broadcast_eval.satellite_position_ecef`.
pub fn satellite_position_ecef(
    elements: &KeplerianElements,
    consts: &ConstellationConstants,
    t_sow_s: f64,
    is_geo: bool,
) -> OrbitState {
    let sqrt_a = elements.sqrt_a;
    let e = elements.e;
    let gm = consts.gm_m3_s2;
    let omega_e = consts.omega_e_rad_s;

    // 1. Semi-major axis and mean motion. a^3 as an explicit multiply chain.
    let a = sqrt_a * sqrt_a;
    let n0 = (gm / (a * a * a)).sqrt();
    let n = n0 + elements.delta_n;

    // 2. Time from ephemeris reference epoch (half-week folded).
    let tk = time_from_reference_s(t_sow_s, elements.toe_sow);

    // 3. Mean anomaly and eccentric anomaly.
    let mk = elements.m0 + n * tk;
    let kepler = eccentric_anomaly(mk, e);
    let ecc_anom = kepler.value;
    let sin_e = ecc_anom.sin();
    let cos_e = ecc_anom.cos();

    // 4. True anomaly (atan2 form) and argument of latitude.
    let e2 = e * e;
    let nu = ((1.0 - e2).sqrt() * sin_e).atan2(cos_e - e);
    let phi = nu + elements.omega;

    // 5. Second-harmonic corrections (sine term first).
    let two_phi = 2.0 * phi;
    let s2 = two_phi.sin();
    let c2 = two_phi.cos();
    let du = elements.cus * s2 + elements.cuc * c2;
    let dr = elements.crs * s2 + elements.crc * c2;
    let di = elements.cis * s2 + elements.cic * c2;

    // 6. Corrected argument of latitude, radius, inclination.
    let u = phi + du;
    let r = a * (1.0 - e * cos_e) + dr;
    let i = elements.i0 + di + elements.idot * tk;

    // 7. Position in the orbital plane.
    let xp = r * u.cos();
    let yp = r * u.sin();

    // 8. Corrected longitude of ascending node. The BeiDou GEO node omits the
    // Earth-rotation-during-tk term (applied by the final rotation instead).
    let omega_k = if is_geo {
        elements.omega0 + elements.omega_dot * tk - omega_e * elements.toe_sow
    } else {
        elements.omega0 + (elements.omega_dot - omega_e) * tk - omega_e * elements.toe_sow
    };

    // 9. Coordinates in the (custom) frame from the node rotation.
    let sin_o = omega_k.sin();
    let cos_o = omega_k.cos();
    let sin_i = i.sin();
    let cos_i = i.cos();
    let xg = xp * cos_o - yp * cos_i * sin_o;
    let yg = xp * sin_o + yp * cos_i * cos_o;
    let zg = yp * sin_i;

    // 10. Earth-fixed coordinates. The standard path is the identity; the BeiDou
    // GEO path applies Rz(omega_e*tk) . Rx(-5deg) (BDS-SIS-ICD).
    let (x, y, z) = if is_geo {
        let deg5 = 5.0_f64.to_radians();
        let cos_phi = deg5.cos();
        let sin_phi = -deg5.sin();
        let z_ang = omega_e * tk;
        let cos_z = z_ang.cos();
        let sin_z = z_ang.sin();
        let yr = yg * cos_phi + zg * sin_phi;
        let zr = -yg * sin_phi + zg * cos_phi;
        (xg * cos_z + yr * sin_z, -xg * sin_z + yr * cos_z, zr)
    } else {
        (xg, yg, zg)
    };

    OrbitState {
        a,
        n0,
        n,
        tk,
        mk,
        eccentric_anomaly: ecc_anom,
        kepler_iterations: kepler.iterations,
        sin_e,
        cos_e,
        nu,
        phi,
        s2,
        c2,
        du,
        dr,
        di,
        u,
        r,
        i,
        xp,
        yp,
        omega_k,
        x_m: x,
        y_m: y,
        z_m: z,
    }
}

/// Evaluate the broadcast satellite clock offset (seconds).
///
/// `sin_e` is the eccentric-anomaly sine from the position evaluation at the
/// same instant; `tgd_s` is the single-frequency group delay. The statement
/// order reproduces `broadcast_eval.satellite_clock_offset_s`.
pub fn satellite_clock_offset_s(
    clock: &ClockPolynomial,
    consts: &ConstellationConstants,
    elements: &KeplerianElements,
    sin_e: f64,
    t_sow_s: f64,
    tgd_s: f64,
) -> ClockOffset {
    let af0 = clock.af0;
    let af1 = clock.af1;
    let af2 = clock.af2;

    // Time from clock reference, folded; then refine out the SV clock itself.
    let dt0 = time_from_reference_s(t_sow_s, clock.toc_sow);
    let mut dt = dt0;
    let mut refine = 0usize;
    while refine < CLOCK_MAX_ITER {
        dt = dt0 - (af0 + af1 * dt + af2 * dt * dt);
        refine += 1;
    }
    let dt_poly = af0 + af1 * dt + af2 * dt * dt;

    // Relativistic eccentricity term (sqrt_a is the broadcast sqrt(A)).
    let dt_rel = consts.dtr_f * elements.e * elements.sqrt_a * sin_e;

    let dt_total = dt_poly + dt_rel - tgd_s;

    ClockOffset {
        dt_clock_poly_s: dt_poly,
        dt_rel_s: dt_rel,
        tgd_s,
        dt_clock_total_s: dt_total,
    }
}

/// A satellite's broadcast orbit and clock evaluated together at one instant.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct SatelliteState {
    /// The orbit evaluation (ECEF position and all intermediates).
    pub orbit: OrbitState,
    /// The clock offset evaluation.
    pub clock: ClockOffset,
}

/// Evaluate the broadcast orbit and clock at the same instant.
///
/// This is the intended public entry point: it solves the orbit and feeds that
/// solution's eccentric-anomaly sine into the clock's relativistic term, so a
/// caller cannot accidentally pair a clock evaluation with `sin(E)` from a
/// different epoch. [`satellite_position_ecef`] and [`satellite_clock_offset_s`]
/// remain available for component-level parity testing.
pub fn satellite_state(
    elements: &KeplerianElements,
    clock: &ClockPolynomial,
    consts: &ConstellationConstants,
    t_sow_s: f64,
    tgd_s: f64,
    is_geo: bool,
) -> SatelliteState {
    let orbit = satellite_position_ecef(elements, consts, t_sow_s, is_geo);
    let clock = satellite_clock_offset_s(clock, consts, elements, orbit.sin_e, t_sow_s, tgd_s);
    SatelliteState { orbit, clock }
}

#[cfg(test)]
mod tests;