use std::collections::BTreeMap;
use crate::astro::constants::earth::OMEGA_E_DOT_RAD_S;
use crate::astro::constants::MU_EARTH;
use crate::constants::KM_TO_M;
use crate::id::GnssSatelliteId;
use crate::sp3::interp::{
instant_to_j2000_seconds, interpolate_precise_state, precise_node_j2000_seconds_from_instant,
NEVILLE_POINTS,
};
use crate::sp3::samples::PreciseEphemerisSample;
use crate::sp3::Sp3;
use crate::{Error, Result};
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct EpochWindow {
from_j2000_s: f64,
through_j2000_s: f64,
}
impl EpochWindow {
pub fn new(from_j2000_s: f64, through_j2000_s: f64) -> Result<Self> {
if !from_j2000_s.is_finite() || !through_j2000_s.is_finite() {
return Err(Error::InvalidInput(
"SP3 continuity window endpoints must be finite".to_string(),
));
}
if from_j2000_s > through_j2000_s {
return Err(Error::InvalidInput(
"SP3 continuity window start must not follow its end".to_string(),
));
}
Ok(Self {
from_j2000_s,
through_j2000_s,
})
}
pub fn from_j2000_s(self) -> f64 {
self.from_j2000_s
}
pub fn through_j2000_s(self) -> f64 {
self.through_j2000_s
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct StencilExtent {
grid_origin_j2000_s: f64,
interval_s: f64,
before_s: f64,
after_s: f64,
}
impl StencilExtent {
pub fn for_sp3(sp3: &Sp3) -> Result<Self> {
let interval_s = sp3.header.epoch_interval_s;
if !interval_s.is_finite() || interval_s <= 0.0 {
return Err(Error::InvalidInput(
"SP3 stencil extent requires a positive finite epoch interval".to_string(),
));
}
let grid_origin_j2000_s = sp3
.epochs_j2000_seconds()
.first()
.copied()
.filter(|epoch| epoch.is_finite())
.ok_or_else(|| {
Error::InvalidInput(
"SP3 stencil extent requires at least one representable epoch".to_string(),
)
})?;
let half_nodes = (NEVILLE_POINTS / 2) as f64;
let half_width_s = half_nodes * interval_s;
Ok(Self {
grid_origin_j2000_s,
interval_s,
before_s: half_width_s,
after_s: half_width_s,
})
}
pub fn before_s(self) -> f64 {
self.before_s
}
pub fn after_s(self) -> f64 {
self.after_s
}
fn influence_bounds(self, window: EpochWindow) -> (f64, f64) {
let pivot_at_or_before = |query: f64| {
self.grid_origin_j2000_s
+ ((query - self.grid_origin_j2000_s) / self.interval_s).floor() * self.interval_s
};
(
pivot_at_or_before(window.from_j2000_s) - self.before_s,
pivot_at_or_before(window.through_j2000_s) + self.after_s,
)
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum WindowContinuityDecision {
Accept,
Refuse,
}
#[derive(Debug, Clone, PartialEq)]
pub struct WindowContinuityVerdict<'a> {
pub decision: WindowContinuityDecision,
pub influencing_defects: Vec<&'a ContinuityDefect>,
pub influencing_splices: Vec<&'a super::combine::MergeContinuityViolation>,
pub all_defects: &'a [ContinuityDefect],
pub all_splices: Vec<&'a super::combine::MergeContinuityViolation>,
}
impl WindowContinuityVerdict<'_> {
pub fn accepted(&self) -> bool {
self.decision == WindowContinuityDecision::Accept
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum OrbitClass {
MeoGnss,
Geosynchronous,
Leo,
}
impl OrbitClass {
const fn min_semi_major_axis_m(self) -> f64 {
match self {
Self::MeoGnss => 25_510_000.0,
Self::Geosynchronous => 42_164_000.0,
Self::Leo => 6_678_000.0,
}
}
const fn max_radius_m(self) -> f64 {
match self {
Self::MeoGnss => 29_600_000.0,
Self::Geosynchronous => 42_164_000.0,
Self::Leo => 8_378_000.0,
}
}
pub fn max_earth_fixed_speed_m_s(self) -> f64 {
let mu_m3_s2 = MU_EARTH * KM_TO_M * KM_TO_M * KM_TO_M;
(mu_m3_s2 / self.min_semi_major_axis_m()).sqrt() + OMEGA_E_DOT_RAD_S * self.max_radius_m()
}
}
#[derive(Debug, Clone, PartialEq)]
pub struct ContinuityOptions {
pub speed_bound: Option<SpeedBound>,
pub residual_tolerance_m: Option<f64>,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum SpeedBound {
OrbitClass(OrbitClass),
ExplicitMaxSpeed(f64),
}
impl SpeedBound {
fn value_m_s(self) -> f64 {
match self {
Self::OrbitClass(class) => class.max_earth_fixed_speed_m_s(),
Self::ExplicitMaxSpeed(bound) => bound,
}
}
}
impl ContinuityOptions {
pub fn for_orbit_class(class: OrbitClass) -> Self {
Self {
speed_bound: Some(SpeedBound::OrbitClass(class)),
residual_tolerance_m: Some(1.0),
}
}
}
#[derive(Debug, Clone, PartialEq)]
pub enum ContinuityDefect {
DuplicateEpoch {
sat: GnssSatelliteId,
epoch_j2000_s: f64,
occurrences: usize,
},
SingleSampleSeries {
sat: GnssSatelliteId,
},
SpeedBound {
sat: GnssSatelliteId,
from_j2000_s: f64,
to_j2000_s: f64,
interval_s: f64,
displacement_m: f64,
implied_speed_m_s: f64,
bound_m_s: f64,
},
HoldOutResidual {
sat: GnssSatelliteId,
epoch_j2000_s: f64,
preceding_j2000_s: f64,
residual_m: f64,
tolerance_m: f64,
},
}
impl ContinuityDefect {
pub fn satellite(&self) -> GnssSatelliteId {
match self {
Self::DuplicateEpoch { sat, .. }
| Self::SingleSampleSeries { sat }
| Self::SpeedBound { sat, .. }
| Self::HoldOutResidual { sat, .. } => *sat,
}
}
pub(super) fn influences(&self, window: EpochWindow, stencil: StencilExtent) -> bool {
let (needed_from, needed_through) = stencil.influence_bounds(window);
let support = match self {
Self::DuplicateEpoch { epoch_j2000_s, .. } => Some((*epoch_j2000_s, *epoch_j2000_s)),
Self::SingleSampleSeries { .. } => None,
Self::SpeedBound {
from_j2000_s,
to_j2000_s,
..
} => Some((from_j2000_s.min(*to_j2000_s), from_j2000_s.max(*to_j2000_s))),
Self::HoldOutResidual {
epoch_j2000_s,
preceding_j2000_s,
..
} => Some((
epoch_j2000_s.min(*preceding_j2000_s),
epoch_j2000_s.max(*preceding_j2000_s),
)),
};
match support {
Some((from, through)) => from <= needed_through && through >= needed_from,
None => true,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ContinuityCheck {
Input,
SpeedBound,
HoldOutResidual,
}
#[derive(Debug, Clone, PartialEq, Default)]
pub struct ContinuityReport {
pub defects: Vec<ContinuityDefect>,
pub pairs_checked: usize,
pub residuals_checked: usize,
pub residuals_skipped: usize,
}
impl ContinuityReport {
pub fn attested(&self) -> bool {
self.defects.is_empty()
}
pub fn defects_from(&self, check: ContinuityCheck) -> impl Iterator<Item = &ContinuityDefect> {
self.defects.iter().filter(move |defect| {
let source = match defect {
ContinuityDefect::DuplicateEpoch { .. }
| ContinuityDefect::SingleSampleSeries { .. } => ContinuityCheck::Input,
ContinuityDefect::SpeedBound { .. } => ContinuityCheck::SpeedBound,
ContinuityDefect::HoldOutResidual { .. } => ContinuityCheck::HoldOutResidual,
};
source == check
})
}
pub fn defects_influencing(
&self,
window: EpochWindow,
stencil: StencilExtent,
) -> Vec<&ContinuityDefect> {
self.defects
.iter()
.filter(|defect| defect.influences(window, stencil))
.collect()
}
pub fn verdict_for_window(
&self,
window: EpochWindow,
stencil: StencilExtent,
) -> WindowContinuityVerdict<'_> {
let influencing_defects = self.defects_influencing(window, stencil);
let decision = if influencing_defects.is_empty() {
WindowContinuityDecision::Accept
} else {
WindowContinuityDecision::Refuse
};
WindowContinuityVerdict {
decision,
influencing_defects,
influencing_splices: Vec::new(),
all_defects: &self.defects,
all_splices: Vec::new(),
}
}
}
struct OrderedSeries {
x: Vec<f64>,
kx: Vec<f64>,
ky: Vec<f64>,
kz: Vec<f64>,
pos_m: Vec<[f64; 3]>,
}
impl OrderedSeries {
fn build(
sat: GnssSatelliteId,
samples: &[&PreciseEphemerisSample],
defects: &mut Vec<ContinuityDefect>,
) -> Option<Self> {
let mut placed: Vec<(f64, [f64; 3])> = Vec::with_capacity(samples.len());
for sample in samples {
let Some(seconds) = instant_to_j2000_seconds(&sample.epoch) else {
continue;
};
if !seconds.is_finite() || !sample.position_ecef_m.iter().all(|c| c.is_finite()) {
continue;
}
let Some(node) = precise_node_j2000_seconds_from_instant(&sample.epoch) else {
continue;
};
placed.push((node, sample.position_ecef_m));
}
placed.sort_by(|a, b| a.0.total_cmp(&b.0));
let mut series = Self {
x: Vec::with_capacity(placed.len()),
kx: Vec::with_capacity(placed.len()),
ky: Vec::with_capacity(placed.len()),
kz: Vec::with_capacity(placed.len()),
pos_m: Vec::with_capacity(placed.len()),
};
let mut index = 0usize;
while index < placed.len() {
let epoch = placed[index].0;
let mut run = 1usize;
while index + run < placed.len() && placed[index + run].0 == epoch {
run += 1;
}
if run > 1 {
defects.push(ContinuityDefect::DuplicateEpoch {
sat,
epoch_j2000_s: epoch,
occurrences: run,
});
} else {
let (node, pos_m) = placed[index];
series.x.push(node);
series.kx.push(pos_m[0] / KM_TO_M);
series.ky.push(pos_m[1] / KM_TO_M);
series.kz.push(pos_m[2] / KM_TO_M);
series.pos_m.push(pos_m);
}
index += run;
}
if series.x.is_empty() {
return None;
}
Some(series)
}
fn len(&self) -> usize {
self.x.len()
}
fn pairs(&self) -> impl Iterator<Item = (usize, usize)> + '_ {
(1..self.x.len()).map(|index| (index - 1, index))
}
}
pub fn check_continuity(
samples: &[PreciseEphemerisSample],
options: &ContinuityOptions,
) -> ContinuityReport {
let mut by_sat: BTreeMap<GnssSatelliteId, Vec<&PreciseEphemerisSample>> = BTreeMap::new();
for sample in samples {
by_sat.entry(sample.sat).or_default().push(sample);
}
let mut report = ContinuityReport::default();
for (sat, sat_samples) in by_sat {
let mut sat_defects = Vec::new();
let Some(series) = OrderedSeries::build(sat, &sat_samples, &mut sat_defects) else {
report.defects.append(&mut sat_defects);
continue;
};
if series.len() < 2 {
sat_defects.push(ContinuityDefect::SingleSampleSeries { sat });
report.defects.append(&mut sat_defects);
continue;
}
if let Some(bound) = options.speed_bound {
check_speed_bound(sat, &series, bound, &mut sat_defects, &mut report);
}
if let Some(tolerance_m) = options.residual_tolerance_m {
check_hold_out_residual(sat, &series, tolerance_m, &mut sat_defects, &mut report);
}
sat_defects.sort_by(|a, b| defect_sort_key(a).total_cmp(&defect_sort_key(b)));
report.defects.append(&mut sat_defects);
}
report
}
fn check_speed_bound(
sat: GnssSatelliteId,
series: &OrderedSeries,
bound: SpeedBound,
defects: &mut Vec<ContinuityDefect>,
report: &mut ContinuityReport,
) {
let bound_m_s = bound.value_m_s();
for (lo, hi) in series.pairs() {
let interval_s = series.x[hi] - series.x[lo];
let displacement_m = distance_m(series.pos_m[lo], series.pos_m[hi]);
report.pairs_checked += 1;
let implied_speed_m_s = displacement_m / interval_s;
if implied_speed_m_s > bound_m_s {
defects.push(ContinuityDefect::SpeedBound {
sat,
from_j2000_s: series.x[lo],
to_j2000_s: series.x[hi],
interval_s,
displacement_m,
implied_speed_m_s,
bound_m_s,
});
}
}
}
fn check_hold_out_residual(
sat: GnssSatelliteId,
series: &OrderedSeries,
tolerance_m: f64,
defects: &mut Vec<ContinuityDefect>,
report: &mut ContinuityReport,
) {
let interior = series.len().saturating_sub(2);
if interior == 0 {
report.residuals_skipped += series.len();
return;
}
for parity in [0usize, 1usize] {
let keep: Vec<usize> = (0..series.len())
.filter(|index| index % 2 == parity)
.collect();
let held: Vec<usize> = (1..series.len() - 1)
.filter(|index| index % 2 != parity)
.collect();
if held.is_empty() {
continue;
}
if keep.len() < 2 {
report.residuals_skipped += held.len();
continue;
}
let x: Vec<f64> = keep.iter().map(|&i| series.x[i]).collect();
let kx: Vec<f64> = keep.iter().map(|&i| series.kx[i]).collect();
let ky: Vec<f64> = keep.iter().map(|&i| series.ky[i]).collect();
let kz: Vec<f64> = keep.iter().map(|&i| series.kz[i]).collect();
for index in held {
let query = series.x[index];
match interpolate_precise_state(sat, &x, &kx, &ky, &kz, &[], query) {
Ok(state) => {
report.residuals_checked += 1;
let predicted = [state.position.x_m, state.position.y_m, state.position.z_m];
let residual_m = distance_m(predicted, series.pos_m[index]);
if residual_m > tolerance_m {
defects.push(ContinuityDefect::HoldOutResidual {
sat,
epoch_j2000_s: query,
preceding_j2000_s: series.x[index - 1],
residual_m,
tolerance_m,
});
}
}
Err(_) => {
report.residuals_skipped += 1;
}
}
}
}
}
fn defect_sort_key(defect: &ContinuityDefect) -> f64 {
match defect {
ContinuityDefect::DuplicateEpoch { epoch_j2000_s, .. } => *epoch_j2000_s,
ContinuityDefect::SingleSampleSeries { .. } => f64::NEG_INFINITY,
ContinuityDefect::SpeedBound { from_j2000_s, .. } => *from_j2000_s,
ContinuityDefect::HoldOutResidual { epoch_j2000_s, .. } => *epoch_j2000_s,
}
}
fn distance_m(a: [f64; 3], b: [f64; 3]) -> f64 {
let dx = b[0] - a[0];
let dy = b[1] - a[1];
let dz = b[2] - a[2];
(dx * dx + dy * dy + dz * dz).sqrt()
}