use std::f64::consts::PI;

use crate::Level;

#[derive(Debug)]
pub struct BiquadCoefficients {
    a1: f64,
    a2: f64,
    b0: f64,
    b1: f64,
    b2: f64,
}

impl BiquadCoefficients {
    pub fn low_pass(center_frequency: f64, sample_rate: f64, q: f64) -> Self {
        let omega = omega(center_frequency, sample_rate);

        let b0 = (1.0 - omega.cos()) / 2.0;
        let b1 = 2.0 * b0;
        let b2 = b0;

        let alpha = omega.sin() / (2.0 * q);
        let a0 = 1.0 + alpha;
        let a1 = -2.0 * omega.cos();
        let a2 = 1.0 - alpha;

        let scale = 1.0 / a0;

        Self {
            a1: a1 * scale,
            a2: a2 * scale,
            b0: b0 * scale,
            b1: b1 * scale,
            b2: b2 * scale,
        }
    }

    pub fn high_pass(center_frequency: f64, sample_rate: f64, q: f64) -> Self {
        let omega = omega(center_frequency, sample_rate);

        let b0 = (1.0 + omega.cos()) / 2.0;
        let b1 = -(1.0 + omega.cos());
        let b2 = b0;

        let alpha = omega.sin() / (2.0 * q);
        let a0 = 1.0 + alpha;
        let a1 = -2.0 * omega.cos();
        let a2 = 1.0 - alpha;

        let scale = 1.0 / a0;

        Self {
            a1: a1 * scale,
            a2: a2 * scale,
            b0: b0 * scale,
            b1: b1 * scale,
            b2: b2 * scale,
        }
    }

    pub fn band_pass(center_frequency: f64, sample_rate: f64, q: f64) -> Self {
        let omega = omega(center_frequency, sample_rate);
        let alpha = omega.sin() / (2.0 * q);

        let b0 = alpha;
        let b1 = 0.0;
        let b2 = -alpha;

        let a0 = 1.0 + alpha;
        let a1 = -2.0 * omega.cos();
        let a2 = 1.0 - alpha;

        let scale = 1.0 / a0;

        Self {
            a1: a1 * scale,
            a2: a2 * scale,
            b0: b0 * scale,
            b1: b1 * scale,
            b2: b2 * scale,
        }
    }

    pub fn notch(center_frequency: f64, sample_rate: f64, q: f64) -> Self {
        let omega = omega(center_frequency, sample_rate);
        let alpha = omega.sin() / (2.0 * q);

        let b0 = 1.0;
        let b1 = -2.0 * omega.cos();
        let b2 = 1.0;

        let a0 = 1.0 + alpha;
        let a1 = -2.0 * omega.cos();
        let a2 = 1.0 - alpha;

        let scale = 1.0 / a0;

        Self {
            a1: a1 * scale,
            a2: a2 * scale,
            b0: b0 * scale,
            b1: b1 * scale,
            b2: b2 * scale,
        }
    }

    pub fn low_shelf(center_frequency: f64, sample_rate: f64, level: Level) -> Self {
        let a = level.as_linear().sqrt();

        let omega = omega(center_frequency, sample_rate);
        let beta = (2.0 * a).sqrt();

        let sin_omega = omega.sin();
        let cos_omega = omega.cos();

        let beta_sin_omega = beta * sin_omega;

        let b0 = a * ((a + 1.0) - (a - 1.0) * cos_omega + beta_sin_omega);
        let b1 = 2.0 * a * ((a - 1.0) - (a + 1.0) * cos_omega);
        let b2 = a * ((a + 1.0) - (a - 1.0) * cos_omega - beta_sin_omega);

        let a0 = (a + 1.0) + (a - 1.0) * cos_omega + beta_sin_omega;
        let a1 = -2.0 * ((a - 1.0) + (a + 1.0) * cos_omega);
        let a2 = (a + 1.0) + (a - 1.0) * cos_omega - beta_sin_omega;

        let scale = 1.0 / a0;

        Self {
            a1: a1 * scale,
            a2: a2 * scale,
            b0: b0 * scale,
            b1: b1 * scale,
            b2: b2 * scale,
        }
    }

    pub fn high_shelf(center_frequency: f64, sample_rate: f64, level: Level) -> Self {
        let a = level.as_linear().sqrt();

        let omega = omega(center_frequency, sample_rate);
        let beta = (a + a).sqrt();

        let sin_omega = omega.sin();
        let cos_omega = omega.cos();

        let beta_sin_omega = beta * sin_omega;

        let b0 = a * ((a + 1.0) + (a - 1.0) * cos_omega + beta_sin_omega);
        let b1 = -2.0 * a * ((a - 1.0) + (a + 1.0) * cos_omega);
        let b2 = a * ((a + 1.0) + (a - 1.0) * cos_omega - beta_sin_omega);

        let a0 = (a + 1.0) - (a - 1.0) * cos_omega + beta_sin_omega;
        let a1 = 2.0 * ((a - 1.0) - (a + 1.0) * cos_omega);
        let a2 = (a + 1.0) - (a - 1.0) * cos_omega - beta_sin_omega;

        let scale = 1.0 / a0;

        Self {
            a1: a1 * scale,
            a2: a2 * scale,
            b0: b0 * scale,
            b1: b1 * scale,
            b2: b2 * scale,
        }
    }

    pub fn a1(&self) -> f64 {
        self.a1
    }

    pub fn a2(&self) -> f64 {
        self.a2
    }

    pub fn b0(&self) -> f64 {
        self.b0
    }

    pub fn b1(&self) -> f64 {
        self.b1
    }

    pub fn b2(&self) -> f64 {
        self.b2
    }
}

fn omega(center_frequency: f64, sample_rate: f64) -> f64 {
    2.0 * PI * center_frequency / sample_rate
}

#[cfg(test)]
mod test {
    use approx::assert_relative_eq;

    use super::*;

    #[test]
    fn low_pass() {
        let coefficients = BiquadCoefficients::low_pass(5_000.0, 48_000.0, 1.414);
        assert_relative_eq!(coefficients.a1(), -1.30564969, epsilon = 1e-6);
        assert_relative_eq!(coefficients.a2(), 0.64573542, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b0(), 0.08502143, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b1(), 0.17004286, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b2(), 0.08502143, epsilon = 1e-6);
    }

    #[test]
    fn high_pass() {
        let coefficients = BiquadCoefficients::high_pass(2_000.0, 96_000.0, 2.5);
        assert_relative_eq!(coefficients.a1(), -1.93244284, epsilon = 1e-6);
        assert_relative_eq!(coefficients.a2(), 0.94911781, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b0(), 0.97039016, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b1(), -1.94078033, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b2(), 0.97039016, epsilon = 1e-6);
    }

    #[test]
    fn band_pass() {
        let coefficients = BiquadCoefficients::band_pass(3_000.0, 44_100.0, 6.0);
        assert_relative_eq!(coefficients.a1(), -1.75929661, epsilon = 1e-6);
        assert_relative_eq!(coefficients.a2(), 0.93321839, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b0(), 0.03339080, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b1(), 0.0, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b2(), -0.03339080, epsilon = 1e-6);
    }

    #[test]
    fn notch() {
        let coefficients = BiquadCoefficients::notch(7_000.0, 88_200.0, 10.0);
        assert_relative_eq!(coefficients.a1(), -1.71542276, epsilon = 1e-6);
        assert_relative_eq!(coefficients.a2(), 0.95329153, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b0(), 0.97664576, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b1(), -1.71542276, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b2(), 0.97664576, epsilon = 1e-6);
    }

    #[test]
    fn low_shelf() {
        let coefficients = BiquadCoefficients::low_shelf(80.0, 48_000.0, Level::from_db(6.0));
        println!("{coefficients:?}");
        assert_relative_eq!(coefficients.a1(), -1.98753939, epsilon = 1e-6);
        assert_relative_eq!(coefficients.a2(), 0.98761655, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b0(), 1.00257352, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b1(), -1.98750100, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b2(), 0.98508142, epsilon = 1e-6);
    }

    #[test]
    fn high_shelf() {
        let coefficients = BiquadCoefficients::high_shelf(5_000.0, 96_000.0, Level::from_db(3.0));
        println!("{coefficients:?}");
        assert_relative_eq!(coefficients.a1(), -1.50372629, epsilon = 1e-6);
        assert_relative_eq!(coefficients.a2(), 0.60441923, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b0(), 1.35784061, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b1(), -2.14483965, epsilon = 1e-6);
        assert_relative_eq!(coefficients.b2(), 0.88769198, epsilon = 1e-6);
    }
}
