use serde::{Deserialize, Serialize};

#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct IIRState(pub [i32; 5]);

impl IIRState {
    #[inline(always)]
    pub fn get_x(&self, index: usize) -> i32 {
        // x0 is at index 0 in a biquad between updates
        self.0[index]
    }

    #[inline(always)]
    pub fn get_y(&self, index: usize) -> i32 {
        // y0 is at index 2 in a biquad between updates
        self.0[2 + index]
    }
}

fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
    // Rounding bias, half up
    let y0 = ((y0 as i64) << shift) + (1 << (shift - 1));
    let y = x
        .iter()
        .zip(a)
        .map(|(x, a)| *x as i64 * *a as i64)
        .fold(y0, |y, xa| y + xa);
    (y >> shift) as i32
}

/// Integer biquad IIR
///
/// See `dsp::iir::IIR` for general implementation details.
/// Offset and limiting disabled to suit lowpass applications.
/// Coefficient scaling fixed and optimized.
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct IIR {
    pub ba: IIRState,
    // pub y_offset: i32,
    // pub y_min: i32,
    // pub y_max: i32,
}

impl IIR {
    /// Coefficient fixed point: signed Q2.30.
    /// Tailored to low-passes PI, II etc.
    pub const SHIFT: u32 = 30;

    /// Feed a new input value into the filter, update the filter state, and
    /// return the new output. Only the state `xy` is modified.
    ///
    /// # Arguments
    /// * `xy` - Current filter state.
    /// * `x0` - New input.
    pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 {
        let n = self.ba.0.len();
        debug_assert!(xy.0.len() == n);
        // `xy` contains       x0 x1 y0 y1 y2
        // Increment time      x1 x2 y1 y2 y3
        // Shift               x1 x1 x2 y1 y2
        // This unrolls better than xy.rotate_right(1)
        xy.0.copy_within(0..n - 1, 1);
        // Store x0            x0 x1 x2 y1 y2
        xy.0[0] = x0;
        // Compute y0 by multiply-accumulate
        let y0 = macc(0, &xy.0, &self.ba.0, IIR::SHIFT);
        // Limit y0
        // let y0 = y0.max(self.y_min).min(self.y_max);
        // Store y0            x0 x1 y0 y1 y2
        xy.0[n / 2] = y0;
        y0
    }
}