cu_sensor_payloads/
ranging.rs

1use bincode::de::Decoder;
2use bincode::enc::Encoder;
3use bincode::error::{DecodeError, EncodeError};
4use bincode::{Decode, Encode};
5use core::fmt;
6use cu29::clock::{CuTime, CuTimeRange};
7use cu29::prelude::*;
8use cu29::units::si::f32::Length;
9use cu29::units::si::length::meter;
10use serde::{Deserialize, Deserializer, Serialize, Serializer, de, ser::SerializeStruct};
11
12pub const RANGE_PEER_ID_CAPACITY: usize = 24;
13
14#[derive(Clone, Copy, Debug, PartialEq, Eq)]
15pub enum RangePeerIdError {
16    Empty,
17    TooLong { len: usize, max: usize },
18}
19
20impl fmt::Display for RangePeerIdError {
21    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
22        match self {
23            Self::Empty => write!(f, "range peer id must not be empty"),
24            Self::TooLong { len, max } => {
25                write!(f, "range peer id length {len} exceeds {max} bytes")
26            }
27        }
28    }
29}
30
31impl core::error::Error for RangePeerIdError {}
32
33/// Fixed-capacity peer identifier used by range observations.
34///
35/// This stays allocation-free on the runtime path while remaining flexible
36/// enough for common tag, anchor, beacon, or node identifiers.
37#[derive(
38    Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize, Encode, Decode, Reflect,
39)]
40pub struct RangePeerId {
41    bytes: [u8; RANGE_PEER_ID_CAPACITY],
42    len: u8,
43}
44
45impl RangePeerId {
46    pub fn new(peer_id: &str) -> Result<Self, RangePeerIdError> {
47        Self::try_from(peer_id)
48    }
49
50    pub fn as_str(&self) -> &str {
51        let len = self.len as usize;
52        core::str::from_utf8(&self.bytes[..len]).expect("RangePeerId stores valid UTF-8")
53    }
54
55    pub fn len(&self) -> usize {
56        self.len as usize
57    }
58
59    pub fn is_empty(&self) -> bool {
60        self.len == 0
61    }
62}
63
64impl TryFrom<&str> for RangePeerId {
65    type Error = RangePeerIdError;
66
67    fn try_from(peer_id: &str) -> Result<Self, Self::Error> {
68        if peer_id.is_empty() {
69            return Err(RangePeerIdError::Empty);
70        }
71        if peer_id.len() > RANGE_PEER_ID_CAPACITY {
72            return Err(RangePeerIdError::TooLong {
73                len: peer_id.len(),
74                max: RANGE_PEER_ID_CAPACITY,
75            });
76        }
77
78        let mut bytes = [0_u8; RANGE_PEER_ID_CAPACITY];
79        bytes[..peer_id.len()].copy_from_slice(peer_id.as_bytes());
80
81        Ok(Self {
82            bytes,
83            len: peer_id.len() as u8,
84        })
85    }
86}
87
88/// Standardized distance measurement to an identified peer.
89#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect)]
90pub struct PeerRangeObservation {
91    pub peer_id: RangePeerId,
92    pub distance: Length,
93    pub rssi_dbm: Option<i16>,
94}
95
96impl Default for PeerRangeObservation {
97    fn default() -> Self {
98        Self {
99            peer_id: RangePeerId::default(),
100            distance: Length::new::<meter>(0.0),
101            rssi_dbm: None,
102        }
103    }
104}
105
106impl PeerRangeObservation {
107    pub fn from_meters(peer_id: RangePeerId, distance_m: f32, rssi_dbm: Option<i16>) -> Self {
108        Self {
109            peer_id,
110            distance: Length::new::<meter>(distance_m),
111            rssi_dbm,
112        }
113    }
114
115    pub fn from_centimeters(peer_id: RangePeerId, distance_cm: u32, rssi_dbm: Option<i16>) -> Self {
116        Self::from_meters(peer_id, distance_cm as f32 / 100.0, rssi_dbm)
117    }
118}
119
120/// A peer range measurement with its own time of validity.
121///
122/// Use this inside aggregate payloads when samples were not captured at the same instant. For a
123/// single `CuMsg<PeerRangeObservation>`, prefer carrying the time in the Copper message envelope.
124#[derive(
125    Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect,
126)]
127pub struct PeerRangeSample {
128    pub tov: CuTime,
129    pub observation: PeerRangeObservation,
130}
131
132impl PeerRangeSample {
133    pub fn new(tov: CuTime, observation: PeerRangeObservation) -> Self {
134        Self { tov, observation }
135    }
136}
137
138/// Bounded collection of timestamped peer ranges.
139///
140/// The enclosing Copper message should use `Tov::Range` when the samples span more than one
141/// validity instant.
142#[derive(Clone, Copy, Debug, PartialEq, Reflect)]
143pub struct PeerRangeSnapshot<const N: usize> {
144    pub len: usize,
145    pub samples: [PeerRangeSample; N],
146}
147
148impl<const N: usize> Encode for PeerRangeSnapshot<N> {
149    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
150        Encode::encode(&self.samples().len(), encoder)?;
151        for sample in self.samples() {
152            Encode::encode(sample, encoder)?;
153        }
154        Ok(())
155    }
156}
157
158impl<const N: usize> Decode<()> for PeerRangeSnapshot<N> {
159    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
160        let len = <usize as Decode<()>>::decode(decoder)?;
161        if len > N {
162            return Err(DecodeError::ArrayLengthMismatch {
163                required: N,
164                found: len,
165            });
166        }
167
168        let mut snapshot = Self::default();
169        for _ in 0..len {
170            let sample = <PeerRangeSample as Decode<()>>::decode(decoder)?;
171            snapshot
172                .push(sample)
173                .map_err(|_| DecodeError::ArrayLengthMismatch {
174                    required: N,
175                    found: len,
176                })?;
177        }
178
179        Ok(snapshot)
180    }
181}
182
183impl<const N: usize> Serialize for PeerRangeSnapshot<N> {
184    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
185    where
186        S: Serializer,
187    {
188        let mut state = serializer.serialize_struct("PeerRangeSnapshot", 2)?;
189        state.serialize_field("len", &self.samples().len())?;
190        state.serialize_field("samples", self.samples())?;
191        state.end()
192    }
193}
194
195impl<'de, const N: usize> Deserialize<'de> for PeerRangeSnapshot<N> {
196    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
197    where
198        D: Deserializer<'de>,
199    {
200        #[derive(Deserialize)]
201        struct PeerRangeSnapshotSerde {
202            len: usize,
203            samples: alloc::vec::Vec<PeerRangeSample>,
204        }
205
206        let decoded = PeerRangeSnapshotSerde::deserialize(deserializer)?;
207        if decoded.len != decoded.samples.len() {
208            return Err(de::Error::custom(
209                "peer range snapshot len does not match samples",
210            ));
211        }
212        if decoded.samples.len() > N {
213            return Err(de::Error::custom("peer range snapshot exceeds capacity"));
214        }
215
216        let mut snapshot = Self::default();
217        for sample in decoded.samples {
218            snapshot
219                .push(sample)
220                .map_err(|_| de::Error::custom("peer range snapshot exceeds capacity"))?;
221        }
222
223        Ok(snapshot)
224    }
225}
226
227impl<const N: usize> Default for PeerRangeSnapshot<N> {
228    fn default() -> Self {
229        Self {
230            len: 0,
231            samples: [PeerRangeSample::default(); N],
232        }
233    }
234}
235
236impl<const N: usize> PeerRangeSnapshot<N> {
237    pub fn new() -> Self {
238        Self::default()
239    }
240
241    pub fn is_empty(&self) -> bool {
242        self.len == 0
243    }
244
245    pub fn is_full(&self) -> bool {
246        self.len >= N
247    }
248
249    pub fn samples(&self) -> &[PeerRangeSample] {
250        &self.samples[..self.len.min(N)]
251    }
252
253    pub fn push(&mut self, sample: PeerRangeSample) -> Result<(), PeerRangeSnapshotFull> {
254        if self.is_full() {
255            return Err(PeerRangeSnapshotFull { capacity: N });
256        }
257
258        self.samples[self.len] = sample;
259        self.len += 1;
260        Ok(())
261    }
262
263    pub fn tov_range(&self) -> Option<CuTimeRange> {
264        let samples = self.samples();
265        if samples.is_empty() {
266            return None;
267        }
268
269        let mut start = samples[0].tov;
270        let mut end = samples[0].tov;
271        for sample in &samples[1..] {
272            start = start.min(sample.tov);
273            end = end.max(sample.tov);
274        }
275
276        Some(CuTimeRange { start, end })
277    }
278}
279
280#[derive(Clone, Copy, Debug, PartialEq, Eq)]
281pub struct PeerRangeSnapshotFull {
282    pub capacity: usize,
283}
284
285impl fmt::Display for PeerRangeSnapshotFull {
286    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
287        write!(f, "peer range snapshot capacity {} is full", self.capacity)
288    }
289}
290
291impl core::error::Error for PeerRangeSnapshotFull {}
292
293#[cfg(test)]
294mod tests {
295    use super::*;
296    use bincode::config;
297    use cu29::units::si::length::meter;
298
299    #[test]
300    fn range_peer_id_round_trip() {
301        let peer_id = RangePeerId::new("DAVID123").unwrap();
302        assert_eq!(peer_id.as_str(), "DAVID123");
303        assert_eq!(peer_id.len(), 8);
304    }
305
306    #[test]
307    fn range_peer_id_rejects_too_long_values() {
308        let err = RangePeerId::new("1234567890123456789012345").unwrap_err();
309        assert!(matches!(err, RangePeerIdError::TooLong { .. }));
310    }
311
312    #[test]
313    fn peer_range_observation_round_trip_encode_decode() {
314        let payload = PeerRangeObservation::from_centimeters(
315            RangePeerId::new("TAG-01").unwrap(),
316            245,
317            Some(-71),
318        );
319
320        let cfg = config::standard();
321        let mut buffer = [0u8; 128];
322        let len = bincode::encode_into_slice(payload, &mut buffer, cfg).unwrap();
323        let (decoded, used) =
324            bincode::decode_from_slice::<PeerRangeObservation, _>(&buffer[..len], cfg).unwrap();
325
326        assert_eq!(used, len);
327        assert_eq!(decoded.peer_id.as_str(), "TAG-01");
328        assert_eq!(decoded.rssi_dbm, Some(-71));
329        assert_eq!(decoded.distance.get::<meter>(), 2.45);
330    }
331
332    #[test]
333    fn peer_range_snapshot_tracks_sample_time_range() {
334        let mut snapshot = PeerRangeSnapshot::<2>::new();
335        snapshot
336            .push(PeerRangeSample::new(
337                CuTime::from_nanos(20),
338                PeerRangeObservation::from_centimeters(
339                    RangePeerId::new("TAG-02").unwrap(),
340                    245,
341                    None,
342                ),
343            ))
344            .unwrap();
345        snapshot
346            .push(PeerRangeSample::new(
347                CuTime::from_nanos(10),
348                PeerRangeObservation::from_centimeters(
349                    RangePeerId::new("TAG-01").unwrap(),
350                    120,
351                    Some(-71),
352                ),
353            ))
354            .unwrap();
355
356        let range = snapshot.tov_range().unwrap();
357        assert_eq!(range.start, CuTime::from_nanos(10));
358        assert_eq!(range.end, CuTime::from_nanos(20));
359        assert_eq!(snapshot.samples()[1].observation.peer_id.as_str(), "TAG-01");
360    }
361
362    #[test]
363    fn peer_range_snapshot_rejects_over_capacity_push() {
364        let mut snapshot = PeerRangeSnapshot::<1>::new();
365        let sample = PeerRangeSample::new(
366            CuTime::from_nanos(1),
367            PeerRangeObservation::from_centimeters(RangePeerId::new("TAG-01").unwrap(), 100, None),
368        );
369
370        snapshot.push(sample).unwrap();
371        assert_eq!(
372            snapshot.push(sample),
373            Err(PeerRangeSnapshotFull { capacity: 1 })
374        );
375    }
376
377    #[test]
378    fn peer_range_snapshot_round_trip_encode_decode() {
379        let mut snapshot = PeerRangeSnapshot::<4>::new();
380        snapshot
381            .push(PeerRangeSample::new(
382                CuTime::from_nanos(10),
383                PeerRangeObservation::from_centimeters(
384                    RangePeerId::new("TAG-01").unwrap(),
385                    100,
386                    Some(-70),
387                ),
388            ))
389            .unwrap();
390        snapshot
391            .push(PeerRangeSample::new(
392                CuTime::from_nanos(20),
393                PeerRangeObservation::from_centimeters(
394                    RangePeerId::new("TAG-02").unwrap(),
395                    200,
396                    None,
397                ),
398            ))
399            .unwrap();
400
401        let cfg = config::standard();
402        let mut buffer = [0u8; 256];
403        let len = bincode::encode_into_slice(snapshot, &mut buffer, cfg).unwrap();
404        let (decoded, used) =
405            bincode::decode_from_slice::<PeerRangeSnapshot<4>, _>(&buffer[..len], cfg).unwrap();
406
407        assert_eq!(used, len);
408        assert_eq!(decoded.len, 2);
409        assert_eq!(decoded.samples()[0].observation.peer_id.as_str(), "TAG-01");
410        assert_eq!(decoded.samples()[1].observation.peer_id.as_str(), "TAG-02");
411    }
412}
cu_sensor_payloads/ranging.rs

cu_sensor_payloads/
ranging.rs
