use crate::sos::SisoSosFilter;
use core::ops::Neg;
use num_traits::{FromPrimitive, MulAdd, Num, ToPrimitive};

/// Determine the gain of a filter at a given frequency by simulating its response to a sinewave.
/// `freq` is normalized to the sample frequency.
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
}

/// Common test logic for the autogenerated Butterworth SOS filters.
/// This test checks that the min and max cutoff ratios have been set correctly
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;
    // Check approximate attenuation at cutoff frequency at the maximum cutoff ratio; should be -3dB or 1/sqrt(2) magnitude
    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);

    // Check convergence of step responses at min and max tabulated cutoff
    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);
}
