lasprs 0.14.3

use super::biquad::Biquad;
use super::*;
use crate::*;
use serde::{Deserialize, Serialize};
use snafu::prelude::*;
type Result<T> = std::result::Result<T, FilterError>;

/// Series of biquads that filter sequentially on an input signal
///
/// # Examples
///
/// See (tests)
///
#[derive(Clone, Debug, Serialize, Deserialize)]
#[cfg_attr(feature = "python-bindings", gen_stub_pyclass, pyclass(from_py_object))]
pub struct SeriesBiquad {
    // A loose gain value
    gain: Flt,
    // List of biquads to process in series
    biqs: Vec<Biquad>,
}

#[cfg(feature = "python-bindings")]
#[cfg_attr(feature = "python-bindings", pymethods)]
impl SeriesBiquad {
    // Print biquad in Python
    fn __repr__(&self) -> String {
        format!("{self:?}")
    }

    /// Create new series filter set. See [SeriesBiquad::new()]
    ///
    #[new]
    pub fn new_py(coefs: PyReadonlyArrayDyn<Flt>) -> PyResult<Self> {
        Ok(SeriesBiquad::new(coefs.as_slice()?)?)
    }
    #[pyo3(name = "unit")]
    #[staticmethod]
    /// See: [Biquad::unit]
    pub fn unit_py() -> SeriesBiquad {
        SeriesBiquad::unit()
    }

    /// See: [SeriesBiquad::filter]
    #[pyo3(name = "filter")]
    pub fn filter_py<'py>(
        &mut self,
        py: Python<'py>,
        input: PyArrayLike1<Flt>,
    ) -> std::result::Result<PyArr1Flt<'py>, PyErr> {
        let mut output = vec![0.0; input.len()?];
        self.filter(input.as_slice()?, &mut output);
        Ok(output.into_pyarray(py))
    }
    #[pyo3(name = "reset")]
    /// See: [SeriesBiquad::reset()]
    pub fn reset_py(&mut self) {
        self.reset();
    }
}
impl SeriesBiquad {
    /// Create new series biquad from vector of biquads. No checks on the
    /// validity or the stability of the biquads are performed.
    ///
    pub fn newFromBiqs(biqs: Vec<Biquad>) -> SeriesBiquad {
        assert!(!biqs.is_empty());
        SeriesBiquad { biqs, gain: 1.0 }
    }

    /// Set an overall gain value to amplify / attenuate overall output
    pub fn setGain(&mut self, g: Flt) {
        self.gain = g;
    }

    /// Return reference to internally stored biquads
    pub fn getBiquads(&self) -> &Vec<Biquad> {
        &self.biqs
    }

    /// Create a new series biquad, having an arbitrary number of biquads.
    ///
    /// # Arguments
    ///
    /// * `filter_coefs` - Vector of biquad coefficients, stored in a single array. The first six
    ///   for the first biquad, and so on.
    ///
    ///
    pub fn new(filter_coefs: &[Flt]) -> Result<SeriesBiquad> {
        ensure!(
            filter_coefs.len().is_multiple_of(6) && filter_coefs.len() >= 6,
            InvalidBiquadInitializationSnafu {
                msg: format!(
                    "filter_coefs length should at least 6 and a multiple of 6, given: {}.",
                    filter_coefs.len()
                )
            }
        );
        let nfilters = filter_coefs.len() / 6;

        let mut biqs: Vec<Biquad> = Vec::with_capacity(nfilters);
        for coefs in filter_coefs.chunks(6) {
            let biq = Biquad::new(coefs)?;
            biqs.push(biq);
        }
        assert!(!biqs.is_empty());

        Ok(SeriesBiquad { biqs, gain: 1.0 })
    }

    /// Unit impulse response series biquad. Input = output
    pub fn unit() -> SeriesBiquad {
        let filter_coefs = &[1., 0., 0., 1., 0., 0.];
        SeriesBiquad::new(filter_coefs).unwrap()
    }

    /// Filter input signal in place
    pub fn filter_inout(&mut self, inout: &mut [Flt]) {
        for biq in self.biqs.iter_mut() {
            biq.filter_inout(inout);
        }
        inout.iter_mut().for_each(|io| *io *= self.gain);
    }
}

impl FilterMethods for SeriesBiquad {
    //! Filter input by applying all biquad filters in series on each input sample, to obtain the
    //! output samples.
    //!
    fn filter(&mut self, input: &[Flt], output: &mut [Flt]) {
        debug_assert_eq!(
            input.len(),
            output.len(),
            "Input and output vectors must have the same length"
        );
        output.copy_from_slice(input);
        self.filter_inout(output);
    }
    fn reset(&mut self) {
        self.biqs.iter_mut().for_each(|f| f.reset());
    }
}
impl<'a, T: AsArray<'a, Flt>> TransferFunction<'a, T> for SeriesBiquad {
    fn tf(&self, fs: StrictlyPositive, freq: T) -> Ccol {
        let freq = freq.into();
        let mut res = self.biqs.first().unwrap().tf(fs, freq) * self.gain;
        for biq in self.biqs.iter().skip(1) {
            res = &res * biq.tf(fs, freq);
        }
        res
    }
}

#[cfg(test)]
mod test {
    use super::*;

    #[test]
    #[should_panic]
    fn test_biquad2() {
        // A a0 coefficient not in the right place, meaning we panic on unwrap
        let filter_coefs = vec![1., 0., 0., 0., 0., 0.];
        let mut ser = SeriesBiquad::new(&filter_coefs).unwrap();
        let inp = vec![1., 0., 0., 0., 0., 0.];
        let mut filtered = vec![0.; inp.len()];
        ser.filter(&inp, &mut filtered);
        assert_eq!(&filtered, &inp);
    }
    #[test]
    fn test_biquad3() {
        let filter_coefs = vec![0.5, 0.5, 0., 1., 0., 0.];
        let mut ser = SeriesBiquad::new(&filter_coefs).unwrap();

        let mut inp = vec![1., 0., 0., 0., 0., 0.];
        let mut filtered = vec![0.; inp.len()];
        ser.filter(&inp, &mut filtered);

        // Change input to see match what should come out of output
        inp[0] = 0.5;
        inp[1] = 0.5;
        assert_eq!(&inp, &filtered);
    }
    #[test]
    fn test_seriesbiquad_tf1() {
        let filter_coefs = vec![1., 0., 0., 1., 0., 0.];
        let ser = SeriesBiquad::new(&filter_coefs).unwrap();
        let tf = ser.tf(1.0.try_into().unwrap(), &[0., 1.]);
        assert_eq!(tf[0].re, 1.0);
        assert_eq!(tf[1].im, 0.0);
    }
    #[test]
    fn test_seriesbiquad_tf2() {
        let filter_coefs = &[0.5, 0., 0., 1., 0., 0., 0.5, 0., 0., 1., 0., 0.];
        let ser = SeriesBiquad::new(filter_coefs).unwrap();
        let tf = ser.tf(1.0.try_into().unwrap(), &[0., 1.]);
        assert_eq!(tf[0].re, 0.25);
        assert_eq!(tf[1].im, 0.0);
    }
}