autocore-std 3.3.66

//! Butterworth low-pass filtering for captured signal traces.
//!
//! A dependency-free implementation of digital Butterworth low-pass filters,
//! built from cascaded second-order ([`Biquad`]) sections and applied with a
//! zero-phase forward-reverse pass ([`filter_zero_phase`]). Intended for
//! offline / post-capture processing where the whole sample buffer is
//! available — e.g. cleaning structural ringing out of a force-plate or
//! load-cell trace before it is charted or reduced to a scalar result.
//!
//! ## Why zero-phase
//!
//! A causal IIR filter delays the signal in a frequency-dependent way, which
//! shifts peaks and edges in time. [`filter_zero_phase`] runs the cascade
//! forward and then backward, cancelling the phase term so features stay at
//! their original sample indices. The cost is that the effective magnitude
//! response is squared (`|H(f)|²`), so a 4th-order design behaves like 8th
//! order in attenuation and the −3 dB point sits at roughly `0.8·fc`.
//!
//! ## Choosing a cutoff
//!
//! Pick a cutoff that sits in the spectral gap between the bandwidth of the
//! signal you care about and the lowest structural resonance you want to
//! reject. The filter can only separate them if such a gap exists — a mode
//! that overlaps the signal band cannot be removed by any low-pass, and needs
//! a different approach (e.g. integrating over a window, which cancels
//! zero-mean ringing).
//!
//! ## Example
//!
//! ```
//! use autocore_std::butterworth::{butterworth4_lowpass, filter_zero_phase};
//!
//! let fs = 5000.0;          // sample rate (Hz)
//! let fc = 40.0;            // cutoff (Hz)
//! let coeffs = butterworth4_lowpass(fc, fs);
//! let raw: Vec<f64> = /* captured samples */ vec![0.0; 1000];
//! let filtered = filter_zero_phase(&coeffs, &raw);
//! assert_eq!(filtered.len(), raw.len());
//! ```

/// A second-order IIR section in Direct Form II Transposed.
///
/// Higher-order filters are built by cascading several of these — see
/// [`butterworth4_lowpass`].
#[derive(Clone, Copy, Debug)]
pub struct Biquad {
    /// Feed-forward coefficient for the current input sample.
    pub b0: f64,
    /// Feed-forward coefficient for the one-sample-delayed input.
    pub b1: f64,
    /// Feed-forward coefficient for the two-sample-delayed input.
    pub b2: f64,
    /// Feedback coefficient for the one-sample-delayed output.
    pub a1: f64,
    /// Feedback coefficient for the two-sample-delayed output.
    pub a2: f64,
}

impl Biquad {
    /// RBJ-style digital low-pass biquad.
    ///
    /// `q = 1/√2` gives a single maximally-flat (Butterworth) section; higher
    /// orders cascade two or more sections with the textbook Butterworth Q
    /// values (see [`butterworth4_lowpass`]). The coefficients are normalised
    /// to unity DC gain.
    pub fn lowpass(cutoff_hz: f64, sample_rate_hz: f64, q: f64) -> Self {
        use std::f64::consts::PI;
        let omega = 2.0 * PI * cutoff_hz / sample_rate_hz;
        let (sin_w, cos_w) = (omega.sin(), omega.cos());
        let alpha = sin_w / (2.0 * q);
        let a0 = 1.0 + alpha;
        Biquad {
            b0: (1.0 - cos_w) * 0.5 / a0,
            b1: (1.0 - cos_w) / a0,
            b2: (1.0 - cos_w) * 0.5 / a0,
            a1: -2.0 * cos_w / a0,
            a2: (1.0 - alpha) / a0,
        }
    }

    /// One sample through this biquad, advancing the mutable state `(z1, z2)`.
    #[inline]
    fn step(&self, x: f64, z: &mut (f64, f64)) -> f64 {
        let y = self.b0 * x + z.0;
        z.0 = self.b1 * x - self.a1 * y + z.1;
        z.1 = self.b2 * x - self.a2 * y;
        y
    }
}

/// 4th-order Butterworth low-pass as two cascaded biquads.
///
/// The two sections have the textbook Butterworth pole Q values for n=4:
/// `Q₁ = 1/(2·sin(π/8)) ≈ 1.306` (higher-Q, near-imaginary-axis pole pair)
/// and `Q₂ = 1/(2·sin(3π/8)) ≈ 0.541` (lower-Q pair). Combined, the magnitude
/// response is maximally flat in the passband — the defining property of
/// Butterworth — with a ~80 dB/decade roll-off (doubled to ~160 dB/decade
/// when applied zero-phase via [`filter_zero_phase`]).
pub fn butterworth4_lowpass(cutoff_hz: f64, sample_rate_hz: f64) -> [Biquad; 2] {
    use std::f64::consts::PI;
    let q1 = 1.0 / (2.0 * (PI / 8.0).sin());
    let q2 = 1.0 / (2.0 * (3.0 * PI / 8.0).sin());
    [
        Biquad::lowpass(cutoff_hz, sample_rate_hz, q1),
        Biquad::lowpass(cutoff_hz, sample_rate_hz, q2),
    ]
}

/// One causal forward pass through a cascaded biquad chain.
///
/// Each section is initialised to its steady state for the first input sample
/// (assuming unity DC gain, which all RBJ low-pass biquads satisfy), so a
/// signal starting at a non-zero DC level passes through without a startup
/// ramp. Prefer [`filter_zero_phase`] for trace processing — a single causal
/// pass shifts features in time.
pub fn filter_forward(coeffs: &[Biquad], x: &[f64]) -> Vec<f64> {
    let x0 = x.first().copied().unwrap_or(0.0);
    let mut state: Vec<(f64, f64)> = coeffs
        .iter()
        .map(|bq| ((1.0 - bq.b0) * x0, (bq.b2 - bq.a2) * x0))
        .collect();
    let mut out = Vec::with_capacity(x.len());
    for &v in x {
        let mut s = v;
        for (bq, st) in coeffs.iter().zip(state.iter_mut()) {
            s = bq.step(s, st);
        }
        out.push(s);
    }
    out
}

/// Zero-phase filter: forward, then time-reversed, then forward again, then
/// time-reversed back.
///
/// The two passes cancel phase delay so peaks stay at their original sample
/// indices, at the cost of squaring the magnitude response (a 4th-order
/// cascade behaves like 8th order). Returns an empty vector for empty input.
pub fn filter_zero_phase(coeffs: &[Biquad], x: &[f64]) -> Vec<f64> {
    if x.is_empty() {
        return Vec::new();
    }
    let forward = filter_forward(coeffs, x);
    let reversed: Vec<f64> = forward.into_iter().rev().collect();
    let twice = filter_forward(coeffs, &reversed);
    twice.into_iter().rev().collect()
}

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

    #[test]
    fn butterworth_passes_dc_unchanged() {
        let coeffs = butterworth4_lowpass(100.0, 5000.0);
        let x = vec![5.0; 500];
        let y = filter_zero_phase(&coeffs, &x);
        // After the brief edge transient, the filtered DC level should match.
        for v in &y[100..400] {
            assert!((v - 5.0).abs() < 1e-6, "DC drift: {}", v - 5.0);
        }
    }

    #[test]
    fn butterworth_attenuates_above_cutoff() {
        // 1 kHz sine through a 100 Hz LP at 5 kHz fs — should be heavily
        // attenuated. 4th-order Butterworth gives ~80 dB/decade; at 10×
        // cutoff that's >40 dB.
        let fs = 5000.0;
        let coeffs = butterworth4_lowpass(100.0, fs);
        let n = 2000;
        let x: Vec<f64> = (0..n)
            .map(|i| (2.0 * std::f64::consts::PI * 1000.0 * i as f64 / fs).sin())
            .collect();
        let y = filter_zero_phase(&coeffs, &x);
        let peak = y[500..1500].iter().map(|v| v.abs()).fold(0.0_f64, f64::max);
        assert!(peak < 0.01, "1 kHz not attenuated: peak={}", peak);
    }

    #[test]
    fn butterworth_preserves_well_below_cutoff() {
        // 10 Hz sine through a 100 Hz LP at 5 kHz fs — pass-band, ≈ unchanged.
        let fs = 5000.0;
        let coeffs = butterworth4_lowpass(100.0, fs);
        let n = 2000;
        let x: Vec<f64> = (0..n)
            .map(|i| (2.0 * std::f64::consts::PI * 10.0 * i as f64 / fs).sin())
            .collect();
        let y = filter_zero_phase(&coeffs, &x);
        // Compare in the middle so edge transients don't matter.
        let max_err = x[500..1500]
            .iter()
            .zip(&y[500..1500])
            .map(|(a, b)| (a - b).abs())
            .fold(0.0_f64, f64::max);
        assert!(max_err < 0.01, "10 Hz attenuated: max_err={}", max_err);
    }

    #[test]
    fn butterworth_zero_phase_does_not_shift_peak() {
        // A Gaussian-shaped pulse. The filtered version's peak should be at
        // the same sample index as the input's peak — that's the whole point
        // of zero-phase filtering.
        let fs = 5000.0;
        let coeffs = butterworth4_lowpass(100.0, fs);
        let n = 1000;
        let centre = 500usize;
        let sigma = 20.0;
        let x: Vec<f64> = (0..n)
            .map(|i| (-((i as f64 - centre as f64).powi(2)) / (2.0 * sigma * sigma)).exp())
            .collect();
        let y = filter_zero_phase(&coeffs, &x);
        let (peak_idx, _) = y
            .iter()
            .enumerate()
            .fold((0, f64::MIN), |acc, (i, &v)| if v > acc.1 { (i, v) } else { acc });
        assert_eq!(peak_idx, centre);
    }

    #[test]
    fn empty_input_returns_empty() {
        let coeffs = butterworth4_lowpass(40.0, 5000.0);
        assert!(filter_zero_phase(&coeffs, &[]).is_empty());
    }
}