sidereon-core 0.8.0

//! Static PPP measurement-model leaf: range-correction algebra, receiver and
//! satellite antenna PCO/PCV projection, troposphere slant and mapping,
//! satellite-clock and relativity corrections, elevation weighting, and the
//! local north/east/up frame geometry the antenna projection depends on.
//!
//! These are pure modeling helpers shared by the float and fixed solve clusters
//! in the parent module; they hold no solve state of their own.

use std::collections::BTreeMap;
use std::f64::consts::PI;

use crate::astro::frames::transforms::itrs_to_geodetic_compute;
use crate::astro::math::vec3::{add3, dot3, scale3, sub3, unit3};
use crate::astro::time::model::{Instant, JulianDateSplit, TimeModelError, TimeScale};

use crate::antenna;
use crate::constants::{C_M_S, DEG_TO_RAD, GPS_EPOCH_TO_J2000_S, KM_TO_M};
use crate::observables::PredictedObservables;
use crate::tropo::{tropo_mapping_unchecked, tropo_slant_unchecked, MappingModel};
use crate::{GnssSatelliteId, Wgs84Geodetic};

use super::{
    estimates_ztd, missing_correction, missing_satellite_clock, FloatEpoch, FloatObservation,
    FloatSolveError, FloatState, MeasurementWeights, MissingCorrection, PppCorrectionLookup,
    RangeCorrections, ReceiverAntennaFrequency, ReceiverAntennaOptions, SatelliteClockCorrections,
    TroposphereOptions,
};

const MIN_ELEVATION_WEIGHT_SCALE: f64 = 1.0e-3;

#[derive(Debug, Clone, Copy)]
pub(super) struct TropoModelState {
    slant_m: f64,
    pub(super) ztd_mapping: f64,
}

pub(super) fn model_troposphere(
    pred: &PredictedObservables,
    receiver_m: [f64; 3],
    epoch: &FloatEpoch,
    tropo: TroposphereOptions,
) -> Result<TropoModelState, FloatSolveError> {
    if !tropo.enabled {
        return Ok(TropoModelState {
            slant_m: 0.0,
            ztd_mapping: 0.0,
        });
    }

    let (lat_deg, lon_deg, height_km) = itrs_to_geodetic_compute(
        receiver_m[0] / KM_TO_M,
        receiver_m[1] / KM_TO_M,
        receiver_m[2] / KM_TO_M,
    )
    .expect("valid receiver ITRS coordinates");
    let receiver = Wgs84Geodetic::new(
        lat_deg.to_radians(),
        normalize_geodetic_lon_rad(lon_deg.to_radians()),
        height_km * KM_TO_M,
    )
    .expect("valid WGS84 geodetic position");
    let jd = JulianDateSplit::new(epoch.jd_whole, epoch.jd_fraction)
        .map_err(ppp_tropo_invalid_julian_split)?;
    let instant = Instant::from_julian_date(TimeScale::Gpst, jd);
    let elevation_rad = pred.elevation_deg.to_radians();
    let slant_m = tropo_slant_unchecked(elevation_rad, receiver, tropo.met, instant);
    let ztd_mapping = if estimates_ztd(tropo) {
        tropo_mapping_unchecked(MappingModel::Niell, elevation_rad, receiver, instant).wet
    } else {
        0.0
    };
    Ok(TropoModelState {
        slant_m,
        ztd_mapping,
    })
}

fn ppp_tropo_invalid_julian_split(error: TimeModelError) -> FloatSolveError {
    let TimeModelError::InvalidInput { field, reason } = error;
    let field = match field {
        "jd_whole" => "ppp epoch jd_whole",
        "fraction" => "ppp epoch jd_fraction",
        _ => "ppp epoch Julian date split",
    };
    FloatSolveError::InvalidInput { field, reason }
}

fn normalize_geodetic_lon_rad(lon_rad: f64) -> f64 {
    if lon_rad <= -PI {
        PI
    } else {
        lon_rad
    }
}

fn applied_troposphere_m(tropo_model: &TropoModelState, state: &FloatState) -> f64 {
    tropo_model.slant_m + state.ztd_m * tropo_model.ztd_mapping
}

pub(super) fn range_corrections_m(
    pred: &PredictedObservables,
    rx_pos: [f64; 3],
    epoch_idx: usize,
    obs: &FloatObservation,
    tropo_model: &TropoModelState,
    state: &FloatState,
    corrections: &RangeCorrections,
) -> Result<f64, FloatSolveError> {
    let receiver_antenna_m =
        receiver_antenna_correction_m(pred, rx_pos, obs, corrections.receiver_antenna.as_ref())?;
    let satellite_clock_m =
        satellite_clock_correction_m(pred, obs, corrections.satellite_clock.as_ref())?;
    let tide_m = solid_earth_tide_correction_m(pred, obs, epoch_idx, &corrections.ppp)?;
    let satellite_antenna_m =
        satellite_antenna_correction_m(pred, obs, epoch_idx, &corrections.ppp)?;
    Ok(applied_troposphere_m(tropo_model, state)
        + receiver_antenna_m
        + sat_clock_relativity_correction_m(pred, corrections.sat_clock_relativity)
        + satellite_clock_m
        + tide_m
        + satellite_antenna_m)
}

pub(super) fn satellite_clock_m(
    pred: &PredictedObservables,
    obs: &FloatObservation,
    clock: Option<&SatelliteClockCorrections>,
) -> Result<f64, FloatSolveError> {
    if let Some(sat_clock_s) = pred.sat_clock_s {
        return Ok(C_M_S * sat_clock_s);
    }
    let Some(clock) = clock else {
        return Err(missing_satellite_clock(obs));
    };
    clock
        .clock_s(obs.sat, pred.transmit_time_j2000_s)
        .map(|sat_clock_s| C_M_S * sat_clock_s)
        .ok_or_else(|| missing_satellite_clock(obs))
}

fn solid_earth_tide_correction_m(
    pred: &PredictedObservables,
    obs: &FloatObservation,
    epoch_idx: usize,
    corrections: &PppCorrectionLookup,
) -> Result<f64, FloatSolveError> {
    if !corrections.tide_enabled {
        return Ok(0.0);
    }
    corrections
        .tide
        .get(&epoch_idx)
        .map(|d| -dot3(*d, pred.los_unit))
        .ok_or_else(|| missing_correction(obs, MissingCorrection::SolidEarthTide))
}

fn satellite_antenna_correction_m(
    pred: &PredictedObservables,
    obs: &FloatObservation,
    epoch_idx: usize,
    corrections: &PppCorrectionLookup,
) -> Result<f64, FloatSolveError> {
    if !corrections.satellite_antenna_enabled {
        return Ok(0.0);
    }
    let key = (obs.sat, epoch_idx);
    let pco = corrections
        .sat_pco_ecef
        .get(&key)
        .ok_or_else(|| missing_correction(obs, MissingCorrection::SatelliteAntennaPco))?;
    let pcv_m = corrections
        .sat_pcv_m
        .get(&key)
        .copied()
        .ok_or_else(|| missing_correction(obs, MissingCorrection::SatelliteAntennaPcv))?;
    Ok(dot3(*pco, pred.los_unit) + pcv_m)
}

pub(super) fn phase_windup_m(
    obs: &FloatObservation,
    epoch_idx: usize,
    corrections: &RangeCorrections,
) -> Result<f64, FloatSolveError> {
    if !corrections.ppp.windup_enabled {
        return Ok(0.0);
    }
    corrections
        .ppp
        .windup_m
        .get(&(obs.sat, epoch_idx))
        .copied()
        .ok_or_else(|| missing_correction(obs, MissingCorrection::PhaseWindup))
}

fn satellite_clock_correction_m(
    pred: &PredictedObservables,
    obs: &FloatObservation,
    clock: Option<&SatelliteClockCorrections>,
) -> Result<f64, FloatSolveError> {
    let Some(clock) = clock else {
        return Ok(0.0);
    };
    match clock.clock_s(obs.sat, pred.transmit_time_j2000_s) {
        Some(rinex_clock_s) => Ok(pred
            .sat_clock_s
            .map(|sat_clock_s| C_M_S * (sat_clock_s - rinex_clock_s))
            .unwrap_or(0.0)),
        None => Err(missing_satellite_clock(obs)),
    }
}

impl SatelliteClockCorrections {
    fn clock_s(&self, sat: GnssSatelliteId, t_j2000_s: f64) -> Option<f64> {
        let gps_s = t_j2000_s + GPS_EPOCH_TO_J2000_S;
        let records = self.series.get(&sat)?;
        interpolate_clock(records, gps_s)
    }
}

fn interpolate_clock(records: &[(f64, f64)], t: f64) -> Option<f64> {
    let mut prev: Option<(f64, f64)> = None;
    for &(ti, bi) in records {
        if ti == t {
            return Some(bi);
        }
        if ti > t {
            let (t0, b0) = prev?;
            return Some(b0 + (bi - b0) * (t - t0) / (ti - t0));
        }
        prev = Some((ti, bi));
    }
    None
}

fn receiver_antenna_correction_m(
    pred: &PredictedObservables,
    rx_pos: [f64; 3],
    obs: &FloatObservation,
    receiver_antenna: Option<&ReceiverAntennaOptions>,
) -> Result<f64, FloatSolveError> {
    let Some(antenna) = receiver_antenna else {
        return Ok(0.0);
    };
    let c1 = single_freq_receiver_antenna_m(pred, rx_pos, obs, antenna, &antenna.freq1_label)?;
    let c2 = single_freq_receiver_antenna_m(pred, rx_pos, obs, antenna, &antenna.freq2_label)?;
    let gamma = antenna.freq1_hz * antenna.freq1_hz
        / (antenna.freq1_hz * antenna.freq1_hz - antenna.freq2_hz * antenna.freq2_hz);
    Ok(-(gamma * c1 - (gamma - 1.0) * c2))
}

fn single_freq_receiver_antenna_m(
    pred: &PredictedObservables,
    rx_pos: [f64; 3],
    obs: &FloatObservation,
    antenna: &ReceiverAntennaOptions,
    frequency: &str,
) -> Result<f64, FloatSolveError> {
    let Some(freq) = antenna.frequencies.iter().find(|f| f.label == frequency) else {
        return Err(missing_correction(
            obs,
            MissingCorrection::ReceiverAntennaFrequency(frequency.to_string()),
        ));
    };
    let Some(los) = unit3(sub3(pred.sat_pos_ecef_m, rx_pos)) else {
        return Err(missing_correction(
            obs,
            MissingCorrection::ReceiverAntennaGeometry,
        ));
    };
    let (north, east, up) = crate::estimation::substrate::frames::local_neu_basis(
        crate::estimation::recipe::FrameRecipe::GeodeticNeuCrossProduct,
        rx_pos,
    );
    let pco_projection = los_projection(freq.pco_m, north, east, up, los);
    let (zenith_deg, azimuth_deg) = los_zenith_azimuth_deg(los, up, north, east);
    let pcv_m = pcv(freq, zenith_deg, Some(azimuth_deg)).ok_or_else(|| {
        missing_correction(
            obs,
            MissingCorrection::ReceiverAntennaPcv(frequency.to_string()),
        )
    })?;
    Ok(pco_projection + pcv_m)
}

fn pcv(freq: &ReceiverAntennaFrequency, zenith_deg: f64, azimuth_deg: Option<f64>) -> Option<f64> {
    let noazi: Vec<(f64, f64)> = freq
        .pcv_samples
        .iter()
        .filter(|s| s.azimuth_deg.is_none())
        .map(|s| (s.zenith_deg, s.value_m))
        .collect();
    let has_azi = freq.pcv_samples.iter().any(|s| s.azimuth_deg.is_some());
    if azimuth_deg.is_none() || !has_azi {
        return interpolate_samples(noazi, zenith_deg);
    }

    let mut by_az: BTreeMap<i64, Vec<(f64, f64)>> = BTreeMap::new();
    for sample in freq.pcv_samples.iter().filter(|s| s.azimuth_deg.is_some()) {
        let az_key = (sample.azimuth_deg.unwrap() * 1.0e9).round() as i64;
        by_az
            .entry(az_key)
            .or_default()
            .push((sample.zenith_deg, sample.value_m));
    }
    if by_az.is_empty() {
        return interpolate_samples(noazi, zenith_deg);
    }
    interpolate_azimuth(by_az, azimuth_deg.unwrap(), zenith_deg)
}

fn interpolate_azimuth(
    azimuth_samples: BTreeMap<i64, Vec<(f64, f64)>>,
    azimuth_deg: f64,
    zenith_deg: f64,
) -> Option<f64> {
    let azimuths: Vec<f64> = azimuth_samples.keys().map(|k| *k as f64 / 1.0e9).collect();
    let azimuth = antenna::normalize_azimuth(azimuth_deg);
    let (low_deg, high_deg) = antenna::azimuth_bracket(&azimuths, azimuth);
    let low_samples = azimuth_samples
        .get(&((low_deg * 1.0e9).round() as i64))
        .cloned()
        .unwrap_or_default();
    let high_samples = azimuth_samples
        .get(&((high_deg * 1.0e9).round() as i64))
        .cloned()
        .unwrap_or_default();
    let low_value = interpolate_samples(low_samples, zenith_deg)?;
    let high_value = interpolate_samples(high_samples, zenith_deg)?;
    Some(antenna::blend_azimuth(
        low_deg, high_deg, azimuth, low_value, high_value,
    ))
}

fn interpolate_samples(mut samples: Vec<(f64, f64)>, zenith_deg: f64) -> Option<f64> {
    samples.sort_by(|a, b| a.0.total_cmp(&b.0));
    antenna::interpolate_zenith_sorted(&samples, zenith_deg)
}

fn sat_clock_relativity_correction_m(pred: &PredictedObservables, enabled: bool) -> f64 {
    if !enabled {
        return 0.0;
    }
    2.0 * dot3(pred.sat_pos_ecef_m, pred.sat_velocity_m_s) / C_M_S
}

pub(super) fn measurement_weight(
    weights: MeasurementWeights,
    code: bool,
    elevation_deg: f64,
) -> f64 {
    let base = if code { weights.code } else { weights.phase };
    if weights.elevation_weighting {
        base * elevation_weight_scale(elevation_deg)
    } else {
        base
    }
}

fn elevation_weight_scale(elevation_deg: f64) -> f64 {
    let sin_el = (elevation_deg * DEG_TO_RAD).sin();
    if !sin_el.is_finite() || sin_el < MIN_ELEVATION_WEIGHT_SCALE {
        MIN_ELEVATION_WEIGHT_SCALE
    } else {
        sin_el
    }
}

fn los_projection(
    neu_offset: [f64; 3],
    north: [f64; 3],
    east: [f64; 3],
    up: [f64; 3],
    los: [f64; 3],
) -> f64 {
    let pco_ecef = add3(
        add3(scale3(north, neu_offset[0]), scale3(east, neu_offset[1])),
        scale3(up, neu_offset[2]),
    );
    dot3(pco_ecef, los)
}

fn los_zenith_azimuth_deg(
    los: [f64; 3],
    up: [f64; 3],
    north: [f64; 3],
    east: [f64; 3],
) -> (f64, f64) {
    let elevation_sin = dot3(los, up).clamp(-1.0, 1.0);
    let zenith_deg = elevation_sin.acos() * 180.0 / std::f64::consts::PI;
    let mut azimuth_deg = dot3(los, east).atan2(dot3(los, north)) * 180.0 / std::f64::consts::PI;
    if azimuth_deg < 0.0 {
        azimuth_deg += 360.0;
    }
    (zenith_deg, azimuth_deg)
}