use core::{
iter::Sum,
ops::{Add, Mul, Sub},
slice::{from_mut, from_ref},
};
use dsp_process::{ChunkIn, ChunkOut, Major, SplitInplace, SplitProcess};
#[inline]
fn get<C: Copy, T, const M: usize, const ODD: bool, const SYM: bool>(
c: &[C; M],
x: &[T],
) -> impl Iterator<Item = T>
where
T: Copy + Add<Output = T> + Sub<Output = T> + Mul<C, Output = T> + Sum,
{
x.windows(2 * M + ODD as usize).map(|x| {
let Some((old, new)) = x.first_chunk::<M>().zip(x.last_chunk::<M>()) else {
unreachable!()
};
let xc = old
.iter()
.zip(new.iter().rev())
.zip(c.iter())
.map(|((old, new), tap)| (if SYM { *new + *old } else { *new - *old }) * *tap)
.sum();
if ODD && SYM { xc + x[M] } else { xc }
})
}
macro_rules! type_fir {
($name:ident, $odd:literal, $sym:literal) => {
#[doc = concat!("Linear phase FIR taps for odd = ", stringify!($odd), " and symmetric = ", stringify!($sym))]
#[derive(Clone, Copy, Debug, Default)]
#[repr(transparent)]
pub struct $name<C>(pub C);
impl<T, const M: usize> $name<[T; M]> {
pub const LEN: usize = 2 * M - 1 + $odd as usize;
#[allow(unused)]
const fn len(&self) -> usize {
Self::LEN
}
}
impl<
C: Copy,
T: Copy + Default + Sub<T, Output = T> + Add<Output = T> + Mul<C, Output = T> + Sum,
const M: usize,
const N: usize,
> SplitProcess<T, T, [T; N]> for $name<[C; M]>
{
fn block(&self, state: &mut [T; N], x: &[T], y: &mut [T]) {
for (x, y) in x.chunks(N - Self::LEN).zip(y.chunks_mut(N - Self::LEN)) {
state[Self::LEN..][..x.len()].copy_from_slice(x);
for (y, x) in y.iter_mut().zip(get::<_, _, _, $odd, $sym>(&self.0, state)) {
*y = x;
}
state.copy_within(x.len()..x.len() + Self::LEN, 0);
}
}
fn process(&self, state: &mut [T; N], x: T) -> T {
let mut y = T::default();
self.block(state, from_ref(&x), from_mut(&mut y));
y
}
}
impl<
C: Copy,
T: Copy + Default + Sub<T, Output = T> + Add<Output = T> + Mul<C, Output = T> + Sum,
const M: usize,
const N: usize,
> SplitInplace<T, [T; N]> for $name<[C; M]>
{
fn inplace(&self, state: &mut [T; N], xy: &mut [T]) {
for xy in xy.chunks_mut(N - Self::LEN) {
state[Self::LEN..][..xy.len()].copy_from_slice(xy);
for (y, x) in xy.iter_mut().zip(get::<_, _, _, $odd, $sym>(&self.0, state)) {
*y = x;
}
state.copy_within(xy.len()..xy.len() + Self::LEN, 0);
}
}
}
};
}
type_fir!(OddSymmetric, true, true);
type_fir!(EvenSymmetric, false, true);
type_fir!(OddAntiSymmetric, true, false);
type_fir!(EvenAntiSymmetric, false, false);
#[derive(Clone, Debug)]
pub struct HbfDec<T> {
even: T, odd: T, }
impl<T: Copy + Default, const N: usize> Default for HbfDec<[T; N]> {
fn default() -> Self {
Self {
even: [T::default(); _],
odd: [T::default(); _],
}
}
}
impl<
C: Copy,
T: Copy + Default + Add<Output = T> + Sub<Output = T> + Mul<C, Output = T> + Sum,
const M: usize,
const N: usize,
> SplitProcess<[T; 2], T, HbfDec<[T; N]>> for EvenSymmetric<[C; M]>
{
fn block(&self, state: &mut HbfDec<[T; N]>, x: &[[T; 2]], y: &mut [T]) {
debug_assert_eq!(x.len(), y.len());
const { assert!(N > Self::LEN) }
for (x, y) in x.chunks(N - Self::LEN).zip(y.chunks_mut(N - Self::LEN)) {
for (x, (even, odd)) in x.iter().zip(
state.even[M - 1..]
.iter_mut()
.zip(state.odd[Self::LEN..].iter_mut()),
) {
*even = x[0];
*odd = x[1];
}
let odd = get::<_, _, _, false, true>(&self.0, &state.odd);
for (y, (odd, even)) in y.iter_mut().zip(odd.zip(state.even.iter().copied())) {
*y = odd + even;
}
state.even.copy_within(x.len()..x.len() + M - 1, 0);
state.odd.copy_within(x.len()..x.len() + Self::LEN, 0);
}
}
fn process(&self, state: &mut HbfDec<[T; N]>, x: [T; 2]) -> T {
let mut y = Default::default();
self.block(state, from_ref(&x), from_mut(&mut y));
y
}
}
#[derive(Clone, Debug)]
pub struct HbfInt<T> {
x: T, }
impl<T: Default + Copy, const N: usize> Default for HbfInt<[T; N]> {
fn default() -> Self {
Self {
x: [T::default(); _],
}
}
}
impl<
C: Copy,
T: Copy + Default + Add<Output = T> + Sub<Output = T> + Mul<C, Output = T> + Sum,
const M: usize,
const N: usize,
> SplitProcess<T, [T; 2], HbfInt<[T; N]>> for EvenSymmetric<[C; M]>
{
fn block(&self, state: &mut HbfInt<[T; N]>, x: &[T], y: &mut [[T; 2]]) {
debug_assert_eq!(x.len(), y.len());
const { assert!(N > Self::LEN) }
for (x, y) in x.chunks(N - Self::LEN).zip(y.chunks_mut(N - Self::LEN)) {
state.x[Self::LEN..][..x.len()].copy_from_slice(x);
let odd = get::<_, _, _, false, true>(&self.0, &state.x);
for (y, (even, odd)) in y.iter_mut().zip(odd.zip(state.x[M..].iter().copied())) {
*y = [even, odd]; }
state.x.copy_within(x.len()..x.len() + Self::LEN, 0);
}
}
fn process(&self, state: &mut HbfInt<[T; N]>, x: T) -> [T; 2] {
let mut y = Default::default();
self.block(state, from_ref(&x), from_mut(&mut y));
y
}
}
type HbfTaps98 = (
EvenSymmetric<[f32; 15]>,
EvenSymmetric<[f32; 6]>,
EvenSymmetric<[f32; 3]>,
EvenSymmetric<[f32; 3]>,
EvenSymmetric<[f32; 2]>,
);
#[allow(clippy::excessive_precision)]
pub const HBF_TAPS_98: HbfTaps98 = (
EvenSymmetric([
7.02144012e-05,
-2.43279582e-04,
6.35026936e-04,
-1.39782541e-03,
2.74613582e-03,
-4.96403839e-03,
8.41806912e-03,
-1.35827601e-02,
2.11004053e-02,
-3.19267647e-02,
4.77024289e-02,
-7.18014345e-02,
1.12942004e-01,
-2.03279594e-01,
6.33592923e-01,
]),
EvenSymmetric([
-0.00086943,
0.00577837,
-0.02201674,
0.06357869,
-0.16627679,
0.61979312,
]),
EvenSymmetric([0.01414651, -0.10439639, 0.59026742]),
EvenSymmetric([0.01227974, -0.09930782, 0.58702834]),
EvenSymmetric([-0.06291796, 0.5629161]),
);
type HbfTaps = (
EvenSymmetric<[f32; 23]>,
EvenSymmetric<[f32; 10]>,
EvenSymmetric<[f32; 5]>,
EvenSymmetric<[f32; 4]>,
EvenSymmetric<[f32; 3]>,
);
#[allow(clippy::excessive_precision)]
pub const HBF_TAPS: HbfTaps = (
EvenSymmetric([
7.60375795e-07,
-3.77494111e-06,
1.26458559e-05,
-3.43188253e-05,
8.10687478e-05,
-1.72971467e-04,
3.40845059e-04,
-6.29522864e-04,
1.10128831e-03,
-1.83933299e-03,
2.95124926e-03,
-4.57290964e-03,
6.87374176e-03,
-1.00656257e-02,
1.44199840e-02,
-2.03025100e-02,
2.82462332e-02,
-3.91128509e-02,
5.44795658e-02,
-7.77002672e-02,
1.17523452e-01,
-2.06185388e-01,
6.34588695e-01,
]),
EvenSymmetric([
-1.12811343e-05,
1.12724671e-04,
-6.07439343e-04,
2.31904511e-03,
-7.00322950e-03,
1.78225473e-02,
-4.01209836e-02,
8.43315989e-02,
-1.83189521e-01,
6.26346521e-01,
]),
EvenSymmetric([0.0007686, -0.00768669, 0.0386536, -0.14002434, 0.60828885]),
EvenSymmetric([-0.00261331, 0.02476858, -0.12112638, 0.59897111]),
EvenSymmetric([0.01186105, -0.09808109, 0.58622005]),
);
pub const HBF_PASSBAND: f32 = 0.4;
pub const HBF_CASCADE_BLOCK: usize = 1 << 5;
pub type HbfDec2 = HbfDec<[f32; HBF_TAPS.0.len() + HBF_CASCADE_BLOCK]>;
pub type HbfDec4 = (
HbfDec<[f32; HBF_TAPS.1.len() + (HBF_CASCADE_BLOCK << 1)]>,
HbfDec2,
);
pub type HbfDec8 = (
HbfDec<[f32; HBF_TAPS.2.len() + (HBF_CASCADE_BLOCK << 2)]>,
HbfDec4,
);
pub type HbfDec16 = (
HbfDec<[f32; HBF_TAPS.3.len() + (HBF_CASCADE_BLOCK << 3)]>,
HbfDec8,
);
pub type HbfDec32 = (
HbfDec<[f32; HBF_TAPS.4.len() + (HBF_CASCADE_BLOCK << 4)]>,
HbfDec16,
);
type HbfDecCascade<const B: usize = HBF_CASCADE_BLOCK> = Major<
(
ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.4.0.len()]>, 2>,
Major<
(
ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.3.0.len()]>, 2>,
Major<
(
ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.2.0.len()]>, 2>,
Major<
(
ChunkIn<&'static EvenSymmetric<[f32; HBF_TAPS.1.0.len()]>, 2>,
&'static EvenSymmetric<[f32; HBF_TAPS.0.0.len()]>,
),
[[f32; 2]; B],
>,
),
[[f32; 4]; B],
>,
),
[[f32; 8]; B],
>,
),
[[f32; 16]; B],
>;
pub const HBF_DEC_CASCADE: HbfDecCascade = Major::new((
ChunkIn(&HBF_TAPS.4),
Major::new((
ChunkIn(&HBF_TAPS.3),
Major::new((
ChunkIn(&HBF_TAPS.2),
Major::new((ChunkIn(&HBF_TAPS.1), &HBF_TAPS.0)),
)),
)),
));
pub const fn hbf_dec_response_length(depth: usize) -> usize {
assert!(depth <= 5);
let mut n = 0;
if depth > 4 {
n /= 2;
n += HBF_TAPS.4.len();
}
if depth > 3 {
n /= 2;
n += HBF_TAPS.3.len();
}
if depth > 2 {
n /= 2;
n += HBF_TAPS.2.len();
}
if depth > 1 {
n /= 2;
n += HBF_TAPS.1.len();
}
if depth > 0 {
n /= 2;
n += HBF_TAPS.0.len();
}
n
}
pub type HbfInt2 = HbfInt<[f32; HBF_TAPS.0.len() + HBF_CASCADE_BLOCK]>;
pub type HbfInt4 = (
HbfInt2,
HbfInt<[f32; HBF_TAPS.1.len() + (HBF_CASCADE_BLOCK << 1)]>,
);
pub type HbfInt8 = (
HbfInt4,
HbfInt<[f32; HBF_TAPS.2.len() + (HBF_CASCADE_BLOCK << 2)]>,
);
pub type HbfInt16 = (
HbfInt8,
HbfInt<[f32; HBF_TAPS.3.len() + (HBF_CASCADE_BLOCK << 3)]>,
);
pub type HbfInt32 = (
HbfInt16,
HbfInt<[f32; HBF_TAPS.4.len() + (HBF_CASCADE_BLOCK << 4)]>,
);
type HbfIntCascade<const B: usize = HBF_CASCADE_BLOCK> = Major<
(
Major<
(
Major<
(
Major<
(
&'static EvenSymmetric<[f32; HBF_TAPS.0.0.len()]>,
ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.1.0.len()]>, 2>,
),
[[f32; 2]; B],
>,
ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.2.0.len()]>, 2>,
),
[[f32; 4]; B],
>,
ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.3.0.len()]>, 2>,
),
[[f32; 8]; B],
>,
ChunkOut<&'static EvenSymmetric<[f32; HBF_TAPS.4.0.len()]>, 2>,
),
[[f32; 16]; B],
>;
pub const HBF_INT_CASCADE: HbfIntCascade = Major::new((
Major::new((
Major::new((
Major::new((&HBF_TAPS.0, ChunkOut(&HBF_TAPS.1))),
ChunkOut(&HBF_TAPS.2),
)),
ChunkOut(&HBF_TAPS.3),
)),
ChunkOut(&HBF_TAPS.4),
));
pub const fn hbf_int_response_length(depth: usize) -> usize {
assert!(depth <= 5);
let mut n = 0;
if depth > 0 {
n += HBF_TAPS.0.len();
n *= 2;
}
if depth > 1 {
n += HBF_TAPS.1.len();
n *= 2;
}
if depth > 2 {
n += HBF_TAPS.2.len();
n *= 2;
}
if depth > 3 {
n += HBF_TAPS.3.len();
n *= 2;
}
if depth > 4 {
n += HBF_TAPS.4.len();
n *= 2;
}
n
}
#[cfg(test)]
mod test {
use super::*;
use dsp_process::{Process, Split};
use rustfft::{FftPlanner, num_complex::Complex};
#[test]
fn test() {
let mut h = Split::new(EvenSymmetric([0.5]), HbfDec::<[_; 5]>::default());
h.block(&[], &mut []);
let x = [1.0; 8];
let mut y = [0.0; 4];
h.block(x.as_chunks().0, &mut y);
assert_eq!(y, [1.5, 2.0, 2.0, 2.0]);
let mut h = Split::new(&HBF_TAPS.3, HbfDec::<[_; 11]>::default());
let x: Vec<_> = (0..8).map(|i| i as f32).collect();
h.block(x.as_chunks().0, &mut y);
println!("{:?}", y);
}
#[test]
fn decim() {
let mut h = HbfDec16::default();
const R: usize = 1 << 4;
let mut y = vec![0.0; 2];
let x: Vec<_> = (0..y.len() * R).map(|i| i as f32).collect();
HBF_DEC_CASCADE
.inner
.1
.block(&mut h, x.as_chunks::<R>().0, &mut y);
println!("{:?}", y);
}
#[test]
fn response_length_dec() {
let mut h = HbfDec16::default();
const R: usize = 4;
let mut y = [0.0; 100];
let x: Vec<f32> = (0..y.len() << R).map(|_| rand::random()).collect();
HBF_DEC_CASCADE
.inner
.1
.block(&mut h, x.as_chunks::<{ 1 << R }>().0, &mut y);
let x = vec![0.0; 1 << 10];
HBF_DEC_CASCADE.inner.1.block(
&mut h,
x.as_chunks::<{ 1 << R }>().0,
&mut y[..x.len() >> R],
);
let n = hbf_dec_response_length(R);
assert!(y[n - 1] != 0.0);
assert_eq!(y[n], 0.0);
}
#[test]
fn interp() {
let mut h = HbfInt16::default();
const R: usize = 4;
let r = hbf_int_response_length(R);
let mut x = vec![0.0; (r >> R) + 1];
x[0] = 1.0;
let mut y = vec![[0.0; 1 << R]; x.len()];
HBF_INT_CASCADE.inner.0.block(&mut h, &x, &mut y);
println!("{:?}", y); let y = y.as_flattened();
assert_ne!(y[r], 0.0);
assert_eq!(y[r + 1..], vec![0.0; y.len() - r - 1]);
let mut y: Vec<_> = y
.iter()
.map(|&x| Complex {
re: x as f64 / (1 << R) as f64,
im: 0.0,
})
.collect();
y.resize(5 << 10, Default::default());
FftPlanner::new().plan_fft_forward(y.len()).process(&mut y);
let p: Vec<_> = y.iter().map(|y| 10.0 * y.norm_sqr().log10()).collect();
let f = p.len() as f32 / (1 << R) as f32;
let p_pass = p[..(f * HBF_PASSBAND).floor() as _]
.iter()
.fold(0.0, |m, p| p.abs().max(m));
assert!(p_pass < 1e-6, "{p_pass}");
let p_stop = p[(f * (1.0 - HBF_PASSBAND)).ceil() as _..p.len() / 2]
.iter()
.fold(-200.0, |m, p| p.max(m));
assert!(p_stop < -141.5, "{p_stop}");
}
#[test]
#[ignore]
fn insn_dec() {
const N: usize = HBF_TAPS.4.0.len();
assert_eq!(N, 3);
const M: usize = 1 << 4;
let mut h = HbfDec::<[_; EvenSymmetric::<[f32; N]>::LEN + M]>::default();
let mut x = [[9.0; 2]; M];
let mut y = [0.0; M];
for _ in 0..1 << 25 {
HBF_TAPS.4.block(&mut h, &x, &mut y);
x[13][1] = y[11]; }
}
#[test]
#[ignore]
fn insn_dec2() {
const N: usize = HBF_TAPS.0.0.len();
assert_eq!(N, 23);
const M: usize = 1 << 9;
let mut h = HbfDec::<[_; EvenSymmetric::<[f32; N]>::LEN + M]>::default();
let mut x = [[9.0; 2]; M];
let mut y = [0.0; M];
for _ in 0..1 << 20 {
HBF_TAPS.0.block(&mut h, &x, &mut y);
x[33][1] = y[11]; }
}
#[test]
#[ignore]
fn insn_casc() {
let mut h = HbfDec16::default();
const R: usize = 4;
let mut x = [[9.0; 1 << R]; 1 << 6];
let mut y = [0.0; 1 << 6];
for _ in 0..1 << 20 {
HBF_DEC_CASCADE.inner.1.block(&mut h, &x, &mut y);
x[33][1] = y[11]; }
}
}