1use crate::artifact_bytes::{ArtifactBytes, DigestProvenance};
10use std::collections::BTreeMap;
11use std::fs;
12use std::mem;
13use std::path::{Path, PathBuf};
14
15use crate::astro::time::model::{Instant, TimeScale};
16use crate::constants::{KM_TO_M, OMEGA_E_DOT_RAD_S, US_TO_S};
17use crate::frame::ItrfPositionM;
18use crate::id::{GnssSatelliteId, GnssSystem};
19use crate::observables::{
20 ObservableEphemerisSource, ObservableState, ObservableStateBatch, ObservablesError,
21};
22use crate::sp3::interp::{instant_to_j2000_seconds, neville, NEVILLE_POINTS};
23use crate::sp3::{PreciseEphemerisInterpolant, Sp3, Sp3State};
24use crate::{validate, Error, Result};
25
26const STORE_MAGIC: &[u8; 8] = b"PEMAP001";
27const STORE_VERSION: u16 = 1;
28const STORE_ALIGNMENT: usize = 4096;
29const STORE_HEADER_LEN: usize = 64;
30const SAT_INDEX_RECORD_LEN: usize = 96;
31const CLOCK_NODE_RECORD_LEN: usize = 24;
32const CLOCK_ARC_RECORD_LEN: usize = 64;
33
34const HEADER_VERSION_OFFSET: usize = 8;
35const HEADER_TIME_SCALE_OFFSET: usize = 10;
36const HEADER_SAT_COUNT_OFFSET: usize = 12;
37const HEADER_INDEX_OFFSET_OFFSET: usize = 16;
38const HEADER_DATA_OFFSET_OFFSET: usize = 24;
39const HEADER_TOTAL_LEN_OFFSET: usize = 32;
40const HEADER_CHECKSUM_OFFSET: usize = 40;
41
42const SAT_SYSTEM_OFFSET: usize = 0;
43const SAT_PRN_OFFSET: usize = 1;
44const SAT_POS_COUNT_OFFSET: usize = 4;
45const SAT_CLOCK_NODE_COUNT_OFFSET: usize = 8;
46const SAT_CLOCK_ARC_COUNT_OFFSET: usize = 12;
47const SAT_POS_X_OFFSET_OFFSET: usize = 16;
48const SAT_POS_KX_OFFSET_OFFSET: usize = 24;
49const SAT_POS_KY_OFFSET_OFFSET: usize = 32;
50const SAT_POS_KZ_OFFSET_OFFSET: usize = 40;
51const SAT_CLOCK_NODE_OFFSET_OFFSET: usize = 48;
52const SAT_CLOCK_ARC_OFFSET_OFFSET: usize = 56;
53const SAT_DATA_OFFSET_OFFSET: usize = 64;
54const SAT_DATA_LEN_OFFSET: usize = 72;
55const SAT_CHECKSUM_OFFSET: usize = 80;
56
57const CLOCK_NODE_X_OFFSET: usize = 0;
58const CLOCK_NODE_US_OFFSET: usize = 8;
59const CLOCK_NODE_EVENT_OFFSET: usize = 16;
60
61const CLOCK_ARC_NODE_COUNT_OFFSET: usize = 0;
62const CLOCK_ARC_COEFF_COUNT_OFFSET: usize = 4;
63const CLOCK_ARC_X_OFFSET_OFFSET: usize = 8;
64const CLOCK_ARC_C0_OFFSET_OFFSET: usize = 16;
65const CLOCK_ARC_C1_OFFSET_OFFSET: usize = 24;
66const CLOCK_ARC_C2_OFFSET_OFFSET: usize = 32;
67const CLOCK_ARC_C3_OFFSET_OFFSET: usize = 40;
68
69const FNV_OFFSET_BASIS: u64 = 0xcbf2_9ce4_8422_2325;
70const FNV_PRIME: u64 = 0x0000_0100_0000_01b3;
71
72#[derive(Debug, Clone, PartialEq, Eq)]
74pub enum PreciseInterpolantStoreError {
75 Io {
77 path: PathBuf,
79 message: String,
81 },
82 Parse {
84 reason: String,
86 },
87 UnsupportedVersion {
89 version: u16,
91 },
92 UnsupportedTimeScale {
94 tag: u8,
96 },
97 UnsupportedSatelliteSystem {
99 tag: u8,
101 },
102 DuplicateSatellite {
104 sat: GnssSatelliteId,
106 },
107 Checksum {
109 expected: u64,
111 found: u64,
113 },
114 SatelliteChecksum {
116 sat: GnssSatelliteId,
118 expected: u64,
120 found: u64,
122 },
123 AttestedChecksumMismatch {
125 claimed: u64,
127 declared: u64,
129 },
130}
131
132impl core::fmt::Display for PreciseInterpolantStoreError {
133 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
134 match self {
135 Self::Io { path, message } => write!(f, "{} failed: {message}", path.display()),
136 Self::Parse { reason } => write!(f, "precise interpolant store parse error: {reason}"),
137 Self::UnsupportedVersion { version } => {
138 write!(
139 f,
140 "precise interpolant store version {version} is not supported"
141 )
142 }
143 Self::UnsupportedTimeScale { tag } => {
144 write!(
145 f,
146 "precise interpolant store time-scale tag {tag} is not supported"
147 )
148 }
149 Self::UnsupportedSatelliteSystem { tag } => {
150 write!(
151 f,
152 "precise interpolant store satellite-system tag {tag} is not supported"
153 )
154 }
155 Self::DuplicateSatellite { sat } => {
156 write!(f, "duplicate precise interpolant satellite {sat}")
157 }
158 Self::Checksum { expected, found } => write!(
159 f,
160 "precise interpolant store checksum expected {expected:#x} but found {found:#x}"
161 ),
162 Self::SatelliteChecksum {
163 sat,
164 expected,
165 found,
166 } => write!(
167 f,
168 "precise interpolant satellite {sat} checksum expected {expected:#x} but found {found:#x}"
169 ),
170 Self::AttestedChecksumMismatch { claimed, declared } => write!(
171 f,
172 "attested precise interpolant checksum {claimed:#x} does not match header checksum {declared:#x}"
173 ),
174 }
175 }
176}
177
178impl std::error::Error for PreciseInterpolantStoreError {}
179
180#[derive(Debug, Clone)]
181enum F64Array<'a> {
182 Borrowed(&'a [f64]),
183 Offset { offset: usize, count: usize },
184}
185
186impl F64Array<'_> {
187 #[cfg(feature = "mmap")]
188 fn into_static(self) -> Option<F64Array<'static>> {
195 match self {
196 Self::Borrowed(_) => None,
197 Self::Offset { offset, count } => Some(F64Array::Offset { offset, count }),
198 }
199 }
200
201 const fn len(&self) -> usize {
202 match self {
203 Self::Borrowed(values) => values.len(),
204 Self::Offset { count, .. } => *count,
205 }
206 }
207
208 fn get(&self, bytes: &[u8], idx: usize) -> f64 {
209 match self {
210 Self::Borrowed(values) => values[idx],
211 Self::Offset { offset, .. } => mapped_f64(bytes, *offset, idx),
212 }
213 }
214}
215
216#[derive(Debug, Clone)]
217struct MmapClockArc<'a> {
218 x: F64Array<'a>,
219 c0: F64Array<'a>,
220 c1: F64Array<'a>,
221 c2: F64Array<'a>,
222 c3: F64Array<'a>,
223}
224
225impl MmapClockArc<'_> {
226 #[cfg(feature = "mmap")]
227 fn into_static(self) -> Option<MmapClockArc<'static>> {
228 Some(MmapClockArc {
229 x: self.x.into_static()?,
230 c0: self.c0.into_static()?,
231 c1: self.c1.into_static()?,
232 c2: self.c2.into_static()?,
233 c3: self.c3.into_static()?,
234 })
235 }
236
237 fn node_count(&self) -> usize {
238 self.x.len()
239 }
240
241 fn coeff_count(&self) -> usize {
242 self.c0.len()
243 }
244}
245
246#[derive(Debug, Clone)]
247struct MmapSeries<'a> {
248 pos_count: usize,
249 clock_node_count: usize,
250 pos_x: F64Array<'a>,
251 pos_kx: F64Array<'a>,
252 pos_ky: F64Array<'a>,
253 pos_kz: F64Array<'a>,
254 clock_arcs: Vec<MmapClockArc<'a>>,
255}
256
257impl MmapSeries<'_> {
258 #[cfg(feature = "mmap")]
259 fn into_static(self) -> Option<MmapSeries<'static>> {
260 Some(MmapSeries {
261 pos_count: self.pos_count,
262 clock_node_count: self.clock_node_count,
263 pos_x: self.pos_x.into_static()?,
264 pos_kx: self.pos_kx.into_static()?,
265 pos_ky: self.pos_ky.into_static()?,
266 pos_kz: self.pos_kz.into_static()?,
267 clock_arcs: self
268 .clock_arcs
269 .into_iter()
270 .map(MmapClockArc::into_static)
271 .collect::<Option<Vec<_>>>()?,
272 })
273 }
274}
275
276#[derive(Debug)]
277struct ParsedStore<'a> {
278 time_scale: TimeScale,
279 satellites: Vec<GnssSatelliteId>,
280 series: BTreeMap<GnssSatelliteId, MmapSeries<'a>>,
281}
282
283#[derive(Clone, Copy)]
284enum ArrayBacking<'a> {
285 Borrowed(&'a [u8]),
286 Offset,
287}
288
289#[derive(Clone, Copy)]
290enum ChecksumValidation {
291 Verified,
292 Attested(u64),
293}
294
295impl ChecksumValidation {
296 const fn digest_provenance(self) -> DigestProvenance {
297 match self {
298 Self::Verified => DigestProvenance::Verified,
299 Self::Attested(_) => DigestProvenance::Attested,
300 }
301 }
302
303 const fn attested_checksum64(self) -> Option<u64> {
304 match self {
305 Self::Verified => None,
306 Self::Attested(checksum64) => Some(checksum64),
307 }
308 }
309
310 const fn verifies_payloads(self) -> bool {
311 matches!(self, Self::Verified)
312 }
313}
314
315pub struct MmapPreciseEphemerisInterpolant<'a> {
322 bytes: ArtifactBytes<'a>,
323 time_scale: TimeScale,
324 satellites: Vec<GnssSatelliteId>,
325 series: BTreeMap<GnssSatelliteId, MmapSeries<'a>>,
326 digest_provenance: DigestProvenance,
327 attested_checksum64: Option<u64>,
328}
329
330impl core::fmt::Debug for MmapPreciseEphemerisInterpolant<'_> {
331 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
332 f.debug_struct("MmapPreciseEphemerisInterpolant")
333 .field("byte_len", &self.bytes.as_slice().len())
334 .field("time_scale", &self.time_scale)
335 .field("satellites", &self.satellites)
336 .field("digest_provenance", &self.digest_provenance)
337 .field("attested_checksum64", &self.attested_checksum64)
338 .finish_non_exhaustive()
339 }
340}
341
342impl MmapPreciseEphemerisInterpolant<'static> {
343 pub fn from_vec(bytes: Vec<u8>) -> core::result::Result<Self, PreciseInterpolantStoreError> {
345 Self::from_backing(
346 ArtifactBytes::Owned(bytes),
347 ArrayBacking::Offset,
348 ChecksumValidation::Verified,
349 )
350 }
351
352 pub fn from_vec_attested(
360 bytes: Vec<u8>,
361 claimed_checksum64: u64,
362 ) -> core::result::Result<Self, PreciseInterpolantStoreError> {
363 Self::from_backing(
364 ArtifactBytes::Owned(bytes),
365 ArrayBacking::Offset,
366 ChecksumValidation::Attested(claimed_checksum64),
367 )
368 }
369
370 pub fn from_path(
380 path: impl AsRef<Path>,
381 ) -> core::result::Result<Self, PreciseInterpolantStoreError> {
382 let path = path.as_ref();
383
384 #[cfg(feature = "mmap")]
385 {
386 let bytes = crate::artifact_bytes::map_file_read_only(path).map_err(|err| {
387 PreciseInterpolantStoreError::Io {
388 path: path.to_path_buf(),
389 message: err.to_string(),
390 }
391 })?;
392 Self::from_mapped_backing(bytes, ChecksumValidation::Verified)
393 }
394
395 #[cfg(not(feature = "mmap"))]
396 {
397 let bytes = fs::read(path).map_err(|err| PreciseInterpolantStoreError::Io {
398 path: path.to_path_buf(),
399 message: err.to_string(),
400 })?;
401 Self::from_vec(bytes)
402 }
403 }
404
405 pub fn from_path_attested(
414 path: impl AsRef<Path>,
415 claimed_checksum64: u64,
416 ) -> core::result::Result<Self, PreciseInterpolantStoreError> {
417 let path = path.as_ref();
418
419 #[cfg(feature = "mmap")]
420 {
421 let bytes = crate::artifact_bytes::map_file_read_only(path).map_err(|err| {
422 PreciseInterpolantStoreError::Io {
423 path: path.to_path_buf(),
424 message: err.to_string(),
425 }
426 })?;
427 Self::from_mapped_backing(bytes, ChecksumValidation::Attested(claimed_checksum64))
428 }
429
430 #[cfg(not(feature = "mmap"))]
431 {
432 let bytes = fs::read(path).map_err(|err| PreciseInterpolantStoreError::Io {
433 path: path.to_path_buf(),
434 message: err.to_string(),
435 })?;
436 Self::from_backing(
437 ArtifactBytes::Owned(bytes),
438 ArrayBacking::Offset,
439 ChecksumValidation::Attested(claimed_checksum64),
440 )
441 }
442 }
443
444 #[cfg(feature = "mmap")]
445 fn from_mapped_backing(
446 bytes: ArtifactBytes<'static>,
447 checksum_validation: ChecksumValidation,
448 ) -> core::result::Result<Self, PreciseInterpolantStoreError> {
449 let parsed = parse_store(bytes.as_slice(), ArrayBacking::Offset, checksum_validation)?;
450 let series = parsed
451 .series
452 .into_iter()
453 .map(|(sat, series)| series.into_static().map(|series| (sat, series)))
454 .collect::<Option<BTreeMap<_, _>>>()
455 .expect("offset-backed parse yields no borrows");
456 Ok(Self {
457 bytes,
458 time_scale: parsed.time_scale,
459 satellites: parsed.satellites,
460 series,
461 digest_provenance: checksum_validation.digest_provenance(),
462 attested_checksum64: checksum_validation.attested_checksum64(),
463 })
464 }
465}
466
467impl<'a> MmapPreciseEphemerisInterpolant<'a> {
468 pub fn from_bytes(bytes: &'a [u8]) -> core::result::Result<Self, PreciseInterpolantStoreError> {
474 Self::from_backing(
475 ArtifactBytes::Borrowed(bytes),
476 ArrayBacking::Borrowed(bytes),
477 ChecksumValidation::Verified,
478 )
479 }
480
481 fn from_backing(
482 bytes: ArtifactBytes<'a>,
483 backing: ArrayBacking<'a>,
484 checksum_validation: ChecksumValidation,
485 ) -> core::result::Result<Self, PreciseInterpolantStoreError> {
486 let parsed = parse_store(bytes.as_slice(), backing, checksum_validation)?;
487 Ok(Self {
488 bytes,
489 time_scale: parsed.time_scale,
490 satellites: parsed.satellites,
491 series: parsed.series,
492 digest_provenance: checksum_validation.digest_provenance(),
493 attested_checksum64: checksum_validation.attested_checksum64(),
494 })
495 }
496
497 #[must_use]
499 pub fn as_bytes(&self) -> &[u8] {
500 self.bytes.as_slice()
501 }
502
503 #[must_use]
506 pub fn is_memory_mapped(&self) -> bool {
507 self.bytes.is_memory_mapped()
508 }
509
510 #[must_use]
512 pub const fn digest_provenance(&self) -> DigestProvenance {
513 self.digest_provenance
514 }
515
516 #[must_use]
521 pub fn checksum64(&self) -> u64 {
522 match self.digest_provenance {
523 DigestProvenance::Verified => precise_interpolant_store_checksum64(self.bytes.as_ref()),
524 DigestProvenance::Attested => self
525 .attested_checksum64
526 .expect("attested precise interpolant reader carries a checksum"),
527 }
528 }
529
530 pub fn verify(&mut self) -> core::result::Result<(), PreciseInterpolantStoreError> {
535 parse_store(
536 self.bytes.as_ref(),
537 ArrayBacking::Offset,
538 ChecksumValidation::Verified,
539 )?;
540 self.digest_provenance = DigestProvenance::Verified;
541 self.attested_checksum64 = None;
542 Ok(())
543 }
544
545 #[must_use]
547 pub const fn time_scale(&self) -> TimeScale {
548 self.time_scale
549 }
550
551 #[must_use]
553 pub fn satellites(&self) -> &[GnssSatelliteId] {
554 &self.satellites
555 }
556
557 pub fn position_at_j2000_seconds(&self, sat: GnssSatelliteId, query: f64) -> Result<Sp3State> {
559 let query = validate::finite(query, "query_j2000_s").map_err(map_query_input)?;
560 let Some(series) = self.series.get(&sat) else {
561 return Err(Error::UnknownSatellite(sat));
562 };
563 interpolate_mapped_state(self.bytes.as_ref(), series, query)
564 }
565
566 pub fn position(&self, sat: GnssSatelliteId, epoch: Instant) -> Result<Sp3State> {
570 if epoch.scale != self.time_scale {
571 return Err(Error::InvalidInput(format!(
572 "mapped precise-interpolant query time scale {} does not match source time scale {}",
573 epoch.scale.abbrev(),
574 self.time_scale.abbrev()
575 )));
576 }
577 let query = instant_to_j2000_seconds(&epoch).ok_or(Error::EpochOutOfRange)?;
578 self.position_at_j2000_seconds(sat, query)
579 }
580
581 pub fn observable_states_at_j2000_s(
583 &self,
584 satellites: &[GnssSatelliteId],
585 epochs_j2000_s: &[f64],
586 ) -> core::result::Result<ObservableStateBatch, ObservablesError> {
587 <Self as ObservableEphemerisSource>::observable_states_at_j2000_s(
588 self,
589 satellites,
590 epochs_j2000_s,
591 )
592 }
593
594 pub fn observable_states_at_shared_j2000_s(
596 &self,
597 satellites: &[GnssSatelliteId],
598 epoch_j2000_s: f64,
599 ) -> ObservableStateBatch {
600 <Self as ObservableEphemerisSource>::observable_states_at_shared_j2000_s(
601 self,
602 satellites,
603 epoch_j2000_s,
604 )
605 }
606}
607
608impl ObservableEphemerisSource for MmapPreciseEphemerisInterpolant<'_> {
609 fn observable_state_at_j2000_s(
610 &self,
611 sat: GnssSatelliteId,
612 t_j2000_s: f64,
613 ) -> core::result::Result<ObservableState, ObservablesError> {
614 let state = self
615 .position_at_j2000_seconds(sat, t_j2000_s)
616 .map_err(ObservablesError::Ephemeris)?;
617 Ok(ObservableState {
618 position_ecef_m: state.position.as_array(),
619 clock_s: state.clock_s,
620 })
621 }
622}
623
624impl PreciseEphemerisInterpolant {
625 pub fn to_mmap_store_bytes(
631 &self,
632 ) -> core::result::Result<Vec<u8>, PreciseInterpolantStoreError> {
633 build_store(self)
634 }
635
636 pub fn write_mmap_store(
638 &self,
639 output_path: impl AsRef<Path>,
640 ) -> core::result::Result<(), PreciseInterpolantStoreError> {
641 let bytes = self.to_mmap_store_bytes()?;
642 let output_path = output_path.as_ref();
643 fs::write(output_path, &bytes).map_err(|err| PreciseInterpolantStoreError::Io {
644 path: output_path.to_path_buf(),
645 message: err.to_string(),
646 })
647 }
648}
649
650impl Sp3 {
651 pub fn precise_interpolant_store_bytes(
653 &self,
654 ) -> core::result::Result<Vec<u8>, PreciseInterpolantStoreError> {
655 PreciseEphemerisInterpolant::from_sp3(self).to_mmap_store_bytes()
656 }
657
658 pub fn write_precise_interpolant_store(
661 &self,
662 output_path: impl AsRef<Path>,
663 ) -> core::result::Result<(), PreciseInterpolantStoreError> {
664 PreciseEphemerisInterpolant::from_sp3(self).write_mmap_store(output_path)
665 }
666}
667
668#[must_use]
673pub fn precise_interpolant_store_checksum64(bytes: &[u8]) -> u64 {
674 artifact_checksum64(bytes)
675}
676
677fn build_store(
678 source: &PreciseEphemerisInterpolant,
679) -> core::result::Result<Vec<u8>, PreciseInterpolantStoreError> {
680 let sat_count = source.node_series().len();
681 let index_end = STORE_HEADER_LEN
682 .checked_add(
683 sat_count
684 .checked_mul(SAT_INDEX_RECORD_LEN)
685 .ok_or_else(|| parse_error("satellite index length overflows usize"))?,
686 )
687 .ok_or_else(|| parse_error("satellite index end overflows usize"))?;
688 let data_offset = align_up(index_end, STORE_ALIGNMENT)?;
689
690 let mut layouts = Vec::with_capacity(sat_count);
691 let mut cursor = data_offset;
692 for (&sat, fitted) in source.node_series() {
693 cursor = align_up(cursor, STORE_ALIGNMENT)?;
694 let data_offset = cursor;
695 let series = &fitted.series;
696 let pos_count = series.x.len();
697 let clock_node_count = series.clk.len();
698 let clock_arc_count = fitted.clock_arcs.len();
699
700 let pos_x_offset = cursor;
701 cursor = add_len(cursor, pos_count, 8)?;
702 let pos_kx_offset = cursor;
703 cursor = add_len(cursor, pos_count, 8)?;
704 let pos_ky_offset = cursor;
705 cursor = add_len(cursor, pos_count, 8)?;
706 let pos_kz_offset = cursor;
707 cursor = add_len(cursor, pos_count, 8)?;
708 let clock_node_offset = cursor;
709 cursor = add_len(cursor, clock_node_count, CLOCK_NODE_RECORD_LEN)?;
710 let clock_arc_offset = cursor;
711 cursor = add_len(cursor, clock_arc_count, CLOCK_ARC_RECORD_LEN)?;
712
713 let mut arcs = Vec::with_capacity(clock_arc_count);
714 for arc in &fitted.clock_arcs {
715 let node_count = arc.x.len();
716 let coeff_count = arc.c0.len();
717 if arc.c1.len() != coeff_count
718 || arc.c2.len() != coeff_count
719 || arc.c3.len() != coeff_count
720 || coeff_count != node_count.saturating_sub(1)
721 {
722 return Err(parse_error("clock arc coefficient shape is inconsistent"));
723 }
724
725 let x_offset = cursor;
726 cursor = add_len(cursor, node_count, 8)?;
727 let c0_offset = cursor;
728 cursor = add_len(cursor, coeff_count, 8)?;
729 let c1_offset = cursor;
730 cursor = add_len(cursor, coeff_count, 8)?;
731 let c2_offset = cursor;
732 cursor = add_len(cursor, coeff_count, 8)?;
733 let c3_offset = cursor;
734 cursor = add_len(cursor, coeff_count, 8)?;
735 arcs.push(PendingClockArcLayout {
736 node_count,
737 coeff_count,
738 x_offset,
739 c0_offset,
740 c1_offset,
741 c2_offset,
742 c3_offset,
743 });
744 }
745
746 layouts.push(PendingSatLayout {
747 sat,
748 data_offset,
749 data_len: cursor - data_offset,
750 pos_x_offset,
751 pos_kx_offset,
752 pos_ky_offset,
753 pos_kz_offset,
754 clock_node_offset,
755 clock_arc_offset,
756 arcs,
757 });
758 }
759
760 let mut out = vec![0u8; cursor];
761 out[..STORE_MAGIC.len()].copy_from_slice(STORE_MAGIC);
762 write_u16(&mut out, HEADER_VERSION_OFFSET, STORE_VERSION);
763 out[HEADER_TIME_SCALE_OFFSET] = time_scale_tag(source.time_scale());
764 write_u32(
765 &mut out,
766 HEADER_SAT_COUNT_OFFSET,
767 u32::try_from(sat_count).map_err(|_| parse_error("satellite count exceeds u32"))?,
768 );
769 write_u64(
770 &mut out,
771 HEADER_INDEX_OFFSET_OFFSET,
772 STORE_HEADER_LEN as u64,
773 );
774 write_u64(&mut out, HEADER_DATA_OFFSET_OFFSET, data_offset as u64);
775 write_u64(&mut out, HEADER_TOTAL_LEN_OFFSET, cursor as u64);
776
777 for (idx, layout) in layouts.iter().enumerate() {
778 let fitted = source
779 .node_series()
780 .get(&layout.sat)
781 .expect("layout satellite came from source");
782 let series = &fitted.series;
783 let record_offset = STORE_HEADER_LEN + idx * SAT_INDEX_RECORD_LEN;
784 let record = &mut out[record_offset..record_offset + SAT_INDEX_RECORD_LEN];
785 record[SAT_SYSTEM_OFFSET] = layout.sat.system.letter() as u8;
786 record[SAT_PRN_OFFSET] = layout.sat.prn;
787 write_u32(
788 record,
789 SAT_POS_COUNT_OFFSET,
790 u32::try_from(series.x.len()).map_err(|_| parse_error("position count exceeds u32"))?,
791 );
792 write_u32(
793 record,
794 SAT_CLOCK_NODE_COUNT_OFFSET,
795 u32::try_from(series.clk.len())
796 .map_err(|_| parse_error("clock node count exceeds u32"))?,
797 );
798 write_u32(
799 record,
800 SAT_CLOCK_ARC_COUNT_OFFSET,
801 u32::try_from(fitted.clock_arcs.len())
802 .map_err(|_| parse_error("clock arc count exceeds u32"))?,
803 );
804 write_u64(record, SAT_POS_X_OFFSET_OFFSET, layout.pos_x_offset as u64);
805 write_u64(
806 record,
807 SAT_POS_KX_OFFSET_OFFSET,
808 layout.pos_kx_offset as u64,
809 );
810 write_u64(
811 record,
812 SAT_POS_KY_OFFSET_OFFSET,
813 layout.pos_ky_offset as u64,
814 );
815 write_u64(
816 record,
817 SAT_POS_KZ_OFFSET_OFFSET,
818 layout.pos_kz_offset as u64,
819 );
820 write_u64(
821 record,
822 SAT_CLOCK_NODE_OFFSET_OFFSET,
823 layout.clock_node_offset as u64,
824 );
825 write_u64(
826 record,
827 SAT_CLOCK_ARC_OFFSET_OFFSET,
828 layout.clock_arc_offset as u64,
829 );
830 write_u64(record, SAT_DATA_OFFSET_OFFSET, layout.data_offset as u64);
831 write_u64(record, SAT_DATA_LEN_OFFSET, layout.data_len as u64);
832
833 write_f64_slice(&mut out, layout.pos_x_offset, &series.x);
834 write_f64_slice(&mut out, layout.pos_kx_offset, &series.kx);
835 write_f64_slice(&mut out, layout.pos_ky_offset, &series.ky);
836 write_f64_slice(&mut out, layout.pos_kz_offset, &series.kz);
837 for (node_idx, &(x, clock_us, event)) in series.clk.iter().enumerate() {
838 let node_offset = layout.clock_node_offset + node_idx * CLOCK_NODE_RECORD_LEN;
839 let node = &mut out[node_offset..node_offset + CLOCK_NODE_RECORD_LEN];
840 write_f64(node, CLOCK_NODE_X_OFFSET, x);
841 write_f64(node, CLOCK_NODE_US_OFFSET, clock_us);
842 node[CLOCK_NODE_EVENT_OFFSET] = u8::from(event);
843 }
844
845 for (arc_idx, arc_layout) in layout.arcs.iter().enumerate() {
846 let arc = &fitted.clock_arcs[arc_idx];
847 let arc_offset = layout.clock_arc_offset + arc_idx * CLOCK_ARC_RECORD_LEN;
848 let record = &mut out[arc_offset..arc_offset + CLOCK_ARC_RECORD_LEN];
849 write_u32(
850 record,
851 CLOCK_ARC_NODE_COUNT_OFFSET,
852 u32::try_from(arc_layout.node_count)
853 .map_err(|_| parse_error("clock arc node count exceeds u32"))?,
854 );
855 write_u32(
856 record,
857 CLOCK_ARC_COEFF_COUNT_OFFSET,
858 u32::try_from(arc_layout.coeff_count)
859 .map_err(|_| parse_error("clock arc coefficient count exceeds u32"))?,
860 );
861 write_u64(
862 record,
863 CLOCK_ARC_X_OFFSET_OFFSET,
864 arc_layout.x_offset as u64,
865 );
866 write_u64(
867 record,
868 CLOCK_ARC_C0_OFFSET_OFFSET,
869 arc_layout.c0_offset as u64,
870 );
871 write_u64(
872 record,
873 CLOCK_ARC_C1_OFFSET_OFFSET,
874 arc_layout.c1_offset as u64,
875 );
876 write_u64(
877 record,
878 CLOCK_ARC_C2_OFFSET_OFFSET,
879 arc_layout.c2_offset as u64,
880 );
881 write_u64(
882 record,
883 CLOCK_ARC_C3_OFFSET_OFFSET,
884 arc_layout.c3_offset as u64,
885 );
886 write_f64_slice(&mut out, arc_layout.x_offset, &arc.x);
887 write_f64_slice(&mut out, arc_layout.c0_offset, &arc.c0);
888 write_f64_slice(&mut out, arc_layout.c1_offset, &arc.c1);
889 write_f64_slice(&mut out, arc_layout.c2_offset, &arc.c2);
890 write_f64_slice(&mut out, arc_layout.c3_offset, &arc.c3);
891 }
892
893 let sat_checksum = fnv1a64(&out[layout.data_offset..layout.data_offset + layout.data_len]);
894 let record = &mut out[record_offset..record_offset + SAT_INDEX_RECORD_LEN];
895 write_u64(record, SAT_CHECKSUM_OFFSET, sat_checksum);
896 }
897
898 let checksum = artifact_checksum64(&out);
899 write_u64(&mut out, HEADER_CHECKSUM_OFFSET, checksum);
900 Ok(out)
901}
902
903#[derive(Debug)]
904struct PendingSatLayout {
905 sat: GnssSatelliteId,
906 data_offset: usize,
907 data_len: usize,
908 pos_x_offset: usize,
909 pos_kx_offset: usize,
910 pos_ky_offset: usize,
911 pos_kz_offset: usize,
912 clock_node_offset: usize,
913 clock_arc_offset: usize,
914 arcs: Vec<PendingClockArcLayout>,
915}
916
917#[derive(Debug)]
918struct PendingClockArcLayout {
919 node_count: usize,
920 coeff_count: usize,
921 x_offset: usize,
922 c0_offset: usize,
923 c1_offset: usize,
924 c2_offset: usize,
925 c3_offset: usize,
926}
927
928fn parse_store<'a>(
929 bytes: &[u8],
930 backing: ArrayBacking<'a>,
931 checksum_validation: ChecksumValidation,
932) -> core::result::Result<ParsedStore<'a>, PreciseInterpolantStoreError> {
933 if bytes.len() < STORE_HEADER_LEN {
934 return Err(parse_error(format!(
935 "store has {} bytes but needs at least {STORE_HEADER_LEN}",
936 bytes.len()
937 )));
938 }
939 if &bytes[..STORE_MAGIC.len()] != STORE_MAGIC {
940 return Err(parse_error("missing precise interpolant store magic"));
941 }
942 let version = read_u16(bytes, HEADER_VERSION_OFFSET)?;
943 if version != STORE_VERSION {
944 return Err(PreciseInterpolantStoreError::UnsupportedVersion { version });
945 }
946
947 let expected_checksum = read_u64(bytes, HEADER_CHECKSUM_OFFSET)?;
948 match checksum_validation {
949 ChecksumValidation::Verified => {
950 let found_checksum = artifact_checksum64(bytes);
951 if expected_checksum != found_checksum {
952 return Err(PreciseInterpolantStoreError::Checksum {
953 expected: expected_checksum,
954 found: found_checksum,
955 });
956 }
957 }
958 ChecksumValidation::Attested(claimed) if claimed != expected_checksum => {
959 return Err(PreciseInterpolantStoreError::AttestedChecksumMismatch {
960 claimed,
961 declared: expected_checksum,
962 });
963 }
964 ChecksumValidation::Attested(_) => {}
965 }
966
967 ensure_zero(bytes, 11, 12, "header reserved byte")?;
968 ensure_zero(bytes, 48, STORE_HEADER_LEN, "header reserved bytes")?;
969 let time_scale = time_scale_from_tag(bytes[HEADER_TIME_SCALE_OFFSET])?;
970 let sat_count = read_u32(bytes, HEADER_SAT_COUNT_OFFSET)? as usize;
971 let index_offset = read_u64(bytes, HEADER_INDEX_OFFSET_OFFSET)? as usize;
972 let data_offset = read_u64(bytes, HEADER_DATA_OFFSET_OFFSET)? as usize;
973 let total_len = read_u64(bytes, HEADER_TOTAL_LEN_OFFSET)? as usize;
974 if total_len != bytes.len() {
975 return Err(parse_error(format!(
976 "header total length {total_len} does not match {}",
977 bytes.len()
978 )));
979 }
980 if index_offset != STORE_HEADER_LEN {
981 return Err(parse_error(format!(
982 "index offset must be {STORE_HEADER_LEN}, got {index_offset}"
983 )));
984 }
985
986 let index_len = sat_count
987 .checked_mul(SAT_INDEX_RECORD_LEN)
988 .ok_or_else(|| parse_error("satellite index length overflows usize"))?;
989 let index_end = index_offset
990 .checked_add(index_len)
991 .ok_or_else(|| parse_error("satellite index end overflows usize"))?;
992 if index_end > bytes.len() {
993 return Err(parse_error("satellite index extends past store length"));
994 }
995 let expected_data_offset = align_up(index_end, STORE_ALIGNMENT)?;
996 if data_offset != expected_data_offset {
997 return Err(parse_error(format!(
998 "data offset must be {expected_data_offset}, got {data_offset}"
999 )));
1000 }
1001 ensure_zero(bytes, index_end, data_offset, "index padding")?;
1002
1003 let mut satellites = Vec::with_capacity(sat_count);
1004 let mut series = BTreeMap::new();
1005 let mut previous = None;
1006 let mut expected_next = data_offset;
1007
1008 for idx in 0..sat_count {
1009 let record_offset = index_offset + idx * SAT_INDEX_RECORD_LEN;
1010 let record = &bytes[record_offset..record_offset + SAT_INDEX_RECORD_LEN];
1011 let sat = read_satellite(record)?;
1012 if previous.is_some_and(|prev| sat <= prev) {
1013 return Err(parse_error(
1014 "satellite index records are not strictly sorted",
1015 ));
1016 }
1017 previous = Some(sat);
1018
1019 ensure_zero(record, 2, 4, "satellite index reserved bytes")?;
1020 ensure_zero(
1021 record,
1022 88,
1023 SAT_INDEX_RECORD_LEN,
1024 "satellite index reserved bytes",
1025 )?;
1026
1027 let pos_count = read_u32(record, SAT_POS_COUNT_OFFSET)? as usize;
1028 let clock_node_count = read_u32(record, SAT_CLOCK_NODE_COUNT_OFFSET)? as usize;
1029 let clock_arc_count = read_u32(record, SAT_CLOCK_ARC_COUNT_OFFSET)? as usize;
1030 if pos_count < 2 {
1031 return Err(parse_error(format!(
1032 "satellite {sat} has invalid position node count {pos_count}"
1033 )));
1034 }
1035
1036 let pos_x_offset = read_u64(record, SAT_POS_X_OFFSET_OFFSET)? as usize;
1037 let pos_kx_offset = read_u64(record, SAT_POS_KX_OFFSET_OFFSET)? as usize;
1038 let pos_ky_offset = read_u64(record, SAT_POS_KY_OFFSET_OFFSET)? as usize;
1039 let pos_kz_offset = read_u64(record, SAT_POS_KZ_OFFSET_OFFSET)? as usize;
1040 let clock_node_offset = read_u64(record, SAT_CLOCK_NODE_OFFSET_OFFSET)? as usize;
1041 let clock_arc_offset = read_u64(record, SAT_CLOCK_ARC_OFFSET_OFFSET)? as usize;
1042 let sat_data_offset = read_u64(record, SAT_DATA_OFFSET_OFFSET)? as usize;
1043 let sat_data_len = read_u64(record, SAT_DATA_LEN_OFFSET)? as usize;
1044 let expected_sat_data_offset = align_up(expected_next, STORE_ALIGNMENT)?;
1045 ensure_zero(
1046 bytes,
1047 expected_next,
1048 expected_sat_data_offset,
1049 "satellite padding",
1050 )?;
1051 if sat_data_offset != expected_sat_data_offset {
1052 return Err(parse_error(format!(
1053 "satellite {sat} data offset must be {expected_sat_data_offset}, got {sat_data_offset}"
1054 )));
1055 }
1056 let sat_data_end = sat_data_offset
1057 .checked_add(sat_data_len)
1058 .ok_or_else(|| parse_error(format!("satellite {sat} data end overflows usize")))?;
1059 if sat_data_end > bytes.len() {
1060 return Err(parse_error(format!(
1061 "satellite {sat} data extends past store length"
1062 )));
1063 }
1064
1065 let sat_checksum = read_u64(record, SAT_CHECKSUM_OFFSET)?;
1066 if checksum_validation.verifies_payloads() {
1067 let found_sat_checksum = fnv1a64(&bytes[sat_data_offset..sat_data_end]);
1068 if sat_checksum != found_sat_checksum {
1069 return Err(PreciseInterpolantStoreError::SatelliteChecksum {
1070 sat,
1071 expected: sat_checksum,
1072 found: found_sat_checksum,
1073 });
1074 }
1075 }
1076
1077 let mut cursor = sat_data_offset;
1078 require_offset(sat, "position x", pos_x_offset, cursor)?;
1079 let pos_x = parse_f64_array(bytes, pos_x_offset, pos_count, sat, "position x", backing)?;
1080 validate_strictly_increasing_f64_array(bytes, &pos_x, sat, "position x")?;
1081 cursor = add_len(cursor, pos_count, 8)?;
1082 require_offset(sat, "position kx", pos_kx_offset, cursor)?;
1083 let pos_kx = parse_f64_array(bytes, pos_kx_offset, pos_count, sat, "position kx", backing)?;
1084 cursor = add_len(cursor, pos_count, 8)?;
1085 require_offset(sat, "position ky", pos_ky_offset, cursor)?;
1086 let pos_ky = parse_f64_array(bytes, pos_ky_offset, pos_count, sat, "position ky", backing)?;
1087 cursor = add_len(cursor, pos_count, 8)?;
1088 require_offset(sat, "position kz", pos_kz_offset, cursor)?;
1089 let pos_kz = parse_f64_array(bytes, pos_kz_offset, pos_count, sat, "position kz", backing)?;
1090 cursor = add_len(cursor, pos_count, 8)?;
1091
1092 require_offset(sat, "clock nodes", clock_node_offset, cursor)?;
1093 for node_idx in 0..clock_node_count {
1094 let node_offset = clock_node_offset + node_idx * CLOCK_NODE_RECORD_LEN;
1095 let node = bytes
1096 .get(node_offset..node_offset + CLOCK_NODE_RECORD_LEN)
1097 .ok_or_else(|| parse_error(format!("satellite {sat} clock node out of bounds")))?;
1098 let x = read_f64(node, CLOCK_NODE_X_OFFSET)?;
1099 let clock_us = read_f64(node, CLOCK_NODE_US_OFFSET)?;
1100 if !x.is_finite() || !clock_us.is_finite() {
1101 return Err(parse_error(format!(
1102 "satellite {sat} clock node {node_idx} is not finite"
1103 )));
1104 }
1105 match node[CLOCK_NODE_EVENT_OFFSET] {
1106 0 | 1 => {}
1107 tag => {
1108 return Err(parse_error(format!(
1109 "satellite {sat} clock node {node_idx} has invalid event tag {tag}"
1110 )));
1111 }
1112 }
1113 ensure_zero(
1114 node,
1115 CLOCK_NODE_EVENT_OFFSET + 1,
1116 CLOCK_NODE_RECORD_LEN,
1117 "clock node reserved bytes",
1118 )?;
1119 }
1120 cursor = add_len(cursor, clock_node_count, CLOCK_NODE_RECORD_LEN)?;
1121
1122 require_offset(sat, "clock arc index", clock_arc_offset, cursor)?;
1123 let clock_arc_index_end = add_len(cursor, clock_arc_count, CLOCK_ARC_RECORD_LEN)?;
1124 let mut arc_cursor = clock_arc_index_end;
1125 let mut arcs = Vec::with_capacity(clock_arc_count);
1126 for arc_idx in 0..clock_arc_count {
1127 let arc_offset = clock_arc_offset + arc_idx * CLOCK_ARC_RECORD_LEN;
1128 let arc_record = &bytes[arc_offset..arc_offset + CLOCK_ARC_RECORD_LEN];
1129 let node_count = read_u32(arc_record, CLOCK_ARC_NODE_COUNT_OFFSET)? as usize;
1130 let coeff_count = read_u32(arc_record, CLOCK_ARC_COEFF_COUNT_OFFSET)? as usize;
1131 if node_count == 0 {
1132 return Err(parse_error(format!(
1133 "satellite {sat} clock arc {arc_idx} is empty"
1134 )));
1135 }
1136 if coeff_count != node_count.saturating_sub(1) {
1137 return Err(parse_error(format!(
1138 "satellite {sat} clock arc {arc_idx} coefficient count {coeff_count} does not match node count {node_count}"
1139 )));
1140 }
1141 let x_offset = read_u64(arc_record, CLOCK_ARC_X_OFFSET_OFFSET)? as usize;
1142 let c0_offset = read_u64(arc_record, CLOCK_ARC_C0_OFFSET_OFFSET)? as usize;
1143 let c1_offset = read_u64(arc_record, CLOCK_ARC_C1_OFFSET_OFFSET)? as usize;
1144 let c2_offset = read_u64(arc_record, CLOCK_ARC_C2_OFFSET_OFFSET)? as usize;
1145 let c3_offset = read_u64(arc_record, CLOCK_ARC_C3_OFFSET_OFFSET)? as usize;
1146 ensure_zero(
1147 arc_record,
1148 CLOCK_ARC_C3_OFFSET_OFFSET + 8,
1149 CLOCK_ARC_RECORD_LEN,
1150 "clock arc reserved bytes",
1151 )?;
1152
1153 require_offset(sat, "clock arc x", x_offset, arc_cursor)?;
1154 let x = parse_f64_array(bytes, x_offset, node_count, sat, "clock arc x", backing)?;
1155 validate_strictly_increasing_f64_array(bytes, &x, sat, "clock arc x")?;
1156 arc_cursor = add_len(arc_cursor, node_count, 8)?;
1157 require_offset(sat, "clock arc c0", c0_offset, arc_cursor)?;
1158 let c0 = parse_f64_array(bytes, c0_offset, coeff_count, sat, "clock arc c0", backing)?;
1159 arc_cursor = add_len(arc_cursor, coeff_count, 8)?;
1160 require_offset(sat, "clock arc c1", c1_offset, arc_cursor)?;
1161 let c1 = parse_f64_array(bytes, c1_offset, coeff_count, sat, "clock arc c1", backing)?;
1162 arc_cursor = add_len(arc_cursor, coeff_count, 8)?;
1163 require_offset(sat, "clock arc c2", c2_offset, arc_cursor)?;
1164 let c2 = parse_f64_array(bytes, c2_offset, coeff_count, sat, "clock arc c2", backing)?;
1165 arc_cursor = add_len(arc_cursor, coeff_count, 8)?;
1166 require_offset(sat, "clock arc c3", c3_offset, arc_cursor)?;
1167 let c3 = parse_f64_array(bytes, c3_offset, coeff_count, sat, "clock arc c3", backing)?;
1168 arc_cursor = add_len(arc_cursor, coeff_count, 8)?;
1169
1170 arcs.push(MmapClockArc { x, c0, c1, c2, c3 });
1171 }
1172
1173 if sat_data_end != arc_cursor {
1174 return Err(parse_error(format!(
1175 "satellite {sat} data length must be {}, got {sat_data_len}",
1176 arc_cursor - sat_data_offset
1177 )));
1178 }
1179
1180 let inserted = series.insert(
1181 sat,
1182 MmapSeries {
1183 pos_count,
1184 clock_node_count,
1185 pos_x,
1186 pos_kx,
1187 pos_ky,
1188 pos_kz,
1189 clock_arcs: arcs,
1190 },
1191 );
1192 if inserted.is_some() {
1193 return Err(PreciseInterpolantStoreError::DuplicateSatellite { sat });
1194 }
1195 satellites.push(sat);
1196 expected_next = sat_data_end;
1197 }
1198
1199 if expected_next != bytes.len() {
1200 return Err(parse_error(format!(
1201 "store has trailing bytes: expected length {expected_next}, got {}",
1202 bytes.len()
1203 )));
1204 }
1205
1206 Ok(ParsedStore {
1207 time_scale,
1208 satellites,
1209 series,
1210 })
1211}
1212
1213fn interpolate_mapped_state(bytes: &[u8], series: &MmapSeries, query: f64) -> Result<Sp3State> {
1214 if series.pos_count < 2 {
1215 return Err(Error::EpochOutOfRange);
1216 }
1217
1218 let nominal = nominal_positive_spacing(bytes, series).ok_or(Error::EpochOutOfRange)?;
1219 let first = series.pos_x.get(bytes, 0);
1220 let last = series.pos_x.get(bytes, series.pos_count - 1);
1221 if query < first - nominal || query > last + nominal {
1222 return Err(Error::EpochOutOfRange);
1223 }
1224
1225 let gap_thresh = 1.5 * nominal;
1226 let mut bi = 0usize;
1227 while bi + 1 < series.pos_count && series.pos_x.get(bytes, bi + 1) <= query {
1228 bi += 1;
1229 }
1230 if bi + 1 < series.pos_count {
1231 let lo = series.pos_x.get(bytes, bi);
1232 let hi = series.pos_x.get(bytes, bi + 1);
1233 if hi - lo > gap_thresh && query > lo + nominal && query < hi - nominal {
1234 return Err(Error::EpochOutOfRange);
1235 }
1236 }
1237
1238 let (x_m, y_m, z_m) = interpolate_mapped_position_neville(bytes, series, query);
1239 let clock_s = interpolate_mapped_clock(bytes, series, query);
1240 Ok(Sp3State {
1241 position: ItrfPositionM::new(x_m, y_m, z_m).expect("valid ITRF position"),
1242 clock_s,
1243 velocity: None,
1244 clock_rate_s_s: None,
1245 flags: crate::sp3::Sp3Flags::default(),
1246 })
1247}
1248
1249fn interpolate_mapped_position_neville(
1250 bytes: &[u8],
1251 series: &MmapSeries,
1252 query: f64,
1253) -> (f64, f64, f64) {
1254 let n = series.pos_count;
1255 let nominal = nominal_positive_spacing(bytes, series).unwrap_or(1.0);
1256 let gap_thresh = 1.5 * nominal;
1257
1258 let mut pivot = 0usize;
1259 while pivot + 1 < n && series.pos_x.get(bytes, pivot + 1) <= query {
1260 pivot += 1;
1261 }
1262 if pivot + 1 < n {
1263 let x_pivot = series.pos_x.get(bytes, pivot);
1264 let x_next = series.pos_x.get(bytes, pivot + 1);
1265 if (x_next - x_pivot) > gap_thresh && query >= x_next - nominal {
1266 pivot += 1;
1267 }
1268 }
1269
1270 let mut run_lo = pivot;
1271 while run_lo > 0
1272 && (series.pos_x.get(bytes, run_lo) - series.pos_x.get(bytes, run_lo - 1)) <= gap_thresh
1273 {
1274 run_lo -= 1;
1275 }
1276 let mut run_hi = pivot + 1;
1277 while run_hi < n
1278 && (series.pos_x.get(bytes, run_hi) - series.pos_x.get(bytes, run_hi - 1)) <= gap_thresh
1279 {
1280 run_hi += 1;
1281 }
1282 let run_len = run_hi - run_lo;
1283
1284 let win = NEVILLE_POINTS.min(run_len);
1285 let half = (NEVILLE_POINTS / 2) as isize;
1286 let mut start = pivot as isize - half;
1287 if start < run_lo as isize {
1288 start = run_lo as isize;
1289 }
1290 if start + win as isize > run_hi as isize {
1291 start = run_hi as isize - win as isize;
1292 }
1293 let start = start as usize;
1294
1295 let mut t = [0.0f64; NEVILLE_POINTS];
1296 let mut px = [0.0f64; NEVILLE_POINTS];
1297 let mut py = [0.0f64; NEVILLE_POINTS];
1298 let mut pz = [0.0f64; NEVILLE_POINTS];
1299 for j in 0..win {
1300 let k = start + j;
1301 let tj = series.pos_x.get(bytes, k) - query;
1302 let kx = series.pos_kx.get(bytes, k);
1303 let ky = series.pos_ky.get(bytes, k);
1304 let kz = series.pos_kz.get(bytes, k);
1305 let (s, c) = (OMEGA_E_DOT_RAD_S * tj).sin_cos();
1306 t[j] = tj;
1307 px[j] = c * kx - s * ky;
1308 py[j] = s * kx + c * ky;
1309 pz[j] = kz;
1310 }
1311
1312 let x_km = neville(&t[..win], &px[..win]);
1313 let y_km = neville(&t[..win], &py[..win]);
1314 let z_km = neville(&t[..win], &pz[..win]);
1315 (x_km * KM_TO_M, y_km * KM_TO_M, z_km * KM_TO_M)
1316}
1317
1318fn interpolate_mapped_clock(bytes: &[u8], series: &MmapSeries, query: f64) -> Option<f64> {
1319 if series.clock_node_count < 2 {
1320 return None;
1321 }
1322 let mut chosen = None;
1323 for (idx, arc) in series.clock_arcs.iter().enumerate() {
1324 if mapped_arc_contains_query(bytes, arc, query) {
1325 chosen = Some(idx);
1326 break;
1327 }
1328 }
1329 let arc = match chosen {
1330 Some(idx) => &series.clock_arcs[idx],
1331 None => nearest_mapped_clock_arc(bytes, &series.clock_arcs, query)?,
1332 };
1333 if arc.node_count() < 2 {
1334 return None;
1335 }
1336 Some(evaluate_mapped_ppoly(bytes, arc, query) * US_TO_S)
1337}
1338
1339fn mapped_arc_contains_query(bytes: &[u8], arc: &MmapClockArc, query: f64) -> bool {
1340 let node_count = arc.node_count();
1341 if node_count == 0 {
1342 return false;
1343 }
1344 let lo = arc.x.get(bytes, 0);
1345 let hi = arc.x.get(bytes, node_count - 1);
1346 query >= lo && query <= hi
1347}
1348
1349fn nearest_mapped_clock_arc<'a, 'b>(
1350 bytes: &[u8],
1351 arcs: &'a [MmapClockArc<'b>],
1352 query: f64,
1353) -> Option<&'a MmapClockArc<'b>> {
1354 arcs.iter()
1355 .filter(|arc| arc.node_count() >= 2)
1356 .min_by(|arc1, arc2| {
1357 let d1 = mapped_span_distance(bytes, arc1, query);
1358 let d2 = mapped_span_distance(bytes, arc2, query);
1359 d1.partial_cmp(&d2).unwrap_or(core::cmp::Ordering::Equal)
1360 })
1361}
1362
1363fn mapped_span_distance(bytes: &[u8], arc: &MmapClockArc, query: f64) -> f64 {
1364 let lo = arc.x.get(bytes, 0);
1365 let hi = arc.x.get(bytes, arc.node_count() - 1);
1366 if query < lo {
1367 lo - query
1368 } else if query > hi {
1369 query - hi
1370 } else {
1371 0.0
1372 }
1373}
1374
1375fn evaluate_mapped_ppoly(bytes: &[u8], arc: &MmapClockArc, query: f64) -> f64 {
1376 let n = arc.node_count();
1377 let last = n - 2;
1378 let interval = if query.is_nan() {
1379 return f64::NAN;
1380 } else if query < arc.x.get(bytes, 0) {
1381 0
1382 } else if query >= arc.x.get(bytes, n - 1) {
1383 last
1384 } else {
1385 let mut lo = 0usize;
1386 let mut hi = n - 1;
1387 while hi - lo > 1 {
1388 let mid = (lo + hi) / 2;
1389 if arc.x.get(bytes, mid) <= query {
1390 lo = mid;
1391 } else {
1392 hi = mid;
1393 }
1394 }
1395 lo
1396 };
1397
1398 debug_assert!(interval < arc.coeff_count());
1399 let s = query - arc.x.get(bytes, interval);
1400 let mut res = 0.0;
1401 let mut z = 1.0;
1402 res += arc.c3.get(bytes, interval) * z;
1403 z *= s;
1404 res += arc.c2.get(bytes, interval) * z;
1405 z *= s;
1406 res += arc.c1.get(bytes, interval) * z;
1407 z *= s;
1408 res += arc.c0.get(bytes, interval) * z;
1409 res
1410}
1411
1412fn nominal_positive_spacing(bytes: &[u8], series: &MmapSeries) -> Option<f64> {
1413 let mut nominal = f64::INFINITY;
1414 for idx in 0..series.pos_count - 1 {
1415 let d = series.pos_x.get(bytes, idx + 1) - series.pos_x.get(bytes, idx);
1416 if d > 0.0 {
1417 nominal = nominal.min(d);
1418 }
1419 }
1420 if nominal.is_finite() {
1421 Some(nominal)
1422 } else {
1423 None
1424 }
1425}
1426
1427fn map_query_input(error: validate::FieldError) -> Error {
1428 Error::InvalidInput(format!("{} {}", error.field(), error.reason()))
1429}
1430
1431fn read_satellite(
1432 record: &[u8],
1433) -> core::result::Result<GnssSatelliteId, PreciseInterpolantStoreError> {
1434 let system_tag = record[SAT_SYSTEM_OFFSET];
1435 let system = GnssSystem::from_letter(char::from(system_tag))
1436 .ok_or(PreciseInterpolantStoreError::UnsupportedSatelliteSystem { tag: system_tag })?;
1437 let prn = record[SAT_PRN_OFFSET];
1438 GnssSatelliteId::new(system, prn).map_err(|err| parse_error(err.to_string()))
1439}
1440
1441fn time_scale_tag(scale: TimeScale) -> u8 {
1442 match scale {
1443 TimeScale::Utc => 1,
1444 TimeScale::Tai => 2,
1445 TimeScale::Tt => 3,
1446 TimeScale::Tcg => 4,
1447 TimeScale::Tdb => 5,
1448 TimeScale::Tcb => 6,
1449 TimeScale::Gpst => 7,
1450 TimeScale::Gst => 8,
1451 TimeScale::Bdt => 9,
1452 TimeScale::Glonasst => 10,
1453 TimeScale::Qzsst => 11,
1454 }
1455}
1456
1457fn time_scale_from_tag(tag: u8) -> core::result::Result<TimeScale, PreciseInterpolantStoreError> {
1458 match tag {
1459 1 => Ok(TimeScale::Utc),
1460 2 => Ok(TimeScale::Tai),
1461 3 => Ok(TimeScale::Tt),
1462 4 => Ok(TimeScale::Tcg),
1463 5 => Ok(TimeScale::Tdb),
1464 6 => Ok(TimeScale::Tcb),
1465 7 => Ok(TimeScale::Gpst),
1466 8 => Ok(TimeScale::Gst),
1467 9 => Ok(TimeScale::Bdt),
1468 10 => Ok(TimeScale::Glonasst),
1469 11 => Ok(TimeScale::Qzsst),
1470 other => Err(PreciseInterpolantStoreError::UnsupportedTimeScale { tag: other }),
1471 }
1472}
1473
1474fn require_offset(
1475 sat: GnssSatelliteId,
1476 field: &str,
1477 got: usize,
1478 expected: usize,
1479) -> core::result::Result<(), PreciseInterpolantStoreError> {
1480 if got == expected {
1481 Ok(())
1482 } else {
1483 Err(parse_error(format!(
1484 "satellite {sat} {field} offset must be {expected}, got {got}"
1485 )))
1486 }
1487}
1488
1489fn parse_f64_array<'a>(
1490 bytes: &[u8],
1491 offset: usize,
1492 count: usize,
1493 sat: GnssSatelliteId,
1494 field: &str,
1495 backing: ArrayBacking<'a>,
1496) -> core::result::Result<F64Array<'a>, PreciseInterpolantStoreError> {
1497 checked_range(bytes, offset, count, 8)?;
1498 let array = match backing {
1499 ArrayBacking::Borrowed(borrowed_bytes) => {
1500 F64Array::Borrowed(borrow_f64_slice(borrowed_bytes, offset, count, sat, field)?)
1501 }
1502 ArrayBacking::Offset => F64Array::Offset { offset, count },
1503 };
1504 for idx in 0..count {
1505 let value = array.get(bytes, idx);
1506 if !value.is_finite() {
1507 return Err(parse_error(format!(
1508 "satellite {sat} {field} value {idx} is not finite"
1509 )));
1510 }
1511 }
1512 Ok(array)
1513}
1514
1515fn validate_strictly_increasing_f64_array(
1516 bytes: &[u8],
1517 values: &F64Array<'_>,
1518 sat: GnssSatelliteId,
1519 field: &str,
1520) -> core::result::Result<(), PreciseInterpolantStoreError> {
1521 for idx in 0..values.len().saturating_sub(1) {
1522 if values.get(bytes, idx + 1) <= values.get(bytes, idx) {
1523 return Err(parse_error(format!(
1524 "satellite {sat} {field} values are not strictly increasing"
1525 )));
1526 }
1527 }
1528 Ok(())
1529}
1530
1531fn borrow_f64_slice<'a>(
1532 bytes: &'a [u8],
1533 offset: usize,
1534 count: usize,
1535 sat: GnssSatelliteId,
1536 field: &str,
1537) -> core::result::Result<&'a [f64], PreciseInterpolantStoreError> {
1538 let len = count
1539 .checked_mul(8)
1540 .ok_or_else(|| parse_error("byte range length overflows usize"))?;
1541 let end = offset
1542 .checked_add(len)
1543 .ok_or_else(|| parse_error("byte range end overflows usize"))?;
1544 let slice = bytes
1545 .get(offset..end)
1546 .ok_or_else(|| parse_error("byte range extends past store length"))?;
1547 if !cfg!(target_endian = "little") {
1548 return Err(parse_error(
1549 "zero-copy precise interpolant f64 arrays require a little-endian target",
1550 ));
1551 }
1552 if !(slice.as_ptr() as usize).is_multiple_of(mem::align_of::<f64>()) {
1553 return Err(parse_error(format!(
1554 "satellite {sat} {field} bytes are not aligned for zero-copy f64 access"
1555 )));
1556 }
1557 let (prefix, values, suffix) = unsafe { slice.align_to::<f64>() };
1560 if !prefix.is_empty() || !suffix.is_empty() || values.len() != count {
1561 return Err(parse_error(format!(
1562 "satellite {sat} {field} bytes cannot be borrowed as f64 values"
1563 )));
1564 }
1565 Ok(values)
1566}
1567
1568fn checked_range(
1569 bytes: &[u8],
1570 offset: usize,
1571 count: usize,
1572 item_len: usize,
1573) -> core::result::Result<(), PreciseInterpolantStoreError> {
1574 let len = count
1575 .checked_mul(item_len)
1576 .ok_or_else(|| parse_error("byte range length overflows usize"))?;
1577 let end = offset
1578 .checked_add(len)
1579 .ok_or_else(|| parse_error("byte range end overflows usize"))?;
1580 if end > bytes.len() {
1581 return Err(parse_error("byte range extends past store length"));
1582 }
1583 Ok(())
1584}
1585
1586fn add_len(
1587 cursor: usize,
1588 count: usize,
1589 item_len: usize,
1590) -> core::result::Result<usize, PreciseInterpolantStoreError> {
1591 let len = count
1592 .checked_mul(item_len)
1593 .ok_or_else(|| parse_error("byte count overflows usize"))?;
1594 cursor
1595 .checked_add(len)
1596 .ok_or_else(|| parse_error("byte cursor overflows usize"))
1597}
1598
1599fn align_up(
1600 value: usize,
1601 alignment: usize,
1602) -> core::result::Result<usize, PreciseInterpolantStoreError> {
1603 let rem = value % alignment;
1604 if rem == 0 {
1605 Ok(value)
1606 } else {
1607 value
1608 .checked_add(alignment - rem)
1609 .ok_or_else(|| parse_error("aligned offset overflows usize"))
1610 }
1611}
1612
1613fn ensure_zero(
1614 bytes: &[u8],
1615 start: usize,
1616 end: usize,
1617 context: &str,
1618) -> core::result::Result<(), PreciseInterpolantStoreError> {
1619 if start > end || end > bytes.len() {
1620 return Err(parse_error(format!("{context} range is out of bounds")));
1621 }
1622 if bytes[start..end].iter().any(|&byte| byte != 0) {
1623 return Err(parse_error(format!("{context} must be zero-filled")));
1624 }
1625 Ok(())
1626}
1627
1628fn parse_error(reason: impl Into<String>) -> PreciseInterpolantStoreError {
1629 PreciseInterpolantStoreError::Parse {
1630 reason: reason.into(),
1631 }
1632}
1633
1634fn artifact_checksum64(bytes: &[u8]) -> u64 {
1635 let mut hash = FNV_OFFSET_BASIS;
1636 for (idx, byte) in bytes.iter().enumerate() {
1637 let value = if (HEADER_CHECKSUM_OFFSET..HEADER_CHECKSUM_OFFSET + 8).contains(&idx) {
1638 0
1639 } else {
1640 *byte
1641 };
1642 hash = (hash ^ u64::from(value)).wrapping_mul(FNV_PRIME);
1643 }
1644 hash
1645}
1646
1647fn fnv1a64(bytes: &[u8]) -> u64 {
1648 bytes.iter().fold(FNV_OFFSET_BASIS, |hash, byte| {
1649 (hash ^ u64::from(*byte)).wrapping_mul(FNV_PRIME)
1650 })
1651}
1652
1653fn mapped_f64(bytes: &[u8], offset: usize, idx: usize) -> f64 {
1654 let start = offset + idx * 8;
1655 f64::from_le_bytes(
1656 bytes[start..start + 8]
1657 .try_into()
1658 .expect("validated f64 range"),
1659 )
1660}
1661
1662fn read_u16(
1663 bytes: &[u8],
1664 offset: usize,
1665) -> core::result::Result<u16, PreciseInterpolantStoreError> {
1666 Ok(u16::from_le_bytes(read_array(bytes, offset)?))
1667}
1668
1669fn read_u32(
1670 bytes: &[u8],
1671 offset: usize,
1672) -> core::result::Result<u32, PreciseInterpolantStoreError> {
1673 Ok(u32::from_le_bytes(read_array(bytes, offset)?))
1674}
1675
1676fn read_u64(
1677 bytes: &[u8],
1678 offset: usize,
1679) -> core::result::Result<u64, PreciseInterpolantStoreError> {
1680 Ok(u64::from_le_bytes(read_array(bytes, offset)?))
1681}
1682
1683fn read_f64(
1684 bytes: &[u8],
1685 offset: usize,
1686) -> core::result::Result<f64, PreciseInterpolantStoreError> {
1687 Ok(f64::from_le_bytes(read_array(bytes, offset)?))
1688}
1689
1690fn read_array<const N: usize>(
1691 bytes: &[u8],
1692 offset: usize,
1693) -> core::result::Result<[u8; N], PreciseInterpolantStoreError> {
1694 let end = offset
1695 .checked_add(N)
1696 .ok_or_else(|| parse_error("numeric field offset overflows usize"))?;
1697 let slice = bytes
1698 .get(offset..end)
1699 .ok_or_else(|| parse_error("numeric field extends past record"))?;
1700 slice
1701 .try_into()
1702 .map_err(|_| parse_error("numeric field has wrong length"))
1703}
1704
1705fn write_u16(bytes: &mut [u8], offset: usize, value: u16) {
1706 bytes[offset..offset + 2].copy_from_slice(&value.to_le_bytes());
1707}
1708
1709fn write_u32(bytes: &mut [u8], offset: usize, value: u32) {
1710 bytes[offset..offset + 4].copy_from_slice(&value.to_le_bytes());
1711}
1712
1713fn write_u64(bytes: &mut [u8], offset: usize, value: u64) {
1714 bytes[offset..offset + 8].copy_from_slice(&value.to_le_bytes());
1715}
1716
1717fn write_f64(bytes: &mut [u8], offset: usize, value: f64) {
1718 bytes[offset..offset + 8].copy_from_slice(&value.to_le_bytes());
1719}
1720
1721fn write_f64_slice(bytes: &mut [u8], offset: usize, values: &[f64]) {
1722 for (idx, value) in values.iter().enumerate() {
1723 write_f64(bytes, offset + idx * 8, *value);
1724 }
1725}