1use super::fusion_time::align_time;
9use super::{PhysicalFieldSemantics, SemanticId, SemanticProjection};
10
11pub use super::fusion_time::{FusionClockStamp, FusionTarget, FusionTimeWindow, TimeAlignment};
12
13pub const MAX_FUSION_CANDIDATES: usize = 64;
15
16#[derive(Clone, Copy, Debug, PartialEq)]
20pub struct CoordinateMapping {
21 mapping_id: u64,
22 source_frame_id: u64,
23 target_frame_id: u64,
24 maximum_error: f64,
25 error_unit_id: SemanticId,
26}
27
28impl CoordinateMapping {
29 pub fn new(
30 mapping_id: u64,
31 source_frame_id: u64,
32 target_frame_id: u64,
33 maximum_error: f64,
34 error_unit_id: SemanticId,
35 ) -> Result<Self, FusionError> {
36 if mapping_id == 0 || source_frame_id == 0 || target_frame_id == 0 {
37 return Err(FusionError::InvalidIdentifier);
38 }
39 if !maximum_error.is_finite() || maximum_error < 0.0 {
40 return Err(FusionError::InvalidCoordinateMapping);
41 }
42 if error_unit_id.value().is_none() {
43 return Err(FusionError::UnknownMappingErrorUnit);
44 }
45 Ok(Self {
46 mapping_id,
47 source_frame_id,
48 target_frame_id,
49 maximum_error,
50 error_unit_id,
51 })
52 }
53}
54
55#[derive(Clone, Copy, Debug, PartialEq, Eq)]
57pub enum SampleQuality {
58 Unknown,
59 Known(SampleQualityMetrics),
60}
61
62#[derive(Clone, Copy, Debug, PartialEq, Eq)]
63pub struct SampleQualityMetrics {
64 score_millionths: u32,
65 uncertainty_millionths: u32,
66}
67
68impl SampleQuality {
69 pub const SCALE: u32 = 1_000_000;
70
71 pub const fn known(
72 score_millionths: u32,
73 uncertainty_millionths: u32,
74 ) -> Result<Self, FusionError> {
75 if score_millionths > Self::SCALE || uncertainty_millionths > Self::SCALE {
76 return Err(FusionError::InvalidQuality);
77 }
78 Ok(Self::Known(SampleQualityMetrics {
79 score_millionths,
80 uncertainty_millionths,
81 }))
82 }
83}
84
85impl SampleQualityMetrics {
86 pub const fn score_millionths(self) -> u32 {
87 self.score_millionths
88 }
89
90 pub const fn uncertainty_millionths(self) -> u32 {
91 self.uncertainty_millionths
92 }
93}
94
95#[derive(Clone, Copy, Debug, PartialEq)]
98pub struct FusionSample {
99 sample_id: u64,
100 physical: PhysicalFieldSemantics,
101 clock: Option<FusionClockStamp>,
102 coordinate_frame_id: SemanticId,
103 projection: Option<SemanticProjection>,
104 quality: SampleQuality,
105 priority: i32,
106}
107
108impl FusionSample {
109 #[allow(clippy::too_many_arguments)]
110 pub fn new(
111 sample_id: u64,
112 physical: PhysicalFieldSemantics,
113 clock: Option<FusionClockStamp>,
114 coordinate_frame_id: SemanticId,
115 projection: Option<SemanticProjection>,
116 quality: SampleQuality,
117 priority: i32,
118 ) -> Result<Self, FusionError> {
119 if sample_id == 0 {
120 return Err(FusionError::InvalidIdentifier);
121 }
122 if projection
123 .as_ref()
124 .is_some_and(|projection| projection.physical() != &physical)
125 {
126 return Err(FusionError::ProjectionSemanticMismatch);
127 }
128 Ok(Self {
129 sample_id,
130 physical,
131 clock,
132 coordinate_frame_id,
133 projection,
134 quality,
135 priority,
136 })
137 }
138}
139
140#[derive(Clone, Copy, Debug, PartialEq, Eq)]
144pub struct FusionPolicy {
145 maximum_samples: usize,
146 maximum_time_span_ns: u64,
147}
148
149impl FusionPolicy {
150 pub const fn new(
151 maximum_samples: usize,
152 maximum_time_span_ns: u64,
153 ) -> Result<Self, FusionError> {
154 if maximum_samples == 0 || maximum_samples > MAX_FUSION_CANDIDATES {
155 return Err(FusionError::InvalidPolicy);
156 }
157 Ok(Self {
158 maximum_samples,
159 maximum_time_span_ns,
160 })
161 }
162}
163
164#[derive(Clone, Copy, Debug, PartialEq)]
166pub struct FusionPlanEntry {
167 sample_id: u64,
168 input_index: usize,
169 physical: PhysicalFieldSemantics,
170 aligned_window: FusionTimeWindow,
171 alignment_id: Option<u64>,
172 mapping_id: u64,
173 mapping_maximum_error: f64,
174 mapping_error_unit_id: SemanticId,
175 projection: Option<SemanticProjection>,
176 quality: SampleQuality,
177}
178
179impl FusionPlanEntry {
180 pub const fn sample_id(self) -> u64 {
181 self.sample_id
182 }
183
184 pub const fn input_index(self) -> usize {
185 self.input_index
186 }
187
188 pub const fn physical(&self) -> &PhysicalFieldSemantics {
189 &self.physical
190 }
191
192 pub const fn aligned_window(self) -> FusionTimeWindow {
193 self.aligned_window
194 }
195
196 pub const fn alignment_id(self) -> Option<u64> {
197 self.alignment_id
198 }
199
200 pub const fn mapping_id(self) -> u64 {
201 self.mapping_id
202 }
203
204 pub const fn mapping_maximum_error(self) -> f64 {
205 self.mapping_maximum_error
206 }
207
208 pub const fn mapping_error_unit_id(self) -> SemanticId {
209 self.mapping_error_unit_id
210 }
211
212 pub const fn projection(self) -> Option<SemanticProjection> {
213 self.projection
214 }
215
216 pub const fn quality(self) -> SampleQuality {
217 self.quality
218 }
219}
220
221pub fn plan_fusion(
232 samples: &[FusionSample],
233 alignments: &[TimeAlignment],
234 mappings: &[CoordinateMapping],
235 target: FusionTarget,
236 policy: FusionPolicy,
237 out: &mut [Option<FusionPlanEntry>],
238) -> Result<usize, FusionError> {
239 if samples.len() > MAX_FUSION_CANDIDATES {
240 return Err(FusionError::TooManyCandidates);
241 }
242 validate_unique_ids(samples)?;
243
244 for sample in samples {
246 resolve_mapping(
247 sample.coordinate_frame_id,
248 target.coordinate_frame_id,
249 mappings,
250 )?;
251 align_time(sample.clock, target, alignments)?;
252 }
253
254 let selected_count = samples.len().min(policy.maximum_samples);
255 if out.len() < selected_count {
256 return Err(FusionError::OutputTooSmall);
257 }
258
259 let mut selected_indices = [usize::MAX; MAX_FUSION_CANDIDATES];
262 for rank in 0..selected_count {
263 let mut best_index: Option<usize> = None;
264 for (index, sample) in samples.iter().enumerate() {
265 if selected_indices[..rank]
266 .iter()
267 .any(|&selected| samples[selected].sample_id == sample.sample_id)
268 {
269 continue;
270 }
271 best_index = match best_index {
272 None => Some(index),
273 Some(current) => {
274 let incumbent = &samples[current];
275 if sample.priority > incumbent.priority
276 || (sample.priority == incumbent.priority
277 && sample.sample_id < incumbent.sample_id)
278 {
279 Some(index)
280 } else {
281 Some(current)
282 }
283 }
284 };
285 }
286 let input_index = best_index.ok_or(FusionError::InvalidPlanState)?;
287 selected_indices[rank] = input_index;
288 }
289
290 let mut planned = [None; MAX_FUSION_CANDIDATES];
291 let mut earliest_lower = u64::MAX;
292 let mut latest_upper = 0u64;
293 for rank in 0..selected_count {
294 let input_index = selected_indices[rank];
295 let entry = make_entry(
296 input_index,
297 &samples[input_index],
298 target,
299 alignments,
300 mappings,
301 )?;
302 earliest_lower = earliest_lower.min(entry.aligned_window.lower_ns);
303 latest_upper = latest_upper.max(entry.aligned_window.upper_ns);
304 planned[rank] = Some(entry);
305 }
306 if selected_count > 0 && latest_upper - earliest_lower > policy.maximum_time_span_ns {
307 return Err(FusionError::TemporalSpanExceeded);
308 }
309
310 out[..selected_count].copy_from_slice(&planned[..selected_count]);
313 Ok(selected_count)
314}
315
316fn validate_unique_ids(samples: &[FusionSample]) -> Result<(), FusionError> {
317 for i in 0..samples.len() {
318 for j in (i + 1)..samples.len() {
319 if samples[i].sample_id == samples[j].sample_id {
320 return Err(FusionError::DuplicateSampleId);
321 }
322 }
323 }
324 Ok(())
325}
326
327fn resolve_mapping(
328 source_frame_id: SemanticId,
329 target_frame_id: u64,
330 mappings: &[CoordinateMapping],
331) -> Result<CoordinateMapping, FusionError> {
332 let source_frame_id = source_frame_id
333 .value()
334 .ok_or(FusionError::UnknownCoordinateFrame)?;
335 let mut found = None;
336 for mapping in mappings {
337 if mapping.source_frame_id == source_frame_id && mapping.target_frame_id == target_frame_id
338 {
339 if found.is_some() {
340 return Err(FusionError::AmbiguousCoordinateMapping);
341 }
342 found = Some(*mapping);
343 }
344 }
345 found.ok_or(FusionError::MissingCoordinateMapping)
346}
347
348fn make_entry(
349 input_index: usize,
350 sample: &FusionSample,
351 target: FusionTarget,
352 alignments: &[TimeAlignment],
353 mappings: &[CoordinateMapping],
354) -> Result<FusionPlanEntry, FusionError> {
355 let (aligned_window, alignment_id) = align_time(sample.clock, target, alignments)?;
356 let mapping = resolve_mapping(
357 sample.coordinate_frame_id,
358 target.coordinate_frame_id,
359 mappings,
360 )?;
361 Ok(FusionPlanEntry {
362 sample_id: sample.sample_id,
363 input_index,
364 physical: sample.physical,
365 aligned_window,
366 alignment_id,
367 mapping_id: mapping.mapping_id,
368 mapping_maximum_error: mapping.maximum_error,
369 mapping_error_unit_id: mapping.error_unit_id,
370 projection: sample.projection,
371 quality: sample.quality,
372 })
373}
374
375#[derive(Clone, Copy, Debug, PartialEq, Eq)]
376pub enum FusionError {
377 InvalidIdentifier,
378 InvalidTime,
379 UnknownTime,
380 UnknownCoordinateFrame,
381 MissingTimeAlignment,
382 AmbiguousTimeAlignment,
383 InvalidTimeAlignment,
384 TimeAlignmentOutsideValidity,
385 MissingCoordinateMapping,
386 AmbiguousCoordinateMapping,
387 UnknownMappingErrorUnit,
388 InvalidCoordinateMapping,
389 InvalidQuality,
390 InvalidPolicy,
391 ProjectionSemanticMismatch,
392 TemporalSpanExceeded,
393 TooManyCandidates,
394 DuplicateSampleId,
395 OutputTooSmall,
396 InvalidPlanState,
397}
398
399#[cfg(test)]
400#[path = "fusion_tests.rs"]
401mod tests;