1use bincode::{Decode, Encode};
2use std::{array, iter};
3
4#[derive(Debug, Clone, Encode, Decode)]
5pub struct IirFilter<const N: usize> {
6    b0: f64,
7    b: [f64; N],
8    a: [f64; N],
9    prev_samples: [f64; N],
10    prev_outputs: [f64; N],
11    tiny_offset: f64,
12}
13
14impl<const N: usize> IirFilter<N> {
15    /// # Panics
16    ///
17    /// Will panic if the const generic `N` is zero, or if either of the coefficients arrays is not
18    /// length `N + 1`.
19    #[must_use]
20    pub fn new(b: &[f64], a: &[f64]) -> Self {
21        assert!(N > 0);
22        assert_eq!(b.len(), N + 1);
23        assert_eq!(a.len(), N + 1);
24
25        Self {
26            b0: b[0],
27            b: b[1..].try_into().unwrap(),
28            a: a[1..].try_into().unwrap(),
29            prev_samples: array::from_fn(|_| 0.0),
30            prev_outputs: array::from_fn(|_| 0.0),
31            tiny_offset: 1e-30,
32        }
33    }
34
35    /// Returns a filter that simply returns input samples as-is
36    #[must_use]
37    pub fn identity() -> Self {
38        Self {
39            b0: 1.0,
40            b: array::from_fn(|_| 0.0),
41            a: array::from_fn(|_| 0.0),
42            prev_samples: array::from_fn(|_| 0.0),
43            prev_outputs: array::from_fn(|_| 0.0),
44            tiny_offset: 0.0,
45        }
46    }
47
48    #[must_use]
49    pub fn filter(&mut self, sample: f64) -> f64 {
50        // Hack to avoid the filter getting stuck at a subnormal value
51        // See <https://www.earlevel.com/main/2019/04/19/floating-point-denormals/>
52        let sample = sample + self.tiny_offset;
53        self.tiny_offset = -self.tiny_offset;
54
55        let output = self.b0 * sample
56            + iter::zip(&self.b, &self.prev_samples).map(|(&coeff, &n)| coeff * n).sum::<f64>()
57            - iter::zip(&self.a, &self.prev_outputs).map(|(&coeff, &n)| coeff * n).sum::<f64>();
58
59        for i in (1..N).rev() {
60            self.prev_samples[i] = self.prev_samples[i - 1];
61            self.prev_outputs[i] = self.prev_outputs[i - 1];
62        }
63        self.prev_samples[0] = sample;
64        self.prev_outputs[0] = output;
65
66        output
67    }
68
69    pub fn reset(&mut self) {
70        self.prev_samples.fill(0.0);
71        self.prev_outputs.fill(0.0);
72    }
73}
74
75pub type FirstOrderIirFilter = IirFilter<1>;
76pub type SecondOrderIirFilter = IirFilter<2>;