idsp/
cic.rs

1use core::ops::AddAssign;
2
3use num_traits::{AsPrimitive, Num, Pow, WrappingAdd, WrappingSub};
4
5/// Cascaded integrator comb structure
6///
7/// Order `N` where `N = 3` is cubic.
8/// Comb delay `M` where `M = 1` is typical.
9/// Use `rate=0` and some larger `M` to implemnent a unit-rate lowpass.
10#[derive(Clone, Debug)]
11pub struct Cic<T, const N: usize, const M: usize = 1> {
12    /// Rate change (fast/slow - 1)
13    /// Interpolator: output/input - 1
14    /// Decimator: input/output - 1
15    rate: u32,
16    /// Up/downsampler state (count down)
17    index: u32,
18    /// Zero order hold behind comb sections.
19    /// Interpolator: Combined with the upsampler
20    /// Decimator: To support `get_decimate()`
21    zoh: T,
22    /// Comb/differentiator state
23    combs: [[T; M]; N],
24    /// Integrator state
25    integrators: [T; N],
26}
27
28impl<T, const N: usize, const M: usize> Cic<T, N, M>
29where
30    T: Num + AddAssign + WrappingAdd + WrappingSub + Pow<usize, Output = T> + Copy + 'static,
31    u32: AsPrimitive<T>,
32    usize: AsPrimitive<T>,
33{
34    const _M: () = assert!(M > 0, "Comb delay must be non-zero");
35
36    /// Create a new zero-initialized filter with the given rate change.
37    pub fn new(rate: u32) -> Self {
38        Self {
39            rate,
40            index: 0,
41            zoh: T::zero(),
42            combs: [[T::zero(); M]; N],
43            integrators: [T::zero(); N],
44        }
45    }
46
47    /// Filter order
48    ///
49    /// * 0: zero order hold
50    /// * 1: linear
51    /// * 2: quadratic
52    /// * 3: cubic interpolation/decimation
53    ///
54    /// etc.
55    pub const fn order(&self) -> usize {
56        N
57    }
58
59    /// Comb delay
60    pub const fn comb_delay(&self) -> usize {
61        M
62    }
63
64    /// Rate change
65    ///
66    /// `fast/slow - 1`
67    pub const fn rate(&self) -> u32 {
68        self.rate
69    }
70
71    /// Set the rate change
72    ///
73    /// `fast/slow - 1`
74    pub fn set_rate(&mut self, rate: u32) {
75        self.rate = rate;
76    }
77
78    /// Zero-initialize the filter state
79    pub fn clear(&mut self) {
80        *self = Self::new(self.rate);
81    }
82
83    /// Accepts/provides new slow-rate sample
84    ///
85    /// Interpolator: accepts new input sample
86    /// Decimator: returns new output sample
87    pub const fn tick(&self) -> bool {
88        self.index == 0
89    }
90
91    /// Current interpolator output
92    pub fn get_interpolate(&self) -> T {
93        self.integrators.last().copied().unwrap_or(self.zoh)
94    }
95
96    /// Current decimator output
97    pub fn get_decimate(&self) -> T {
98        self.zoh
99    }
100
101    /// Filter gain
102    pub fn gain(&self) -> T {
103        (M.as_() * (self.rate.as_() + T::one())).pow(N)
104    }
105
106    /// Right shift amount
107    ///
108    /// `log2(gain())` if gain is a power of two,
109    /// otherwise an upper bound.
110    pub const fn gain_log2(&self) -> u32 {
111        (u32::BITS - (M as u32 * self.rate + (M - 1) as u32).leading_zeros()) * N as u32
112    }
113
114    /// Impulse response length
115    pub const fn response_length(&self) -> usize {
116        self.rate as usize * N
117    }
118
119    /// Establish a settled filter state
120    pub fn settle_interpolate(&mut self, x: T) {
121        self.clear();
122        if let Some(c) = self.combs.first_mut() {
123            *c = [x; M];
124        } else {
125            self.zoh = x;
126        }
127        let g = self.gain();
128        if let Some(i) = self.integrators.last_mut() {
129            *i = x * g;
130        }
131    }
132
133    /// Establish a settled filter state
134    ///
135    /// Unimplemented!
136    pub fn settle_decimate(&mut self, x: T) {
137        self.clear();
138        self.zoh = x * self.gain();
139        unimplemented!();
140    }
141
142    /// Optionally ingest a new low-rate sample and
143    /// retrieve the next output.
144    ///
145    /// A new sample must be supplied at the correct time (when [`Cic::tick()`] is true)
146    pub fn interpolate(&mut self, x: Option<T>) -> T {
147        if let Some(x) = x {
148            debug_assert_eq!(self.index, 0);
149            self.index = self.rate;
150            self.zoh = self.combs.iter_mut().fold(x, |x, c| {
151                let y = x - c[0];
152                c.copy_within(1.., 0);
153                c[M - 1] = x;
154                y
155            });
156        } else {
157            self.index -= 1;
158        }
159        self.integrators.iter_mut().fold(self.zoh, |x, i| {
160            // Overflow is not OK
161            *i += x;
162            *i
163        })
164    }
165
166    /// Ingest a new high-rate sample and optionally retrieve next output.
167    pub fn decimate(&mut self, x: T) -> Option<T> {
168        let x = self.integrators.iter_mut().fold(x, |x, i| {
169            // Overflow is OK if bitwidth is sufficient (input * gain)
170            *i = i.wrapping_add(&x);
171            *i
172        });
173        if let Some(index) = self.index.checked_sub(1) {
174            self.index = index;
175            None
176        } else {
177            self.index = self.rate;
178            self.zoh = self.combs.iter_mut().fold(x, |x, c| {
179                // Overflows expected
180                let y = x.wrapping_sub(&c[0]);
181                c.copy_within(1.., 0);
182                c[M - 1] = x;
183                y
184            });
185            Some(self.zoh)
186        }
187    }
188}
189
190#[cfg(test)]
191mod test {
192    use core::cmp::Ordering;
193
194    use super::*;
195    use quickcheck_macros::quickcheck;
196
197    #[quickcheck]
198    fn new(rate: u32) {
199        let _ = Cic::<i64, 3>::new(rate);
200    }
201
202    #[quickcheck]
203    fn identity_dec(x: Vec<i64>) {
204        let mut dec = Cic::<_, 3>::new(0);
205        for x in x {
206            assert_eq!(x, dec.decimate(x).unwrap());
207            assert_eq!(x, dec.get_decimate());
208        }
209    }
210
211    #[quickcheck]
212    fn identity_int(x: Vec<i64>) {
213        const N: usize = 3;
214        let mut int = Cic::<_, N>::new(0);
215        for x in x {
216            assert_eq!(x >> N, int.interpolate(Some(x >> N)));
217            assert_eq!(x >> N, int.get_interpolate());
218        }
219    }
220
221    #[quickcheck]
222    fn response_length_gain_settle(x: Vec<i32>, rate: u32) {
223        let mut int = Cic::<_, 3>::new(rate);
224        let shift = int.gain_log2();
225        if shift >= 32 {
226            return;
227        }
228        assert!(int.gain() <= 1 << shift);
229        for x in x {
230            while !int.tick() {
231                int.interpolate(None);
232            }
233            let y_last = int.get_interpolate();
234            let y_want = x as i64 * int.gain();
235            for i in 0..2 * int.response_length() {
236                let y = int.interpolate(if int.tick() { Some(x as i64) } else { None });
237                assert_eq!(y, int.get_interpolate());
238                if i < int.response_length() {
239                    match y_want.cmp(&y_last) {
240                        Ordering::Greater => assert!((y_last..y_want).contains(&y)),
241                        Ordering::Less => assert!((y_want..y_last).contains(&(y - 1))),
242                        Ordering::Equal => assert_eq!(y_want, y),
243                    }
244                } else {
245                    assert_eq!(y, y_want);
246                }
247            }
248        }
249    }
250
251    #[quickcheck]
252    fn settle(rate: u32, x: i32) {
253        let mut int = Cic::<i64, 3>::new(rate);
254        if int.gain_log2() >= 32 {
255            return;
256        }
257        int.settle_interpolate(x as _);
258        // let mut dec = Cic::<i64, 3>::new(rate);
259        // dec.settle_decimate(x as _);
260        for _ in 0..100 {
261            let y = int.interpolate(if int.tick() { Some(x as _) } else { None });
262            assert_eq!(y, x as i64 * int.gain());
263            assert_eq!(y, int.get_interpolate());
264            // assert_eq!(dec.get_decimate(), x as i64 * dec.gain());
265            // if let Some(y) = dec.decimate(x as _) {
266            //     assert_eq!(y, x as i64 * dec.gain());
267            // }
268        }
269    }
270
271    #[quickcheck]
272    fn unit_rate(x: (i32, i32, i32, i32, i32)) {
273        let x: [i32; 5] = x.into();
274        let mut cic = Cic::<i64, 3, 3>::new(0);
275        assert!(cic.gain_log2() == 6);
276        assert!(cic.gain() == (cic.comb_delay() as i64).pow(cic.order() as _));
277        for x in x {
278            assert!(cic.tick());
279            let y = cic.decimate(x as _).unwrap();
280            println!("{x:11} {y:11}");
281        }
282        for _ in 0..100 {
283            let y = cic.decimate(0 as _).unwrap();
284            assert_eq!(y, cic.get_decimate());
285            println!("{y:11}");
286            println!("");
287        }
288    }
289}