1use core::ops::AddAssign;
2
3use num_traits::{AsPrimitive, Num, Pow, WrappingAdd, WrappingSub};
4
5#[derive(Clone, Debug)]
11pub struct Cic<T, const N: usize, const M: usize = 1> {
12 rate: u32,
16 index: u32,
18 zoh: T,
22 combs: [[T; M]; N],
24 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 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 pub const fn order(&self) -> usize {
56 N
57 }
58
59 pub const fn comb_delay(&self) -> usize {
61 M
62 }
63
64 pub const fn rate(&self) -> u32 {
68 self.rate
69 }
70
71 pub fn set_rate(&mut self, rate: u32) {
75 self.rate = rate;
76 }
77
78 pub fn clear(&mut self) {
80 *self = Self::new(self.rate);
81 }
82
83 pub const fn tick(&self) -> bool {
88 self.index == 0
89 }
90
91 pub fn get_interpolate(&self) -> T {
93 self.integrators.last().copied().unwrap_or(self.zoh)
94 }
95
96 pub fn get_decimate(&self) -> T {
98 self.zoh
99 }
100
101 pub fn gain(&self) -> T {
103 (M.as_() * (self.rate.as_() + T::one())).pow(N)
104 }
105
106 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 pub const fn response_length(&self) -> usize {
116 self.rate as usize * N
117 }
118
119 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 pub fn settle_decimate(&mut self, x: T) {
137 self.clear();
138 self.zoh = x * self.gain();
139 unimplemented!();
140 }
141
142 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 *i += x;
162 *i
163 })
164 }
165
166 pub fn decimate(&mut self, x: T) -> Option<T> {
168 let x = self.integrators.iter_mut().fold(x, |x, i| {
169 *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 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 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 }
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}