codename-206/src/dsp/biquad.rs

// SPDX-License-Identifier: GPL-3.0-or-later
//! Generic second-order IIR biquad, Transposed Direct Form II.
//!
//! Coefficient formulas are the RBJ Audio EQ Cookbook
//! (<https://www.w3.org/TR/audio-eq-cookbook/>), prenormalised by `a0`. Scalar `f32`; we run
//! one filter per channel rather than SIMD to match the rest of the per-channel DSP.

use std::f32::consts;

/// Butterworth Q (= 1/√2). Two cascaded Butterworth sections make a 4th-order Linkwitz-Riley.
pub const NEUTRAL_Q: f32 = consts::FRAC_1_SQRT_2;

/// Prenormalised biquad coefficients `[b0, b1, b2, a1, a2]` (already divided by `a0`).
#[derive(Clone, Copy)]
pub struct BiquadCoefficients {
    b0: f32,
    b1: f32,
    b2: f32,
    a1: f32,
    a2: f32,
}

impl Default for BiquadCoefficients {
    fn default() -> Self {
        Self::identity()
    }
}

impl BiquadCoefficients {
    /// Passes the signal through unchanged.
    pub fn identity() -> Self {
        Self { b0: 1.0, b1: 0.0, b2: 0.0, a1: 0.0, a2: 0.0 }
    }

    pub fn lowpass(sample_rate: f32, frequency: f32, q: f32) -> Self {
        let (cos_w0, alpha) = Self::omega(sample_rate, frequency, q);
        let a0 = 1.0 + alpha;
        Self {
            b0: ((1.0 - cos_w0) / 2.0) / a0,
            b1: (1.0 - cos_w0) / a0,
            b2: ((1.0 - cos_w0) / 2.0) / a0,
            a1: (-2.0 * cos_w0) / a0,
            a2: (1.0 - alpha) / a0,
        }
    }

    pub fn highpass(sample_rate: f32, frequency: f32, q: f32) -> Self {
        let (cos_w0, alpha) = Self::omega(sample_rate, frequency, q);
        let a0 = 1.0 + alpha;
        Self {
            b0: ((1.0 + cos_w0) / 2.0) / a0,
            b1: -(1.0 + cos_w0) / a0,
            b2: ((1.0 + cos_w0) / 2.0) / a0,
            a1: (-2.0 * cos_w0) / a0,
            a2: (1.0 - alpha) / a0,
        }
    }

    pub fn allpass(sample_rate: f32, frequency: f32, q: f32) -> Self {
        let (cos_w0, alpha) = Self::omega(sample_rate, frequency, q);
        let a0 = 1.0 + alpha;
        Self {
            b0: (1.0 - alpha) / a0,
            b1: (-2.0 * cos_w0) / a0,
            b2: (1.0 + alpha) / a0,
            a1: (-2.0 * cos_w0) / a0,
            a2: (1.0 - alpha) / a0,
        }
    }

    /// Shared intermediate terms: `(cos ω0, α)`.
    fn omega(sample_rate: f32, frequency: f32, q: f32) -> (f32, f32) {
        let w0 = consts::TAU * (frequency / sample_rate);
        (w0.cos(), w0.sin() / (2.0 * q))
    }
}

/// A biquad filter holding its two state variables.
#[derive(Clone, Copy, Default)]
pub struct Biquad {
    coefficients: BiquadCoefficients,
    s1: f32,
    s2: f32,
}

impl Biquad {
    /// Replace the coefficients (keeps the state — fine for smooth coefficient changes).
    pub fn set_coefficients(&mut self, coefficients: BiquadCoefficients) {
        self.coefficients = coefficients;
    }

    /// Process one sample (Transposed Direct Form II).
    #[inline]
    pub fn process(&mut self, x: f32) -> f32 {
        let c = &self.coefficients;
        let y = c.b0 * x + self.s1;
        self.s1 = c.b1 * x - c.a1 * y + self.s2;
        self.s2 = c.b2 * x - c.a2 * y;
        y
    }

    /// Clear the filter state.
    pub fn reset(&mut self) {
        self.s1 = 0.0;
        self.s2 = 0.0;
    }
}

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

    const SR: f32 = 48_000.0;

    fn magnitude_at(mut coeffs_filter: Biquad, freq: f32) -> f32 {
        use std::f32::consts::TAU;
        let n = 16_000usize;
        let mut acc = 0.0f64;
        for i in 0..n {
            let x = (TAU * freq * i as f32 / SR).sin();
            let y = coeffs_filter.process(x);
            if i >= n - 8_000 {
                acc += (y * y) as f64;
            }
        }
        // RMS of a unit sine is 1/√2; divide it out to get the magnitude response.
        ((acc / 8_000.0).sqrt() as f32) * std::f32::consts::SQRT_2
    }

    #[test]
    fn lowpass_passes_dc_blocks_highs() {
        let lp = {
            let mut b = Biquad::default();
            b.set_coefficients(BiquadCoefficients::lowpass(SR, 1_000.0, NEUTRAL_Q));
            b
        };
        assert!((magnitude_at(lp, 100.0) - 1.0).abs() < 0.05); // ~passband
        assert!(magnitude_at(lp, 12_000.0) < 0.05); // ~stopband
    }

    #[test]
    fn allpass_is_unity_magnitude() {
        for &f in &[100.0, 1_000.0, 8_000.0] {
            let mut b = Biquad::default();
            b.set_coefficients(BiquadCoefficients::allpass(SR, 2_000.0, NEUTRAL_Q));
            assert!((magnitude_at(b, f) - 1.0).abs() < 0.02, "allpass not flat at {f} Hz");
        }
    }
}