wickra_core/indicators/
distance_ssd.rs1use std::collections::VecDeque;
4
5use crate::error::{Error, Result};
6use crate::traits::Indicator;
7
8#[derive(Debug, Clone)]
45pub struct DistanceSsd {
46 period: usize,
47 window: VecDeque<(f64, f64)>,
48}
49
50impl DistanceSsd {
51 pub fn new(period: usize) -> Result<Self> {
57 if period < 2 {
58 return Err(Error::InvalidPeriod {
59 message: "distance SSD needs period >= 2",
60 });
61 }
62 Ok(Self {
63 period,
64 window: VecDeque::with_capacity(period),
65 })
66 }
67
68 pub const fn period(&self) -> usize {
70 self.period
71 }
72}
73
74impl Indicator for DistanceSsd {
75 type Input = (f64, f64);
76 type Output = f64;
77
78 fn update(&mut self, input: (f64, f64)) -> Option<f64> {
79 if !input.0.is_finite() || !input.1.is_finite() {
80 return None;
81 }
82 if self.window.len() == self.period {
83 self.window.pop_front();
84 }
85 self.window.push_back(input);
86 if self.window.len() < self.period {
87 return None;
88 }
89 let &(a_first, b_first) = self.window.front().expect("window is full");
90 if a_first == 0.0 || b_first == 0.0 {
91 return Some(0.0);
93 }
94 let ssd = self
95 .window
96 .iter()
97 .map(|&(a, b)| {
98 let gap = a / a_first - b / b_first;
99 gap * gap
100 })
101 .sum();
102 Some(ssd)
103 }
104
105 fn reset(&mut self) {
106 self.window.clear();
107 }
108
109 fn warmup_period(&self) -> usize {
110 self.period
111 }
112
113 fn is_ready(&self) -> bool {
114 self.window.len() == self.period
115 }
116
117 fn name(&self) -> &'static str {
118 "DistanceSsd"
119 }
120}
121
122#[cfg(test)]
123mod tests {
124 use super::*;
125 use crate::traits::BatchExt;
126 use approx::assert_relative_eq;
127
128 #[test]
129 fn rejects_period_below_two() {
130 assert!(DistanceSsd::new(1).is_err());
131 assert!(DistanceSsd::new(2).is_ok());
132 }
133
134 #[test]
135 fn accessors_and_metadata() {
136 let d = DistanceSsd::new(20).unwrap();
137 assert_eq!(d.period(), 20);
138 assert_eq!(d.warmup_period(), 20);
139 assert_eq!(d.name(), "DistanceSsd");
140 assert!(!d.is_ready());
141 }
142
143 #[test]
144 fn warmup_returns_none() {
145 let mut d = DistanceSsd::new(3).unwrap();
146 assert_eq!(d.update((1.0, 1.0)), None);
147 assert_eq!(d.update((2.0, 2.0)), None);
148 assert!(d.update((3.0, 3.0)).is_some());
149 assert!(d.is_ready());
150 }
151
152 #[test]
153 fn identical_normalised_paths_have_zero_distance() {
154 let pairs: Vec<(f64, f64)> = (0..20)
156 .map(|t| {
157 let a = 100.0 + f64::from(t);
158 (a, 2.0 * a)
159 })
160 .collect();
161 let last = DistanceSsd::new(10)
162 .unwrap()
163 .batch(&pairs)
164 .into_iter()
165 .flatten()
166 .last()
167 .unwrap();
168 assert_relative_eq!(last, 0.0, epsilon = 1e-12);
169 }
170
171 #[test]
172 fn diverging_paths_have_positive_distance() {
173 let pairs: Vec<(f64, f64)> = (0..20)
174 .map(|t| (100.0 + f64::from(t), 100.0 + 3.0 * f64::from(t)))
175 .collect();
176 let last = DistanceSsd::new(10)
177 .unwrap()
178 .batch(&pairs)
179 .into_iter()
180 .flatten()
181 .last()
182 .unwrap();
183 assert!(last > 0.0, "ssd {last}");
184 }
185
186 #[test]
187 fn hand_computed_value() {
188 let pairs = [(1.0, 1.0), (2.0, 4.0), (3.0, 9.0)];
191 let last = DistanceSsd::new(3)
192 .unwrap()
193 .batch(&pairs)
194 .into_iter()
195 .flatten()
196 .last()
197 .unwrap();
198 assert_relative_eq!(last, 40.0, epsilon = 1e-12);
199 }
200
201 #[test]
202 fn zero_start_returns_zero() {
203 let pairs = [(0.0, 1.0), (2.0, 2.0), (3.0, 3.0)];
205 let last = DistanceSsd::new(3)
206 .unwrap()
207 .batch(&pairs)
208 .into_iter()
209 .flatten()
210 .last()
211 .unwrap();
212 assert_eq!(last, 0.0);
213 }
214
215 #[test]
216 fn reset_clears_state() {
217 let mut d = DistanceSsd::new(4).unwrap();
218 d.batch(&[(1.0, 1.0), (2.0, 2.0), (3.0, 4.0), (4.0, 5.0), (5.0, 6.0)]);
219 assert!(d.is_ready());
220 d.reset();
221 assert!(!d.is_ready());
222 assert_eq!(d.update((1.0, 1.0)), None);
223 }
224
225 #[test]
226 fn batch_equals_streaming() {
227 let pairs: Vec<(f64, f64)> = (0..60)
228 .map(|t| {
229 let a = 100.0 + f64::from(t);
230 (a, 100.0 + 1.2 * f64::from(t) + (f64::from(t) * 0.5).sin())
231 })
232 .collect();
233 let batch = DistanceSsd::new(15).unwrap().batch(&pairs);
234 let mut d = DistanceSsd::new(15).unwrap();
235 let streamed: Vec<_> = pairs.iter().map(|p| d.update(*p)).collect();
236 assert_eq!(batch, streamed);
237 }
238
239 #[test]
240 fn non_finite_input_returns_none() {
241 let mut d = DistanceSsd::new(3).unwrap();
242 assert_eq!(d.update((f64::NAN, 1.0)), None);
243 assert_eq!(d.update((1.0, f64::INFINITY)), None);
244 assert_eq!(d.update((1.0, 1.0)), None);
246 assert_eq!(d.update((2.0, 4.0)), None);
247 assert!(d.update((3.0, 9.0)).is_some());
248 }
249}