cu_aligner/
buffers.rs

1use circular_buffer::FixedCircularBuffer;
2use cu29::bincode::de::{Decode, Decoder};
3use cu29::bincode::enc::{Encode, Encoder};
4use cu29::bincode::error::{DecodeError, EncodeError};
5use cu29::prelude::*;
6
7/// An augmented circular buffer that allows for time-based operations.
8pub struct TimeboundCircularBuffer<const S: usize, P, M>
9where
10    P: CuMsgPayload,
11    M: Metadata,
12{
13    pub inner: FixedCircularBuffer<CuStampedData<P, M>, S>,
14}
15
16#[allow(dead_code)]
17fn extract_tov_time_left(tov: &Tov) -> Option<CuTime> {
18    match tov {
19        Tov::Time(time) => Some(*time),
20        Tov::Range(range) => Some(range.start), // Use the start of the range for alignment
21        Tov::None => None,
22    }
23}
24
25fn extract_tov_time_right(tov: &Tov) -> Option<CuTime> {
26    match tov {
27        Tov::Time(time) => Some(*time),
28        Tov::Range(range) => Some(range.end), // Use the end of the range for alignment
29        Tov::None => None,
30    }
31}
32
33fn encode_buffered_msg<P, E>(
34    msg: &CuStampedData<P, CuMsgMetadata>,
35    encoder: &mut E,
36) -> Result<(), EncodeError>
37where
38    P: CuMsgPayload,
39    E: Encoder,
40{
41    let bytes = cu29::bincode::encode_to_vec(msg, cu29::bincode::config::standard())?;
42    Encode::encode(&bytes, encoder)
43}
44
45fn decode_buffered_msg<P, D>(
46    decoder: &mut D,
47) -> Result<CuStampedData<P, CuMsgMetadata>, DecodeError>
48where
49    P: CuMsgPayload,
50    D: Decoder,
51{
52    let bytes: Vec<u8> = Decode::decode(decoder)?;
53    let (msg, bytes_read): (CuStampedData<P, CuMsgMetadata>, usize) =
54        cu29::bincode::decode_from_slice(&bytes, cu29::bincode::config::standard())?;
55    if bytes_read != bytes.len() {
56        return Err(DecodeError::OtherString(
57            "alignment buffer message snapshot had trailing bytes".to_string(),
58        ));
59    }
60    Ok(msg)
61}
62
63impl<const S: usize, P> Default for TimeboundCircularBuffer<S, P, CuMsgMetadata>
64where
65    P: CuMsgPayload,
66{
67    fn default() -> Self {
68        Self::new()
69    }
70}
71
72impl<const S: usize, P> TimeboundCircularBuffer<S, P, CuMsgMetadata>
73where
74    P: CuMsgPayload,
75{
76    pub fn new() -> Self {
77        Self {
78            // It is assumed to be sorted by time with non overlapping ranges if they are Tov::Range
79            inner: FixedCircularBuffer::<CuStampedData<P, CuMsgMetadata>, S>::new(),
80        }
81    }
82
83    /// Gets a slice of messages that fall within the given time range.
84    /// In case of a Tov::Range, the message is included if its start and end time fall within the range.
85    pub fn iter_window(
86        &self,
87        start_time: CuTime,
88        end_time: CuTime,
89    ) -> impl Iterator<Item = &CuStampedData<P, CuMsgMetadata>> {
90        self.inner.iter().filter(move |msg| match msg.tov {
91            Tov::Time(time) => time >= start_time && time <= end_time,
92            Tov::Range(range) => range.start >= start_time && range.end <= end_time,
93            _ => false,
94        })
95    }
96
97    /// Remove all the messages that are older than the given time horizon.
98    pub fn purge(&mut self, time_horizon: CuTime) {
99        // Find the index of the first element that should be retained
100        let drain_end = self
101            .inner
102            .iter()
103            .position(|msg| match msg.tov {
104                Tov::Time(time) => time >= time_horizon,
105                Tov::Range(range) => range.end >= time_horizon,
106                _ => false,
107            })
108            .unwrap_or(self.inner.len()); // If none match, drain the entire buffer
109
110        // Drain all elements before the `drain_end` index
111        self.inner.drain(..drain_end);
112    }
113
114    /// Get the most recent time of the messages in the buffer.
115    pub fn most_recent_time(&self) -> CuResult<Option<CuTime>> {
116        let mut latest: Option<CuTime> = None;
117        for msg in self.inner.iter() {
118            let time = extract_tov_time_right(&msg.tov).ok_or_else(|| {
119                CuError::from("Trying to align temporal data with no time information")
120            })?;
121            latest = Some(latest.map_or(time, |current_max| current_max.max(time)));
122        }
123        Ok(latest)
124    }
125
126    /// Push a message into the buffer.
127    pub fn push(&mut self, msg: CuStampedData<P, CuMsgMetadata>) {
128        self.inner.push_back(msg);
129    }
130
131    pub fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
132        Encode::encode(&(self.inner.len() as u64), encoder)?;
133        for msg in self.inner.iter() {
134            encode_buffered_msg(msg, encoder)?;
135        }
136        Ok(())
137    }
138
139    pub fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
140        let len: u64 = Decode::decode(decoder)?;
141        let len = usize::try_from(len).map_err(|_| {
142            DecodeError::OtherString("alignment buffer length does not fit usize".to_string())
143        })?;
144        if len > S {
145            return Err(DecodeError::ArrayLengthMismatch {
146                required: S,
147                found: len,
148            });
149        }
150
151        self.inner.clear();
152        for _ in 0..len {
153            self.inner.push_back(decode_buffered_msg(decoder)?);
154        }
155        Ok(())
156    }
157}
158
159#[macro_export]
160macro_rules! alignment_buffers {
161    ($struct_name:ident, $($name:ident: TimeboundCircularBuffer<$size:expr, CuStampedData<$payload:ty, CuMsgMetadata>>),*) => {
162        struct $struct_name {
163            target_alignment_window: cu29::clock::CuDuration, // size of the most recent data window to align
164            stale_data_horizon: cu29::clock::CuDuration,  // time horizon for purging stale data
165            $(pub $name: $crate::buffers::TimeboundCircularBuffer<$size, $payload, CuMsgMetadata>),*
166        }
167
168        impl $struct_name {
169            pub fn new(target_alignment_window: cu29::clock::CuDuration, stale_data_horizon: cu29::clock::CuDuration) -> Self {
170                Self {
171                    target_alignment_window,
172                    stale_data_horizon,
173                    $($name: $crate::buffers::TimeboundCircularBuffer::<$size, $payload, CuMsgMetadata>::new()),*
174                }
175            }
176
177            /// Call this to be sure we discard the old/ non relevant data
178            #[allow(dead_code)]
179            pub fn purge(&mut self, now: cu29::clock::CuTime) {
180                let horizon_time = now - self.stale_data_horizon;
181                // purge all the stale data from the TimeboundCircularBuffers first
182                $(self.$name.purge(horizon_time);)*
183            }
184
185            /// Get the most recent set of aligned data from all the buffers matching the constraints set at construction.
186            #[allow(dead_code)]
187            pub fn get_latest_aligned_data(
188                &mut self,
189            ) -> Option<($(impl Iterator<Item = &cu29::cutask::CuStampedData<$payload, CuMsgMetadata>>),*)> {
190                // Now find the min of the max of the last time for all buffers
191                // meaning the most recent time at which all buffers have data
192                let most_recent_time = [
193                    $(self.$name.most_recent_time().unwrap_or(None)),*
194                ]
195                .into_iter()
196                .flatten()
197                .min()?;
198
199                let time_to_get_complete_window = most_recent_time - self.target_alignment_window;
200                Some(($(self.$name.iter_window(time_to_get_complete_window, most_recent_time)),*))
201            }
202
203            #[allow(dead_code)]
204            pub fn freeze<E: cu29::bincode::enc::Encoder>(&self, encoder: &mut E) -> Result<(), cu29::bincode::error::EncodeError> {
205                $(self.$name.freeze(encoder)?;)*
206                Ok(())
207            }
208
209            #[allow(dead_code)]
210            pub fn thaw<D: cu29::bincode::de::Decoder>(&mut self, decoder: &mut D) -> Result<(), cu29::bincode::error::DecodeError> {
211                $(self.$name.thaw(decoder)?;)*
212                Ok(())
213            }
214        }
215    };
216}
217
218pub use alignment_buffers;
219
220#[cfg(test)]
221mod tests {
222    use cu29::clock::Tov;
223    use cu29::cutask::*;
224    use std::time::Duration;
225
226    #[test]
227    fn simple_init_test() {
228        alignment_buffers!(AlignmentBuffers, buffer1: TimeboundCircularBuffer<10, CuStampedData<u32, CuMsgMetadata>>, buffer2: TimeboundCircularBuffer<12, CuStampedData<u64, CuMsgMetadata>>);
229
230        let buffers =
231            AlignmentBuffers::new(Duration::from_secs(1).into(), Duration::from_secs(2).into());
232        assert_eq!(buffers.buffer1.inner.capacity(), 10);
233        assert_eq!(buffers.buffer2.inner.capacity(), 12);
234    }
235
236    #[test]
237    fn purge_test() {
238        alignment_buffers!(AlignmentBuffers, buffer1: TimeboundCircularBuffer<10, CuStampedData<u32, CuMsgMetadata>>, buffer2: TimeboundCircularBuffer<12, CuStampedData<u32, CuMsgMetadata>>);
239
240        let mut buffers =
241            AlignmentBuffers::new(Duration::from_secs(1).into(), Duration::from_secs(2).into());
242
243        let mut msg1 = CuStampedData::new(Some(1));
244        msg1.tov = Tov::Time(Duration::from_secs(1).into());
245        buffers.buffer1.inner.push_back(msg1.clone());
246        buffers.buffer2.inner.push_back(msg1);
247        // within the horizon
248        buffers.purge(Duration::from_secs(2).into());
249        assert_eq!(buffers.buffer1.inner.len(), 1);
250        assert_eq!(buffers.buffer2.inner.len(), 1);
251        // outside the horizon
252        buffers.purge(Duration::from_secs(5).into());
253        assert_eq!(buffers.buffer1.inner.len(), 0);
254        assert_eq!(buffers.buffer2.inner.len(), 0);
255    }
256
257    #[test]
258    fn empty_buffers_test() {
259        alignment_buffers!(
260            AlignmentBuffers,
261            buffer1: TimeboundCircularBuffer<10, CuStampedData<u32, CuMsgMetadata>>,
262            buffer2: TimeboundCircularBuffer<12, CuStampedData<u32, CuMsgMetadata>>
263        );
264
265        let mut buffers = AlignmentBuffers::new(
266            Duration::from_secs(2).into(), // 2-second alignment window
267            Duration::from_secs(5).into(), // 5-second stale data horizon
268        );
269
270        // Advance time to 10 seconds
271        assert!(buffers.get_latest_aligned_data().is_none());
272    }
273
274    #[test]
275    fn horizon_and_window_alignment_test() {
276        alignment_buffers!(
277            AlignmentBuffers,
278            buffer1: TimeboundCircularBuffer<10, CuStampedData<u32, CuMsgMetadata>>,
279            buffer2: TimeboundCircularBuffer<12, CuStampedData<u32, CuMsgMetadata>>
280        );
281
282        let mut buffers = AlignmentBuffers::new(
283            Duration::from_secs(2).into(), // 2-second alignment window
284            Duration::from_secs(5).into(), // 5-second stale data horizon
285        );
286
287        // Insert messages with timestamps
288        let mut msg1 = CuStampedData::new(Some(1));
289        msg1.tov = Tov::Time(Duration::from_secs(1).into());
290        buffers.buffer1.inner.push_back(msg1.clone());
291        buffers.buffer2.inner.push_back(msg1);
292
293        let mut msg2 = CuStampedData::new(Some(3));
294        msg2.tov = Tov::Time(Duration::from_secs(3).into());
295        buffers.buffer2.inner.push_back(msg2);
296
297        let mut msg3 = CuStampedData::new(Some(4));
298        msg3.tov = Tov::Time(Duration::from_secs(4).into());
299        buffers.buffer1.inner.push_back(msg3.clone());
300        buffers.buffer2.inner.push_back(msg3);
301
302        // Advance time to 7 seconds; horizon is 7 - 5 = everything 2+ should stay
303        let now = Duration::from_secs(7).into();
304        // Emulate a normal workflow here.
305        buffers.purge(now);
306        if let Some((iter1, iter2)) = buffers.get_latest_aligned_data() {
307            let collected1: Vec<_> = iter1.collect();
308            let collected2: Vec<_> = iter2.collect();
309
310            // Verify only messages within the alignment window [5, 7] are returned
311            assert_eq!(collected1.len(), 1);
312            assert_eq!(collected2.len(), 2);
313
314            assert_eq!(collected1[0].payload(), Some(&4));
315            assert_eq!(collected2[0].payload(), Some(&3));
316            assert_eq!(collected2[1].payload(), Some(&4));
317        } else {
318            panic!("Expected aligned data, but got None");
319        }
320
321        // Ensure older messages outside the horizon [>2 seconds] are purged
322        assert_eq!(buffers.buffer1.inner.len(), 1);
323        assert_eq!(buffers.buffer2.inner.len(), 2);
324    }
325}
cu_aligner/buffers.rs

cu_aligner/
buffers.rs
