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