Skip to main content

qualia_core_db/sensing/
fusion_time.rs

1//! Explicit clock-domain alignment and conservative uncertainty windows.
2
3use super::fusion::FusionError;
4
5/// A source clock timestamp with an integer nanosecond worst-case absolute
6/// error radius. The true timestamp is assumed to lie in the inclusive interval
7/// `[max(0, ticks_ns - uncertainty_ns), ticks_ns + uncertainty_ns]`.
8/// Clock IDs identify monotonic domains; they do not imply UTC or host time.
9#[derive(Clone, Copy, Debug, PartialEq, Eq)]
10pub struct FusionClockStamp {
11    clock_id: u64,
12    restart_epoch: u32,
13    ticks_ns: u64,
14    uncertainty_ns: u64,
15}
16
17impl FusionClockStamp {
18    pub const fn new(
19        clock_id: u64,
20        restart_epoch: u32,
21        ticks_ns: u64,
22        uncertainty_ns: u64,
23    ) -> Result<Self, FusionError> {
24        if clock_id == 0 {
25            return Err(FusionError::InvalidIdentifier);
26        }
27        if ticks_ns.checked_add(uncertainty_ns).is_none() {
28            return Err(FusionError::InvalidTime);
29        }
30        Ok(Self {
31            clock_id,
32            restart_epoch,
33            ticks_ns,
34            uncertainty_ns,
35        })
36    }
37}
38
39/// An explicit, constant-offset mapping from a source clock/epoch into the
40/// selected target clock. It is valid only over the declared source-time range.
41#[derive(Clone, Copy, Debug, PartialEq, Eq)]
42pub struct TimeAlignment {
43    alignment_id: u64,
44    source_clock_id: u64,
45    source_epoch: u32,
46    target_clock_id: u64,
47    target_epoch: u32,
48    valid_from_ns: u64,
49    valid_through_ns: u64,
50    offset_ns: i128,
51    /// Worst-case absolute error bound on `offset_ns` over the validity range.
52    /// It is conservatively added to the sample's absolute timestamp bound.
53    uncertainty_ns: u64,
54}
55
56impl TimeAlignment {
57    #[allow(clippy::too_many_arguments)]
58    pub const fn new(
59        alignment_id: u64,
60        source_clock_id: u64,
61        source_epoch: u32,
62        target_clock_id: u64,
63        target_epoch: u32,
64        valid_from_ns: u64,
65        valid_through_ns: u64,
66        offset_ns: i128,
67        uncertainty_ns: u64,
68    ) -> Result<Self, FusionError> {
69        if alignment_id == 0 || source_clock_id == 0 || target_clock_id == 0 {
70            return Err(FusionError::InvalidIdentifier);
71        }
72        if valid_from_ns > valid_through_ns {
73            return Err(FusionError::InvalidTimeAlignment);
74        }
75        Ok(Self {
76            alignment_id,
77            source_clock_id,
78            source_epoch,
79            target_clock_id,
80            target_epoch,
81            valid_from_ns,
82            valid_through_ns,
83            offset_ns,
84            uncertainty_ns,
85        })
86    }
87}
88
89/// Target clock and coordinate frame for a plan.
90#[derive(Clone, Copy, Debug, PartialEq, Eq)]
91pub struct FusionTarget {
92    pub(super) clock_id: u64,
93    pub(super) restart_epoch: u32,
94    pub(super) coordinate_frame_id: u64,
95}
96
97impl FusionTarget {
98    pub const fn new(
99        clock_id: u64,
100        restart_epoch: u32,
101        coordinate_frame_id: u64,
102    ) -> Result<Self, FusionError> {
103        if clock_id == 0 || coordinate_frame_id == 0 {
104            return Err(FusionError::InvalidIdentifier);
105        }
106        Ok(Self {
107            clock_id,
108            restart_epoch,
109            coordinate_frame_id,
110        })
111    }
112}
113
114/// Inclusive aligned time interval, expressed only in the target clock domain.
115#[derive(Clone, Copy, Debug, PartialEq, Eq)]
116pub struct FusionTimeWindow {
117    pub(super) clock_id: u64,
118    pub(super) restart_epoch: u32,
119    pub(super) lower_ns: u64,
120    pub(super) upper_ns: u64,
121}
122
123impl FusionTimeWindow {
124    pub const fn clock_id(self) -> u64 {
125        self.clock_id
126    }
127
128    pub const fn restart_epoch(self) -> u32 {
129        self.restart_epoch
130    }
131
132    pub const fn lower_ns(self) -> u64 {
133        self.lower_ns
134    }
135
136    pub const fn upper_ns(self) -> u64 {
137        self.upper_ns
138    }
139}
140
141pub(super) fn align_time(
142    stamp: Option<FusionClockStamp>,
143    target: FusionTarget,
144    alignments: &[TimeAlignment],
145) -> Result<(FusionTimeWindow, Option<u64>), FusionError> {
146    let stamp = stamp.ok_or(FusionError::UnknownTime)?;
147    let matching_domain =
148        stamp.clock_id == target.clock_id && stamp.restart_epoch == target.restart_epoch;
149    let (center_ns, alignment_id, alignment_uncertainty) = if matching_domain {
150        (stamp.ticks_ns, None, 0)
151    } else {
152        let mut found = None;
153        for alignment in alignments {
154            if alignment.source_clock_id == stamp.clock_id
155                && alignment.source_epoch == stamp.restart_epoch
156                && alignment.target_clock_id == target.clock_id
157                && alignment.target_epoch == target.restart_epoch
158            {
159                if found.is_some() {
160                    return Err(FusionError::AmbiguousTimeAlignment);
161                }
162                found = Some(*alignment);
163            }
164        }
165        let alignment = found.ok_or(FusionError::MissingTimeAlignment)?;
166        let source_lower = stamp.ticks_ns.saturating_sub(stamp.uncertainty_ns);
167        let source_upper = stamp
168            .ticks_ns
169            .checked_add(stamp.uncertainty_ns)
170            .ok_or(FusionError::InvalidTime)?;
171        if source_lower < alignment.valid_from_ns || source_upper > alignment.valid_through_ns {
172            return Err(FusionError::TimeAlignmentOutsideValidity);
173        }
174        let center = i128::from(stamp.ticks_ns)
175            .checked_add(alignment.offset_ns)
176            .ok_or(FusionError::InvalidTime)?;
177        if center < 0 || center > i128::from(u64::MAX) {
178            return Err(FusionError::InvalidTime);
179        }
180        (
181            center as u64,
182            Some(alignment.alignment_id),
183            alignment.uncertainty_ns,
184        )
185    };
186    let uncertainty_ns = stamp
187        .uncertainty_ns
188        .checked_add(alignment_uncertainty)
189        .ok_or(FusionError::InvalidTime)?;
190    let upper_ns = center_ns
191        .checked_add(uncertainty_ns)
192        .ok_or(FusionError::InvalidTime)?;
193    let lower_ns = center_ns.saturating_sub(uncertainty_ns);
194    Ok((
195        FusionTimeWindow {
196            clock_id: target.clock_id,
197            restart_epoch: target.restart_epoch,
198            lower_ns,
199            upper_ns,
200        },
201        alignment_id,
202    ))
203}