1use core::{
37 iter::Sum,
38 ops::{Add, Mul, Sub},
39 slice::{from_mut, from_ref},
40};
41
42use dsp_process::{ChunkIn, ChunkOut, Major, SplitProcess};
43
44#[inline]
46fn get<C: Copy, T, const M: usize, const ODD: bool, const SYM: bool>(
47 c: &[C; M],
48 x: &[T],
49) -> impl Iterator<Item = T>
50where
51 T: Copy + Add<Output = T> + Sub<Output = T> + Mul<C, Output = T> + Sum,
52{
53 x.windows(2 * M + ODD as usize).map(|x| {
55 let Some((old, new)) = x.first_chunk::<M>().zip(x.last_chunk::<M>()) else {
56 unreachable!()
57 };
58 let xc = old
61 .iter()
62 .zip(new.iter().rev())
63 .zip(c.iter())
64 .map(|((old, new), tap)| (if SYM { *new + *old } else { *new - *old }) * *tap)
65 .sum();
66 if ODD { xc + x[M] } else { xc }
67 })
68}
69
70macro_rules! type_fir {
71 ($name:ident, $odd:literal, $sym:literal) => {
72 #[doc = concat!("Linear phase FIR taps for odd = ", stringify!($odd), " and symmetric = ", stringify!($sym))]
73 #[derive(Clone, Copy, Debug, Default)]
74 #[repr(transparent)]
75 pub struct $name<C>(pub C);
76 impl<T, const M: usize> $name<[T; M]> {
77 pub const LEN: usize = 2 * M - 1 + $odd as usize;
79 }
80
81 impl<
82 C: Copy,
83 T: Copy + Default + Sub<T, Output = T> + Add<Output = T> + Mul<C, Output = T> + Sum,
84 const M: usize,
85 const N: usize,
86 > SplitProcess<T, T, [T; N]> for $name<[C; M]>
87 {
88 fn block(&self, state: &mut [T; N], x: &[T], y: &mut [T]) {
89 for (x, y) in x.chunks(N - Self::LEN).zip(y.chunks_mut(N - Self::LEN)) {
90 state[Self::LEN..][..x.len()].copy_from_slice(x);
91 for (y, x) in y.iter_mut().zip(get::<_, _, _, $odd, $sym>(&self.0, state)) {
92 *y = x;
93 }
94 state.copy_within(x.len()..x.len() + Self::LEN, 0);
95 }
96 }
97
98 fn process(&self, state: &mut [T; N], x: T) -> T {
99 let mut y = T::default();
100 self.block(state, from_ref(&x), from_mut(&mut y));
101 y
102 }
103 }
104 };
105}
106type_fir!(OddSymmetric, true, true);
109type_fir!(EvenSymmetric, false, true);
111type_fir!(OddAntiSymmetric, true, false);
114type_fir!(EvenAntiSymmetric, false, false);
116
117#[derive(Clone, Debug)]
119pub struct HbfDec<T> {
120 even: T, odd: T, }
123
124impl<T: Copy + Default, const N: usize> Default for HbfDec<[T; N]> {
125 fn default() -> Self {
126 Self {
127 even: [T::default(); _],
128 odd: [T::default(); _],
129 }
130 }
131}
132
133impl<
134 C: Copy,
135 T: Copy + Default + Add<Output = T> + Sub<Output = T> + Mul<C, Output = T> + Sum,
136 const M: usize,
137 const N: usize,
138> SplitProcess<[T; 2], T, HbfDec<[T; N]>> for EvenSymmetric<[C; M]>
139{
140 fn block(&self, state: &mut HbfDec<[T; N]>, x: &[[T; 2]], y: &mut [T]) {
141 debug_assert_eq!(x.len(), y.len());
142 const { assert!(N > Self::LEN) }
143 for (x, y) in x.chunks(N - Self::LEN).zip(y.chunks_mut(N - Self::LEN)) {
144 for (x, (even, odd)) in x.iter().zip(
146 state.even[M - 1..]
147 .iter_mut()
148 .zip(state.odd[Self::LEN..].iter_mut()),
149 ) {
150 *even = x[0];
151 *odd = x[1];
152 }
153 let odd = get::<_, _, _, false, true>(&self.0, &state.odd);
155 for (y, (odd, even)) in y.iter_mut().zip(odd.zip(state.even.iter().copied())) {
156 *y = odd + even;
157 }
158 state.even.copy_within(x.len()..x.len() + M - 1, 0);
160 state.odd.copy_within(x.len()..x.len() + Self::LEN, 0);
161 }
162 }
163
164 fn process(&self, state: &mut HbfDec<[T; N]>, x: [T; 2]) -> T {
165 let mut y = Default::default();
166 self.block(state, from_ref(&x), from_mut(&mut y));
167 y
168 }
169}
170
171#[derive(Clone, Debug)]
173pub struct HbfInt<T> {
174 x: T, }
176
177impl<T: Default + Copy, const N: usize> Default for HbfInt<[T; N]> {
178 fn default() -> Self {
179 Self {
180 x: [T::default(); _],
181 }
182 }
183}
184
185impl<
186 C: Copy,
187 T: Copy + Default + Add<Output = T> + Sub<Output = T> + Mul<C, Output = T> + Sum,
188 const M: usize,
189 const N: usize,
190> SplitProcess<T, [T; 2], HbfInt<[T; N]>> for EvenSymmetric<[C; M]>
191{
192 fn block(&self, state: &mut HbfInt<[T; N]>, x: &[T], y: &mut [[T; 2]]) {
193 debug_assert_eq!(x.len(), y.len());
194 const { assert!(N > Self::LEN) }
195 for (x, y) in x.chunks(N - Self::LEN).zip(y.chunks_mut(N - Self::LEN)) {
196 state.x[Self::LEN..][..x.len()].copy_from_slice(x);
198 let odd = get::<_, _, _, false, true>(&self.0, &state.x);
200 for (y, (even, odd)) in y.iter_mut().zip(odd.zip(state.x[M..].iter().copied())) {
201 *y = [even, odd]; }
203 state.x.copy_within(x.len()..x.len() + Self::LEN, 0);
205 }
206 }
207
208 fn process(&self, state: &mut HbfInt<[T; N]>, x: T) -> [T; 2] {
209 let mut y = Default::default();
210 self.block(state, from_ref(&x), from_mut(&mut y));
211 y
212 }
213}
214
215type HbfTaps98 = (
217 EvenSymmetric<[f32; 15]>,
218 EvenSymmetric<[f32; 6]>,
219 EvenSymmetric<[f32; 3]>,
220 EvenSymmetric<[f32; 3]>,
221 EvenSymmetric<[f32; 2]>,
222);
223
224#[allow(clippy::excessive_precision)]
235pub const HBF_TAPS_98: HbfTaps98 = (
236 EvenSymmetric([
238 7.02144012e-05,
239 -2.43279582e-04,
240 6.35026936e-04,
241 -1.39782541e-03,
242 2.74613582e-03,
243 -4.96403839e-03,
244 8.41806912e-03,
245 -1.35827601e-02,
246 2.11004053e-02,
247 -3.19267647e-02,
248 4.77024289e-02,
249 -7.18014345e-02,
250 1.12942004e-01,
251 -2.03279594e-01,
252 6.33592923e-01,
253 ]),
254 EvenSymmetric([
256 -0.00086943,
257 0.00577837,
258 -0.02201674,
259 0.06357869,
260 -0.16627679,
261 0.61979312,
262 ]),
263 EvenSymmetric([0.01414651, -0.10439639, 0.59026742]),
265 EvenSymmetric([0.01227974, -0.09930782, 0.58702834]),
267 EvenSymmetric([-0.06291796, 0.5629161]),
269);
270
271type HbfTaps = (
273 EvenSymmetric<[f32; 23]>,
274 EvenSymmetric<[f32; 10]>,
275 EvenSymmetric<[f32; 5]>,
276 EvenSymmetric<[f32; 4]>,
277 EvenSymmetric<[f32; 3]>,
278);
279
280#[allow(clippy::excessive_precision)]
285pub const HBF_TAPS: HbfTaps = (
286 EvenSymmetric([
287 7.60375795e-07,
288 -3.77494111e-06,
289 1.26458559e-05,
290 -3.43188253e-05,
291 8.10687478e-05,
292 -1.72971467e-04,
293 3.40845059e-04,
294 -6.29522864e-04,
295 1.10128831e-03,
296 -1.83933299e-03,
297 2.95124926e-03,
298 -4.57290964e-03,
299 6.87374176e-03,
300 -1.00656257e-02,
301 1.44199840e-02,
302 -2.03025100e-02,
303 2.82462332e-02,
304 -3.91128509e-02,
305 5.44795658e-02,
306 -7.77002672e-02,
307 1.17523452e-01,
308 -2.06185388e-01,
309 6.34588695e-01,
310 ]),
311 EvenSymmetric([
312 -1.12811343e-05,
313 1.12724671e-04,
314 -6.07439343e-04,
315 2.31904511e-03,
316 -7.00322950e-03,
317 1.78225473e-02,
318 -4.01209836e-02,
319 8.43315989e-02,
320 -1.83189521e-01,
321 6.26346521e-01,
322 ]),
323 EvenSymmetric([0.0007686, -0.00768669, 0.0386536, -0.14002434, 0.60828885]),
324 EvenSymmetric([-0.00261331, 0.02476858, -0.12112638, 0.59897111]),
325 EvenSymmetric([0.01186105, -0.09808109, 0.58622005]),
326);
327
328pub const HBF_PASSBAND: f32 = 0.4;
330
331pub const HBF_CASCADE_BLOCK: usize = 1 << 5;
335
336pub type HbfDec2 =
341 HbfDec<[f32; EvenSymmetric::<[f32; HBF_TAPS.0.0.len()]>::LEN + HBF_CASCADE_BLOCK]>;
342pub type HbfDec4 = (
344 HbfDec<[f32; EvenSymmetric::<[f32; HBF_TAPS.1.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 1)]>,
345 HbfDec2,
346);
347pub type HbfDec8 = (
349 HbfDec<[f32; EvenSymmetric::<[f32; HBF_TAPS.2.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 2)]>,
350 HbfDec4,
351);
352pub type HbfDec16 = (
354 HbfDec<[f32; EvenSymmetric::<[f32; HBF_TAPS.3.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 3)]>,
355 HbfDec8,
356);
357pub type HbfDec32 = (
359 HbfDec<[f32; EvenSymmetric::<[f32; HBF_TAPS.4.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 4)]>,
360 HbfDec16,
361);
362
363type HbfDecCascade<const B: usize = HBF_CASCADE_BLOCK> = Major<
364 (
365 ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.4.0.len()]>, 2>,
366 Major<
367 (
368 ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.3.0.len()]>, 2>,
369 Major<
370 (
371 ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.2.0.len()]>, 2>,
372 Major<
373 (
374 ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.1.0.len()]>, 2>,
375 &'static EvenSymmetric<[f32; HBF_TAPS.0.0.len()]>,
376 ),
377 [[f32; 2]; B],
378 >,
379 ),
380 [[f32; 4]; B],
381 >,
382 ),
383 [[f32; 8]; B],
384 >,
385 ),
386 [[f32; 16]; B],
387>;
388
389pub const HBF_DEC_CASCADE: HbfDecCascade = Major::new((
391 ChunkIn(&HBF_TAPS.4),
392 Major::new((
393 ChunkIn(&HBF_TAPS.3),
394 Major::new((
395 ChunkIn(&HBF_TAPS.2),
396 Major::new((ChunkIn(&HBF_TAPS.1), &HBF_TAPS.0)),
397 )),
398 )),
399));
400
401pub const fn hbf_dec_response_length(depth: usize) -> usize {
403 assert!(depth < 5);
404 let mut n = 0;
405 if depth > 0 {
406 n /= 2;
407 n += EvenSymmetric::<[f32; HBF_TAPS.3.0.len()]>::LEN;
408 }
409 if depth > 1 {
410 n /= 2;
411 n += EvenSymmetric::<[f32; HBF_TAPS.2.0.len()]>::LEN;
412 }
413 if depth > 2 {
414 n /= 2;
415 n += EvenSymmetric::<[f32; HBF_TAPS.1.0.len()]>::LEN;
416 }
417 if depth > 3 {
418 n /= 2;
419 n += EvenSymmetric::<[f32; HBF_TAPS.0.0.len()]>::LEN;
420 }
421 n
422}
423
424pub type HbfInt2 =
429 HbfInt<[f32; EvenSymmetric::<[f32; HBF_TAPS.0.0.len()]>::LEN + HBF_CASCADE_BLOCK]>;
430pub type HbfInt4 = (
432 HbfInt2,
433 HbfInt<[f32; EvenSymmetric::<[f32; HBF_TAPS.1.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 1)]>,
434);
435pub type HbfInt8 = (
437 HbfInt4,
438 HbfInt<[f32; EvenSymmetric::<[f32; HBF_TAPS.2.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 2)]>,
439);
440pub type HbfInt16 = (
442 HbfInt8,
443 HbfInt<[f32; EvenSymmetric::<[f32; HBF_TAPS.3.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 3)]>,
444);
445pub type HbfInt32 = (
447 HbfInt16,
448 HbfInt<[f32; EvenSymmetric::<[f32; HBF_TAPS.4.0.len()]>::LEN + (HBF_CASCADE_BLOCK << 4)]>,
449);
450
451type HbfIntCascade<const B: usize = HBF_CASCADE_BLOCK> = Major<
452 (
453 Major<
454 (
455 Major<
456 (
457 Major<
458 (
459 &'static EvenSymmetric<[f32; HBF_TAPS.0.0.len()]>,
460 ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.1.0.len()]>, 2>,
461 ),
462 [[f32; 2]; B],
463 >,
464 ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.2.0.len()]>, 2>,
465 ),
466 [[f32; 4]; B],
467 >,
468 ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.3.0.len()]>, 2>,
469 ),
470 [[f32; 8]; B],
471 >,
472 ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.4.0.len()]>, 2>,
473 ),
474 [[f32; 16]; B],
475>;
476
477pub const HBF_INT_CASCADE: HbfIntCascade = Major::new((
479 Major::new((
480 Major::new((
481 Major::new((&HBF_TAPS.0, ChunkOut(&HBF_TAPS.1))),
482 ChunkOut(&HBF_TAPS.2),
483 )),
484 ChunkOut(&HBF_TAPS.3),
485 )),
486 ChunkOut(&HBF_TAPS.4),
487));
488
489pub const fn hbf_int_response_length(depth: usize) -> usize {
491 assert!(depth < 5);
492 let mut n = 0;
493 if depth > 0 {
494 n += EvenSymmetric::<[f32; HBF_TAPS.0.0.len()]>::LEN;
495 n *= 2;
496 }
497 if depth > 1 {
498 n += EvenSymmetric::<[f32; HBF_TAPS.1.0.len()]>::LEN;
499 n *= 2;
500 }
501 if depth > 2 {
502 n += EvenSymmetric::<[f32; HBF_TAPS.2.0.len()]>::LEN;
503 n *= 2;
504 }
505 if depth > 3 {
506 n += EvenSymmetric::<[f32; HBF_TAPS.3.0.len()]>::LEN;
507 n *= 2;
508 }
509 n
510}
511
512#[cfg(test)]
513mod test {
514 use super::*;
515 use dsp_process::{Process, Split};
516 use rustfft::{FftPlanner, num_complex::Complex};
517
518 #[test]
519 fn test() {
520 let mut h = Split::new(EvenSymmetric([0.5]), HbfDec::<[_; 5]>::default());
521 h.as_mut().block(&[], &mut []);
522
523 let x = [1.0; 8];
524 let mut y = [0.0; 4];
525 h.as_mut().block(x.as_chunks().0, &mut y);
526 assert_eq!(y, [1.5, 2.0, 2.0, 2.0]);
527
528 let mut h = Split::new(&HBF_TAPS.3, HbfDec::<[_; 11]>::default());
529 let x: Vec<_> = (0..8).map(|i| i as f32).collect();
530 h.as_mut().block(x.as_chunks().0, &mut y);
531 println!("{:?}", y);
532 }
533
534 #[test]
535 fn decim() {
536 let mut h = HbfDec16::default();
537 const R: usize = 1 << 4;
538 let mut y = vec![0.0; 2];
539 let x: Vec<_> = (0..y.len() * R).map(|i| i as f32).collect();
540 HBF_DEC_CASCADE
541 .inner
542 .1
543 .block(&mut h, x.as_chunks::<R>().0, &mut y);
544 println!("{:?}", y);
545 }
546
547 #[test]
548 fn response_length_dec() {
549 let mut h = HbfDec16::default();
550 const R: usize = 4;
551 let mut y = [0.0; 100];
552 let x: Vec<f32> = (0..y.len() << R).map(|_| rand::random()).collect();
553 HBF_DEC_CASCADE
554 .inner
555 .1
556 .block(&mut h, x.as_chunks::<{ 1 << R }>().0, &mut y);
557 let x = vec![0.0; 1 << 10];
558 HBF_DEC_CASCADE.inner.1.block(
559 &mut h,
560 x.as_chunks::<{ 1 << R }>().0,
561 &mut y[..x.len() >> R],
562 );
563 let n = hbf_dec_response_length(R);
564 assert!(y[n - 1] != 0.0);
565 assert_eq!(y[n], 0.0);
566 }
567
568 #[test]
569 fn interp() {
570 let mut h = HbfInt16::default();
571 const R: usize = 4;
572 let r = hbf_int_response_length(R);
573 let mut x = vec![0.0; (r >> R) + 1];
574 x[0] = 1.0;
575 let mut y = vec![[0.0; 1 << R]; x.len()];
576 HBF_INT_CASCADE.inner.0.block(&mut h, &x, &mut y);
577 println!("{:?}", y); let y = y.as_flattened();
579 assert_ne!(y[r], 0.0);
580 assert_eq!(y[r + 1..], vec![0.0; y.len() - r - 1]);
581
582 let mut y: Vec<_> = y
583 .iter()
584 .map(|&x| Complex {
585 re: x as f64 / (1 << R) as f64,
586 im: 0.0,
587 })
588 .collect();
589 y.resize(5 << 10, Default::default());
591 FftPlanner::new().plan_fft_forward(y.len()).process(&mut y);
592 let p: Vec<_> = y.iter().map(|y| 10.0 * y.norm_sqr().log10()).collect();
594 let f = p.len() as f32 / (1 << R) as f32;
595 let p_pass = p[..(f * HBF_PASSBAND).floor() as _]
597 .iter()
598 .fold(0.0, |m, p| p.abs().max(m));
599 assert!(p_pass < 1e-6, "{p_pass}");
600 let p_stop = p[(f * (1.0 - HBF_PASSBAND)).ceil() as _..p.len() / 2]
602 .iter()
603 .fold(-200.0, |m, p| p.max(m));
604 assert!(p_stop < -141.5, "{p_stop}");
605 }
606
607 #[test]
610 #[ignore]
611 fn insn_dec() {
612 const N: usize = HBF_TAPS.4.0.len();
613 assert_eq!(N, 3);
614 const M: usize = 1 << 4;
615 let mut h = HbfDec::<[_; EvenSymmetric::<[f32; N]>::LEN + M]>::default();
616 let mut x = [[9.0; 2]; M];
617 let mut y = [0.0; M];
618 for _ in 0..1 << 25 {
619 HBF_TAPS.4.block(&mut h, &x, &mut y);
620 x[13][1] = y[11]; }
622 }
623
624 #[test]
627 #[ignore]
628 fn insn_dec2() {
629 const N: usize = HBF_TAPS.0.0.len();
630 assert_eq!(N, 23);
631 const M: usize = 1 << 9;
632 let mut h = HbfDec::<[_; EvenSymmetric::<[f32; N]>::LEN + M]>::default();
633 let mut x = [[9.0; 2]; M];
634 let mut y = [0.0; M];
635 for _ in 0..1 << 20 {
636 HBF_TAPS.0.block(&mut h, &x, &mut y);
637 x[33][1] = y[11]; }
639 }
640
641 #[test]
644 #[ignore]
645 fn insn_casc() {
646 let mut h = HbfDec16::default();
647 const R: usize = 4;
648 let mut x = [[9.0; 1 << R]; 1 << 6];
649 let mut y = [0.0; 1 << 6];
650 for _ in 0..1 << 20 {
651 HBF_DEC_CASCADE.inner.1.block(&mut h, &x, &mut y);
652 x[33][1] = y[11]; }
654 }
655
656 }