1extern crate alloc;
6use crate::fitting::LinearRegression;
7use crate::sampling::Sample;
8use crate::streaming::Summary;
9use alloc::collections::BTreeMap;
10use core::fmt::Debug;
11use core::ops::Deref;
12use core::ops::{Add, Div, Mul, Sub};
13use irox_time::epoch::Timestamp;
14use irox_time::Duration;
15use irox_tools::debug_assert_eq_eps;
16use irox_tools::f64::FloatExt;
17
18pub trait KernelGenerator {
20 fn required_number_of_points(&self) -> usize;
23
24 fn absolute_value_offset(&self) -> usize {
27 (self.required_number_of_points() - 1) / 2
28 }
29 fn get_kernel_value(&self, offset: f64) -> f64;
33
34 fn expected_weighted_sum(&self) -> f64 {
38 1.0f64
39 }
40}
41pub struct SavitszkyGolaySmoother23 {
42 m: usize,
43}
44impl SavitszkyGolaySmoother23 {
45 pub const fn new(m: usize) -> Self {
46 Self { m }
47 }
48 pub const fn absolute_value_offset(&self) -> usize {
49 (self.m - 1) / 2
50 }
51 pub const fn get_kernel_value(&self, offset: f64) -> f64 {
52 let m = self.absolute_value_offset() as f64;
53 let msq = m * m;
54 let m2 = 2. * m;
55 let m3 = 3. * m;
56 let a = 3. * (3. * msq + m3 - 1. - 5. * offset * offset);
57 let b = (m2 + 3.) * (m2 + 1.) * (m2 - 1.);
58 a / b
59 }
60}
61impl KernelGenerator for SavitszkyGolaySmoother23 {
62 fn required_number_of_points(&self) -> usize {
63 self.m
64 }
65
66 fn get_kernel_value(&self, offset: f64) -> f64 {
67 SavitszkyGolaySmoother23::get_kernel_value(self, offset)
68 }
69}
70pub struct SavitszkyGolaySmoother24Builder;
71impl KernelBuilder for SavitszkyGolaySmoother24Builder {
72 type Output = SavitszkyGolaySmoother23;
73
74 fn generate_kernel(&self, num_samples: usize) -> Option<Self::Output> {
75 (num_samples >= self.minimum_samples()).then(|| SavitszkyGolaySmoother23::new(num_samples))
76 }
77
78 fn minimum_samples(&self) -> usize {
79 3
80 }
81}
82macro_rules! make_fn {
83 ($name:ident,$strukt:ident) => {
84 #[allow(clippy::indexing_slicing)]
85 const fn $name<const N: usize>() -> [f64; N] {
86 let m = ((N - 1) / 2) as i32;
87 let sv = $strukt::new(N);
88 let mut off = -m;
89 let mut out = [0.; N];
90 let mut idx = 0;
91 while idx < N {
92 out[idx] = sv.get_kernel_value(off as f64);
93 idx += 1;
94 off += 1;
95 }
96 out
97 }
98 };
99}
100make_fn!(make_savitskygolay_23, SavitszkyGolaySmoother23);
101pub const SAVITZKY_GOLAY_SMOOTH_23_5: [f64; 5] = make_savitskygolay_23::<5>();
102pub const SAVITZKY_GOLAY_SMOOTH_23_7: [f64; 7] = make_savitskygolay_23::<7>();
103pub const SAVITZKY_GOLAY_SMOOTH_23_9: [f64; 9] = make_savitskygolay_23::<9>();
104
105pub struct SavitszkyGolaySmoother45 {
106 m: usize,
107 denom: f64,
108 b: f64,
109}
110impl SavitszkyGolaySmoother45 {
111 pub const fn new(m: usize) -> Self {
112 let mf = ((m - 1) / 2) as f64;
113 let twom = mf * 2.;
114 let mf2 = mf * mf;
115 let mf3 = mf * mf2;
116 let mf4 = mf * mf3;
117 let denom = (twom + 5.) * (twom + 3.) * (twom + 1.) * (twom - 1.) * (twom - 3.);
118 let b = 15. * mf4 + 30. * mf3 - 35. * mf2 - 50. * mf + 12.;
119 Self { m, denom, b }
120 }
121 pub const fn absolute_value_offset(&self) -> usize {
122 (self.m - 1) / 2
123 }
124 pub const fn get_kernel_value(&self, offset: f64) -> f64 {
125 let m = self.absolute_value_offset() as f64;
126 let s2 = offset * offset;
127 let s4 = s2 * s2;
128 let twom = 2. * m;
129 let m2 = m * m;
130 let a = 15. / 4.;
131 let c = 35. * (2. * m2 + twom - 3.) * s2;
132 a * ((self.b - c + 63. * s4) / self.denom)
133 }
134}
135impl KernelGenerator for SavitszkyGolaySmoother45 {
136 fn required_number_of_points(&self) -> usize {
137 self.m
138 }
139 fn get_kernel_value(&self, offset: f64) -> f64 {
140 SavitszkyGolaySmoother45::get_kernel_value(self, offset)
141 }
142}
143make_fn!(make_savitskygolay_45, SavitszkyGolaySmoother45);
144pub const SAVITZKY_GOLAY_SMOOTH_45_5: [f64; 5] = make_savitskygolay_45::<5>();
145pub const SAVITZKY_GOLAY_SMOOTH_45_7: [f64; 7] = make_savitskygolay_45::<7>();
146pub const SAVITZKY_GOLAY_SMOOTH_45_9: [f64; 9] = make_savitskygolay_45::<9>();
147
148pub struct SavitskyGolay1DerivOrder2 {
149 m: usize,
150 denom: f64,
151}
152impl SavitskyGolay1DerivOrder2 {
153 pub const fn new(m: usize) -> Self {
154 let mf = ((m - 1) / 2) as f64;
155 let denom = (2. * mf + 1.) * (mf + 1.) * mf;
156 Self { m, denom }
157 }
158 pub const fn absolute_value_offset(&self) -> usize {
159 (self.m - 1) / 2
160 }
161 pub const fn get_kernel_value(&self, offset: f64) -> f64 {
162 (3. * offset) / self.denom
163 }
164}
165impl KernelGenerator for SavitskyGolay1DerivOrder2 {
166 fn required_number_of_points(&self) -> usize {
167 self.m
168 }
169 fn get_kernel_value(&self, offset: f64) -> f64 {
170 SavitskyGolay1DerivOrder2::get_kernel_value(self, offset)
171 }
172
173 fn expected_weighted_sum(&self) -> f64 {
174 0.0
175 }
176}
177make_fn!(make_savitskygolay_1d2, SavitskyGolay1DerivOrder2);
178pub const SAVITZKY_GOLAY_1D_2_5: [f64; 5] = make_savitskygolay_1d2::<5>();
179pub const SAVITZKY_GOLAY_1D_2_7: [f64; 7] = make_savitskygolay_1d2::<7>();
180pub const SAVITZKY_GOLAY_1D_2_9: [f64; 9] = make_savitskygolay_1d2::<9>();
181
182pub struct SavitzkyGolay1DerivOrder2Builder;
183impl KernelBuilder for SavitzkyGolay1DerivOrder2Builder {
184 type Output = SavitskyGolay1DerivOrder2;
185
186 fn generate_kernel(&self, num_samples: usize) -> Option<Self::Output> {
187 (num_samples >= self.minimum_samples()).then(|| SavitskyGolay1DerivOrder2::new(num_samples))
188 }
189
190 fn minimum_samples(&self) -> usize {
191 3
192 }
193}
194
195pub struct SavitzkyGolay1DerivOrder34 {
196 m: usize,
197 denom: f64,
198 a: f64,
199 b: f64,
200}
201impl SavitzkyGolay1DerivOrder34 {
202 pub const fn new(m: usize) -> Self {
203 let mf = ((m - 1) / 2) as f64;
204
205 let mut denom: f64 = 2. * mf + 3.;
206 denom *= 2. * mf + 1.;
207 denom *= 2. * mf - 1.;
208 denom *= mf + 2.;
209 denom *= mf + 1.;
210 denom *= mf;
211 denom *= mf - 1.;
212
213 let mf2 = mf * mf;
214 let mf3 = mf2 * mf;
215 let mf4 = mf3 * mf;
216
217 let a = 3. * mf4 + 6. * mf3 - 3. * mf + 1.;
218 let b = 3. * mf2 + 3. * mf - 1.;
219
220 Self { m, denom, a, b }
221 }
222
223 pub const fn absolute_value_offset(&self) -> usize {
224 (self.m - 1) / 2
225 }
226 pub const fn get_kernel_value(&self, offset: f64) -> f64 {
227 let a = 5. * self.a * offset;
228 let o2 = offset * offset;
229 let o3 = o2 * offset;
230 let b = 7. * self.b * o3;
231
232 let top = 5. * (a - b);
233
234 top / self.denom
235 }
236}
237impl KernelGenerator for SavitzkyGolay1DerivOrder34 {
238 fn required_number_of_points(&self) -> usize {
239 self.m
240 }
241
242 fn get_kernel_value(&self, offset: f64) -> f64 {
243 SavitzkyGolay1DerivOrder34::get_kernel_value(self, offset)
244 }
245
246 fn expected_weighted_sum(&self) -> f64 {
247 0.0
248 }
249}
250pub struct SavitzkyGolay1DerivOrder34Builder;
251impl KernelBuilder for SavitzkyGolay1DerivOrder34Builder {
252 type Output = SavitzkyGolay1DerivOrder34;
253
254 fn generate_kernel(&self, num_samples: usize) -> Option<Self::Output> {
255 (num_samples >= self.minimum_samples())
256 .then(|| SavitzkyGolay1DerivOrder34::new(num_samples))
257 }
258
259 fn minimum_samples(&self) -> usize {
260 3
261 }
262}
263make_fn!(make_savitskygolay_1d34, SavitzkyGolay1DerivOrder34);
264pub const SAVITZKY_GOLAY_1D_3_5: [f64; 5] = make_savitskygolay_1d34::<5>();
265pub const SAVITZKY_GOLAY_1D_3_7: [f64; 7] = make_savitskygolay_1d34::<7>();
266pub const SAVITZKY_GOLAY_1D_3_9: [f64; 9] = make_savitskygolay_1d34::<9>();
267
268pub struct TimeWindow<T> {
273 values: BTreeMap<Timestamp<T>, f64>,
274 window_duration: Duration,
275}
276impl<T: Copy> TimeWindow<T> {
277 pub fn new(window_duration: Duration) -> Self {
278 Self {
279 window_duration,
280 values: BTreeMap::<Timestamp<T>, f64>::new(),
281 }
282 }
283 pub fn insert(&mut self, timestamp: Timestamp<T>, value: f64) {
284 self.values.insert(timestamp, value);
285 let Some(last) = self.values.last_key_value() else {
286 return;
287 };
288 let window_start = last.0 - self.window_duration;
289 self.values = self.values.split_off(&window_start);
290 }
291 pub fn clear(&mut self) {
292 self.values.clear();
293 }
294
295 pub fn add_sample(&mut self, samp: Sample<T>) {
296 self.insert(samp.time, samp.value);
297 }
298
299 #[must_use]
301 pub fn first_key_value(&self) -> Option<(&Timestamp<T>, &f64)> {
302 self.values.first_key_value()
303 }
304 #[must_use]
306 pub fn last_key_value(&self) -> Option<(&Timestamp<T>, &f64)> {
307 self.values.last_key_value()
308 }
309
310 #[must_use]
312 pub fn len(&self) -> usize {
313 self.values.len()
314 }
315 #[must_use]
316 pub fn is_empty(&self) -> bool {
317 self.len() == 0
318 }
319
320 #[must_use]
322 pub fn data(&self) -> Vec<f64> {
323 self.values.values().copied().collect()
324 }
325
326 pub fn iter(&self) -> impl Iterator<Item = (&Timestamp<T>, &f64)> + Clone {
327 self.values.iter()
328 }
329
330 #[must_use]
331 pub fn map_data<V, F: Fn((&Timestamp<T>, &f64)) -> V>(&self, fun: F) -> Vec<V> {
332 let mut out = Vec::with_capacity(self.len());
333 for v in &self.values {
334 out.push(fun(v));
335 }
336 out
337 }
338}
339
340#[derive(Debug, Default, Copy, Clone, PartialEq, Eq)]
343pub enum WindowBinStrategy {
344 Lower,
346 #[default]
348 Center,
349 Upper,
351}
352pub trait KernelBuilder {
353 type Output: KernelGenerator;
354 fn generate_kernel(&self, num_samples: usize) -> Option<Self::Output>;
355 fn minimum_samples(&self) -> usize;
356}
357pub struct TimedWindowFilter<T, K: KernelGenerator> {
362 values: TimeWindow<T>,
363 window_duration: Duration,
364 bin_strategy: WindowBinStrategy,
365 kernel_generator: Box<dyn KernelBuilder<Output = K>>,
366}
367impl<T: Copy, K: KernelGenerator> TimedWindowFilter<T, K> {
368 pub fn new(
369 window_duration: Duration,
370 bin_strategy: WindowBinStrategy,
371 kernel_generator: Box<dyn KernelBuilder<Output = K>>,
372 ) -> Self {
373 Self {
374 window_duration,
375 bin_strategy,
376 kernel_generator,
377 values: TimeWindow::new(window_duration * 2.0),
378 }
379 }
380 pub fn add_sample(&mut self, sample: Sample<T>) -> Option<Sample<T>> {
381 self.insert(sample.time, sample.value)
382 }
383 pub fn insert(&mut self, time: Timestamp<T>, value: f64) -> Option<Sample<T>> {
387 self.values.insert(time, value);
388
389 if self.kernel_generator.minimum_samples() > self.values.len() {
390 return None;
392 }
393 let earliest = self.values.first_key_value()?;
394 let latest = self.values.last_key_value()?;
395 let stored_range = latest.0 - earliest.0;
396 if stored_range <= self.window_duration {
397 return None;
399 }
400 let numvals = self.values.len();
401 if numvals & 0x01 == 0x00 {
402 return None;
404 }
405 let last = *latest.0;
406 let window_start = last - self.window_duration;
407
408 let filter = self.kernel_generator.generate_kernel(numvals)?;
409
410 let center_time = window_start + self.window_duration / 2.;
411 let mut out = 0f64;
413 let mut tally = 0f64;
414 for (idx, (_time, val)) in self.values.iter().enumerate() {
415 let idx = idx as i32 - filter.absolute_value_offset() as i32;
416 let kernel = filter.get_kernel_value(idx as f64);
418 out += kernel * val;
419 tally += kernel;
420 }
421 let scale = 1.0 - (filter.expected_weighted_sum() - tally);
422 out /= scale;
423 debug_assert_eq_eps!(filter.expected_weighted_sum(), tally, 1e-15);
424 let out_time = match self.bin_strategy {
425 WindowBinStrategy::Lower => window_start,
426 WindowBinStrategy::Center => center_time,
427 WindowBinStrategy::Upper => last,
428 };
429 self.values.clear();
430 Some(Sample::new(out, out_time))
431 }
432}
433#[derive()]
437pub struct WindowBin<V, I, R> {
438 pub width: R,
439 pub start: I,
440 pub summary: Summary<V>,
441}
442impl<V: Default, I, R> WindowBin<V, I, R> {
443 pub fn new(width: R, start: I) -> Self {
444 Self {
445 width,
446 start,
447 summary: Summary::default(),
448 }
449 }
450}
451impl<
452 T: Sub<T, Output = T>
453 + PartialOrd
454 + Copy
455 + Default
456 + Div<f64, Output = T>
457 + Add<T, Output = T>
458 + Mul<f64, Output = T>
459 + Mul<T, Output = T>
460 + FloatExt<Type = T>,
461 I,
462 R,
463 > WindowBin<T, I, R>
464{
465 pub fn insert(&mut self, value: T) {
466 self.summary.add_sample(value);
467 }
468}
469impl<V, I, R> Deref for WindowBin<V, I, R> {
470 type Target = Summary<V>;
471 fn deref(&self) -> &Self::Target {
472 &self.summary
473 }
474}
475pub struct BinStatistics<V, I, R> {
479 pub bin_width: R,
480 pub bins: BTreeMap<i64, WindowBin<V, I, R>>,
481 pub anchor: Option<I>,
482}
483
484impl<T: Copy> BinStatistics<f64, Timestamp<T>, Duration> {
485 pub fn new(bin_width: Duration) -> Self {
486 Self {
487 bin_width,
488 bins: Default::default(),
489 anchor: None,
490 }
491 }
492 fn bindex(&mut self, timestamp: Timestamp<T>) -> i64 {
493 let anchor = *self.anchor.get_or_insert(timestamp);
494 ((timestamp - anchor) / self.bin_width).round() as i64
495 }
496 pub fn insert(
500 &mut self,
501 timestamp: Timestamp<T>,
502 value: f64,
503 ) -> &WindowBin<f64, Timestamp<T>, Duration> {
504 let bin_index = self.bindex(timestamp);
505 let bin = self.bins.entry(bin_index).or_insert_with(|| {
506 let anchor = *self.anchor.get_or_insert(timestamp);
507 let start = anchor + bin_index as f64 * self.bin_width;
508 WindowBin::new(self.bin_width, start)
509 });
510 bin.insert(value);
511 bin
512 }
513 pub fn remove_data_before(&mut self, timestamp: Timestamp<T>) {
516 let bin_index = self.bindex(timestamp) - 1;
517 self.bins = self.bins.split_off(&bin_index);
518 }
519 pub fn len(&self) -> usize {
520 self.bins.len()
521 }
522 pub fn is_empty(&self) -> bool {
523 self.bins.is_empty()
524 }
525 pub fn iter(&self) -> impl Iterator<Item = (&i64, &WindowBin<f64, Timestamp<T>, Duration>)> {
526 self.bins.iter()
527 }
528}
529
530pub struct TimedLinearSlopeFilter<T> {
531 values: TimeWindow<T>,
532 window_duration: Duration,
533 bin_strategy: WindowBinStrategy,
534}
535impl<T: Copy> TimedLinearSlopeFilter<T> {
536 pub fn new(window_duration: Duration, bin_strategy: WindowBinStrategy) -> Self {
537 Self {
538 window_duration,
539 bin_strategy,
540 values: TimeWindow::new(window_duration),
541 }
542 }
543 pub fn add_sample(&mut self, sample: Sample<T>) -> Option<Sample<T>> {
544 self.insert(sample.time, sample.value)
545 }
546 pub fn insert(&mut self, time: Timestamp<T>, value: f64) -> Option<Sample<T>> {
550 self.values.insert(time, value);
551
552 let earliest = self.values.first_key_value()?;
553 let latest = self.values.last_key_value()?;
554 let last = *latest.0;
555 let stored_range = latest.0 - earliest.0;
556 let window_start = last - self.window_duration;
557 let center_time = window_start + self.window_duration / 2.;
558 if stored_range < (self.window_duration * 0.95) {
559 return None;
561 }
562
563 let reg = LinearRegression::from_data(
564 self.values.iter(),
565 |(t, _v)| t.get_offset().value(),
566 |(_t, v)| **v,
567 )?;
568 let out = reg.slope;
569 let out_time = match self.bin_strategy {
570 WindowBinStrategy::Lower => window_start,
571 WindowBinStrategy::Center => center_time,
572 WindowBinStrategy::Upper => last,
573 };
574 self.values.clear();
575 Some(Sample::new(out, out_time))
576 }
577}
578
579#[cfg(test)]
580mod tests {
581 use crate::windows::*;
582 use irox_tools::{assert_eq_eps, assert_eq_eps_slice};
583
584 #[test]
585 pub fn test_savitz23() {
586 let sv = SavitszkyGolaySmoother23::new(9);
587 assert_eq!(9, sv.required_number_of_points());
588 assert_eq!(4, sv.absolute_value_offset());
589
590 let values = [
591 -21. / 231.,
592 14. / 231.,
593 39. / 231.,
594 54. / 231.,
595 59. / 231.,
596 54. / 231.,
597 39. / 231.,
598 14. / 231.,
599 -21. / 231.,
600 ];
601
602 for (idx, v) in (-4..4).enumerate() {
603 assert_eq_eps!(values[idx], sv.get_kernel_value(v as f64), f64::EPSILON);
604 }
605 assert_eq_eps_slice!(values, SAVITZKY_GOLAY_SMOOTH_23_9, f64::EPSILON);
606 assert_eq_eps!(
607 1.0,
608 SAVITZKY_GOLAY_SMOOTH_23_9.iter().sum::<f64>(),
609 f64::EPSILON
610 );
611 }
612
613 #[test]
614 pub fn test_savitz45() {
615 let sv = SavitszkyGolaySmoother45::new(9);
616 assert_eq!(9, sv.required_number_of_points());
617 assert_eq!(4, sv.absolute_value_offset());
618
619 let values = [
620 15. / 429.,
621 -55. / 429.,
622 30. / 429.,
623 135. / 429.,
624 179. / 429.,
625 135. / 429.,
626 30. / 429.,
627 -55. / 429.,
628 15. / 429.,
629 ];
630
631 for (idx, v) in (-4..4).enumerate() {
632 assert_eq_eps!(values[idx], sv.get_kernel_value(v as f64), 1e-15);
633 }
634 assert_eq_eps_slice!(values, SAVITZKY_GOLAY_SMOOTH_45_9, f64::EPSILON);
635 assert_eq_eps!(
636 1.0,
637 SAVITZKY_GOLAY_SMOOTH_45_9.iter().sum::<f64>(),
638 f64::EPSILON
639 );
640 }
641
642 #[test]
643 pub fn test_savitz_1d2() {
644 let sv = SavitskyGolay1DerivOrder2::new(9);
645 assert_eq!(9, sv.required_number_of_points());
646 assert_eq!(4, sv.absolute_value_offset());
647
648 let values = [
649 -4. / 60.,
650 -3. / 60.,
651 -2. / 60.,
652 -1. / 60.,
653 0.,
654 1. / 60.,
655 2. / 60.,
656 3. / 60.,
657 4. / 60.,
658 ];
659
660 for (idx, v) in (-4..4).enumerate() {
661 assert_eq_eps!(values[idx], sv.get_kernel_value(v as f64), 1e-15);
662 }
663 assert_eq_eps_slice!(values, SAVITZKY_GOLAY_1D_2_9, f64::EPSILON);
664 assert_eq_eps!(0.0, SAVITZKY_GOLAY_1D_2_9.iter().sum::<f64>(), f64::EPSILON);
665 }
666
667 #[test]
668 pub fn test_savitz_1d34() {
669 let sv = SavitzkyGolay1DerivOrder34::new(9);
670 assert_eq!(9, sv.required_number_of_points());
671 assert_eq!(4, sv.absolute_value_offset());
672
673 let values = [
674 86. / 1188.,
675 -142. / 1188.,
676 -193. / 1188.,
677 -126. / 1188.,
678 0.,
679 126. / 1188.,
680 193. / 1188.,
681 142. / 1188.,
682 -86. / 1188.,
683 ];
684
685 for (idx, v) in (-4..=4).enumerate() {
686 assert_eq_eps!(values[idx], sv.get_kernel_value(v as f64), 1e-15);
687 }
688 assert_eq_eps_slice!(values, SAVITZKY_GOLAY_1D_3_9, f64::EPSILON);
689 assert_eq_eps!(0.0, SAVITZKY_GOLAY_1D_3_9.iter().sum::<f64>(), f64::EPSILON);
690 }
691}