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