use num_traits::{Float, FromPrimitive, One, Zero};

pub fn sos_filt<T>(x: &mut Vec<T>, sos: &Vec<[f64; 6]>) -> Result<(), String> where
    T: Float + FromPrimitive + Zero + Clone + One,
{
    // let mut x: Vec<T> = x.iter().map(|&v| T::from(v).unwrap()).collect();

    let n = x.len();
    let mut zi = vec![(T::zero(), T::zero()); sos.len()];

    for (section_idx, &[b0, b1, b2, a0, a1, a2]) in sos.iter().enumerate() {
        assert!(
            (a0 - 1.0).abs() < 1e-12,
            "a0 must be 1.0 for scipy compatibility"
        );

        let mut z1 = zi[section_idx].0;
        let mut z2 = zi[section_idx].1;

        for i in 0..n{
            let input = x[i];
            let output = T::from(b0).unwrap() * input + z1;
            z1 = T::from(b1).unwrap() * input - T::from(a1).unwrap() * output + z2;
            z2 = T::from(b2).unwrap() * input - T::from(a2).unwrap() * output;
            x[i] = output;
        }

        zi[section_idx] = (z1, z2);
    }

    Ok(())
}
