use crate::sos::SisoSosFilter;
use core::ops::Neg;
use num_traits::{FromPrimitive, MulAdd, Num, ToPrimitive};
pub fn simulate_gain_sinewave<const SECTIONS: usize, T>(
filter: &mut SisoSosFilter<SECTIONS, T>,
freq: f64,
n: usize,
) -> f64
where
T: Num + Copy + MulAdd<Output = T> + Neg<Output = T> + FromPrimitive + ToPrimitive,
{
let input: Vec<T> = (0..n)
.map(|i| T::from_f64((2.0 * std::f64::consts::PI * freq * i as f64).sin()).unwrap())
.collect();
let original_rms = (input
.iter()
.map(|&v| (v * v).to_f64().unwrap())
.sum::<f64>()
/ n as f64)
.sqrt();
let mut output = Vec::with_capacity(n);
filter.reset();
for &u in &input {
output.push(filter.update(u));
}
filter.reset();
let output_rms = (output
.iter()
.map(|&v| (v * v).to_f64().unwrap())
.sum::<f64>()
/ n as f64)
.sqrt();
output_rms / original_rms
}
pub fn test_filter<const SECTIONS: usize, T>(
min_cutoff_ratio: f64,
max_cutoff_ratio: f64,
butter_fn: fn(f64) -> Result<SisoSosFilter<SECTIONS, T>, &'static str>,
) where
T: Num + Copy + MulAdd<Output = T> + Neg<Output = T> + FromPrimitive + ToPrimitive,
{
let order = 2 * SECTIONS;
let mut filtermax = butter_fn(max_cutoff_ratio).unwrap();
let gain = simulate_gain_sinewave(&mut filtermax, max_cutoff_ratio, 1024);
let gain_expected = 1.0 / 2f64.sqrt();
let attenuation_rel_err = (gain - gain_expected).abs() / gain_expected;
println!("order {order} attenuation rel err {attenuation_rel_err}");
assert!(attenuation_rel_err < 0.05);
let mut filtermin = butter_fn(min_cutoff_ratio).unwrap();
(0..10_000).for_each(|_| {
filtermin.update(T::from_f64(1.0).unwrap());
});
let step_min_final = filtermin.update(T::from_f64(1.0).unwrap());
println!(
"order {order} step min final {:?}",
step_min_final.to_f64().unwrap()
);
let step_min_rel_err = (step_min_final.to_f64().unwrap() - 1.0).abs() / 1.0;
println!("order {order} step min rel err {step_min_rel_err}");
assert!(step_min_rel_err < 1e-4);
filtermax.reset();
(0..1024).for_each(|_| {
filtermax.update(T::from_f64(1.0).unwrap());
});
let step_max_final = filtermax.update(T::from_f64(1.0).unwrap());
let step_max_rel_err = (step_max_final.to_f64().unwrap() - 1.0).abs() / 1.0;
println!("order {order} step max rel err {step_max_rel_err}");
assert!(step_max_rel_err < 1e-6);
}