Phoenix/src/rta.rs

//! Realtime-analyzer filter primitives.
//!
//! Fractional-octave bands are designed as complete Butterworth bandpasses:
//! an analog low-pass prototype is transformed to a bandpass, pre-warped and
//! mapped with the bilinear transform, then emitted as distinct SOS sections.

pub const RTW_THIRD_OCTAVE_CENTERS: &[f32] = &[
    20.0, 25.0, 31.5, 40.0, 50.0, 63.0, 80.0, 100.0, 125.0, 160.0, 200.0, 250.0, 315.0, 400.0,
    500.0, 630.0, 800.0, 1_000.0, 1_250.0, 1_600.0, 2_000.0, 2_500.0, 3_150.0, 4_000.0, 5_000.0,
    6_300.0, 8_000.0, 10_000.0, 12_500.0, 16_000.0, 20_000.0,
];

/// Maps a rounded preferred label to its exact IEC base-ten center frequency.
pub fn exact_fractional_octave_center(nominal_hz: f32, bands_per_octave: usize) -> f32 {
    let bpo = bands_per_octave.max(1) as f32;
    let step = (bpo * (nominal_hz / 1_000.0).log10() / 0.3).round();
    1_000.0 * 10.0f32.powf(0.3 * step / bpo)
}

pub fn fractional_octave_edges(center_hz: f32, bands_per_octave: usize) -> (f32, f32) {
    let factor = 10.0f32.powf(3.0 / (20.0 * bands_per_octave.max(1) as f32));
    (center_hz / factor, center_hz * factor)
}

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

#[derive(Clone, Copy, Debug)]
struct Complex {
    re: f64,
    im: f64,
}

impl Complex {
    fn new(re: f64, im: f64) -> Self {
        Self { re, im }
    }

    fn add(self, other: Self) -> Self {
        Self::new(self.re + other.re, self.im + other.im)
    }

    fn sub(self, other: Self) -> Self {
        Self::new(self.re - other.re, self.im - other.im)
    }

    fn mul(self, other: Self) -> Self {
        Self::new(
            self.re * other.re - self.im * other.im,
            self.re * other.im + self.im * other.re,
        )
    }

    fn scale(self, value: f64) -> Self {
        Self::new(self.re * value, self.im * value)
    }

    fn div(self, other: Self) -> Self {
        let denom = other.re * other.re + other.im * other.im;
        Self::new(
            (self.re * other.re + self.im * other.im) / denom,
            (self.im * other.re - self.re * other.im) / denom,
        )
    }

    fn abs(self) -> f64 {
        (self.re * self.re + self.im * self.im).sqrt()
    }

    fn sqrt(self) -> Self {
        let magnitude = self.abs();
        let re = ((magnitude + self.re) * 0.5).max(0.0).sqrt();
        let im = ((magnitude - self.re) * 0.5)
            .max(0.0)
            .sqrt()
            .copysign(self.im);
        Self::new(re, im)
    }
}

fn prewarp(freq_hz: f64, sample_rate: f64) -> f64 {
    2.0 * sample_rate * (std::f64::consts::PI * freq_hz / sample_rate).tan()
}

fn bilinear_pole(pole: Complex, sample_rate: f64) -> Complex {
    let two_fs = Complex::new(2.0 * sample_rate, 0.0);
    two_fs.add(pole).div(two_fs.sub(pole))
}

/// Designs a 2N-order digital Butterworth bandpass from an N-order prototype.
/// `prototype_order = 3` produces the conventional sixth-order analyzer band.
pub fn design_fractional_octave_band(
    center_hz: f32,
    lower_hz: f32,
    upper_hz: f32,
    sample_rate: u32,
    prototype_order: usize,
) -> Vec<BiquadCoeffs> {
    let fs = f64::from(sample_rate.max(8_000));
    let nyquist = fs * 0.5;
    let lower = f64::from(lower_hz).clamp(0.01, nyquist * 0.999_8);
    let upper = f64::from(upper_hz).clamp(lower * 1.000_001, nyquist * 0.999_9);
    let order = prototype_order.clamp(1, 4);
    let omega_1 = prewarp(lower, fs);
    let omega_2 = prewarp(upper, fs);
    let bandwidth = omega_2 - omega_1;
    let omega_0_sq = omega_1 * omega_2;
    let mut positive_poles = Vec::with_capacity(order);

    for index in 0..order {
        let angle = std::f64::consts::PI * (2 * index + 1 + order) as f64 / (2 * order) as f64;
        let prototype_pole = Complex::new(angle.cos(), angle.sin());
        let bp = prototype_pole.scale(bandwidth);
        let discriminant = bp.mul(bp).sub(Complex::new(4.0 * omega_0_sq, 0.0)).sqrt();
        for root in [
            bp.add(discriminant).scale(0.5),
            bp.sub(discriminant).scale(0.5),
        ] {
            let digital = bilinear_pole(root, fs);
            if digital.im > 1.0e-10 {
                positive_poles.push(digital);
            }
        }
    }

    positive_poles.sort_by(|left, right| left.re.total_cmp(&right.re));
    let mut sections: Vec<BiquadCoeffs> = positive_poles
        .into_iter()
        .map(|pole| BiquadCoeffs {
            // Each section receives one zero at DC and one at Nyquist.
            b0: 1.0,
            b1: 0.0,
            b2: -1.0,
            a1: -2.0 * pole.re,
            a2: pole.re * pole.re + pole.im * pole.im,
        })
        .collect();

    if sections.is_empty() {
        return sections;
    }
    let magnitude = cascade_magnitude(&sections, center_hz, sample_rate).max(1.0e-30);
    let per_section_gain = (1.0 / magnitude).powf(1.0 / sections.len() as f64);
    for section in &mut sections {
        section.b0 *= per_section_gain;
        section.b1 *= per_section_gain;
        section.b2 *= per_section_gain;
    }
    sections
}

pub fn cascade_magnitude(sections: &[BiquadCoeffs], freq_hz: f32, sample_rate: u32) -> f64 {
    let omega = 2.0 * std::f64::consts::PI * f64::from(freq_hz) / f64::from(sample_rate.max(8_000));
    let z1 = Complex::new(omega.cos(), -omega.sin());
    let z2 = z1.mul(z1);
    sections.iter().fold(1.0, |magnitude, section| {
        let numerator = Complex::new(section.b0, 0.0)
            .add(z1.scale(section.b1))
            .add(z2.scale(section.b2));
        let denominator = Complex::new(1.0, 0.0)
            .add(z1.scale(section.a1))
            .add(z2.scale(section.a2));
        magnitude * numerator.div(denominator).abs()
    })
}

pub fn cascade_db(sections: &[BiquadCoeffs], freq_hz: f32, sample_rate: u32) -> f32 {
    (20.0
        * cascade_magnitude(sections, freq_hz, sample_rate)
            .max(1.0e-30)
            .log10()) as f32
}

pub fn integrate_power(
    previous: f64,
    block_power: f64,
    block_samples: usize,
    sample_rate: u32,
    tau_seconds: f32,
) -> f64 {
    let dt = block_samples as f64 / f64::from(sample_rate.max(1));
    let tau = f64::from(tau_seconds.max(0.001));
    let alpha = 1.0 - (-dt / tau).exp();
    previous + alpha * (block_power - previous)
}

#[derive(Clone, Copy)]
struct StereoBiquad {
    coeffs: BiquadCoeffs,
    z1_l: f64,
    z2_l: f64,
    z1_r: f64,
    z2_r: f64,
}

pub struct StereoCascade {
    sections: Vec<StereoBiquad>,
}

impl StereoCascade {
    pub fn new(coefficients: Vec<BiquadCoeffs>) -> Self {
        Self {
            sections: coefficients
                .into_iter()
                .map(|coeffs| StereoBiquad {
                    coeffs,
                    z1_l: 0.0,
                    z2_l: 0.0,
                    z1_r: 0.0,
                    z2_r: 0.0,
                })
                .collect(),
        }
    }

    pub fn process(&mut self, left: f32, right: f32) -> (f32, f32) {
        let mut out_l = f64::from(left);
        let mut out_r = f64::from(right);
        for section in &mut self.sections {
            out_l = section.process(out_l, false);
            out_r = section.process(out_r, true);
        }
        (out_l as f32, out_r as f32)
    }
}

impl StereoBiquad {
    fn process(&mut self, input: f64, right: bool) -> f64 {
        let (z1, z2) = if right {
            (&mut self.z1_r, &mut self.z2_r)
        } else {
            (&mut self.z1_l, &mut self.z2_l)
        };
        let output = self.coeffs.b0 * input + *z1;
        *z1 = self.coeffs.b1 * input - self.coeffs.a1 * output + *z2;
        *z2 = self.coeffs.b2 * input - self.coeffs.a2 * output;
        output
    }
}

pub struct FrequencyWeighting {
    mode: &'static str,
    cascade: StereoCascade,
}

impl FrequencyWeighting {
    pub fn new(mode: &str, sample_rate: u32) -> Self {
        let mode = normalize_weighting(mode);
        let cascade = StereoCascade::new(design_weighting(mode, sample_rate));
        Self { mode, cascade }
    }

    pub fn process(&mut self, left: f32, right: f32) -> (f32, f32) {
        if self.mode == "z" {
            return (left, right);
        }
        self.cascade.process(left, right)
    }
}

pub fn normalize_weighting(value: &str) -> &'static str {
    match value.trim().to_ascii_lowercase().as_str() {
        "a" => "a",
        "c" => "c",
        _ => "z",
    }
}

fn mapped_real_pole(freq_hz: f64, sample_rate: f64) -> f64 {
    let analog = prewarp(freq_hz.min(sample_rate * 0.499), sample_rate);
    (2.0 * sample_rate - analog) / (2.0 * sample_rate + analog)
}

fn real_pole_section(zero_1: f64, zero_2: f64, pole_1: f64, pole_2: f64) -> BiquadCoeffs {
    BiquadCoeffs {
        b0: 1.0,
        b1: -(zero_1 + zero_2),
        b2: zero_1 * zero_2,
        a1: -(pole_1 + pole_2),
        a2: pole_1 * pole_2,
    }
}

fn design_weighting(mode: &str, sample_rate: u32) -> Vec<BiquadCoeffs> {
    if mode == "z" {
        return Vec::new();
    }
    let fs = f64::from(sample_rate.max(8_000));
    let p1 = mapped_real_pole(20.598_997, fs);
    let p2 = mapped_real_pole(107.652_65, fs);
    let p3 = mapped_real_pole(737.862_23, fs);
    let p4 = mapped_real_pole(12_194.217, fs);
    let mut sections = if mode == "a" {
        vec![
            real_pole_section(1.0, 1.0, p1, p1),
            real_pole_section(1.0, 1.0, p2, p3),
            real_pole_section(-1.0, -1.0, p4, p4),
        ]
    } else {
        vec![
            real_pole_section(1.0, 1.0, p1, p1),
            real_pole_section(-1.0, -1.0, p4, p4),
        ]
    };
    let magnitude = cascade_magnitude(&sections, 1_000.0, sample_rate).max(1.0e-30);
    let gain = (1.0 / magnitude).powf(1.0 / sections.len() as f64);
    for section in &mut sections {
        section.b0 *= gain;
        section.b1 *= gain;
        section.b2 *= gain;
    }
    sections
}

pub fn weighting_response_db(mode: &str, freq_hz: f32, sample_rate: u32) -> f32 {
    cascade_db(
        &design_weighting(normalize_weighting(mode), sample_rate),
        freq_hz,
        sample_rate,
    )
}

pub fn weighting_reference_db(mode: &str, freq_hz: f32) -> f32 {
    if !freq_hz.is_finite() || freq_hz <= 0.0 {
        return 0.0;
    }
    let f2 = freq_hz * freq_hz;
    match normalize_weighting(mode) {
        "a" => {
            let numerator = (12_194.0f32 * 12_194.0) * f2 * f2;
            let denominator = (f2 + 20.6f32 * 20.6)
                * ((f2 + 107.7f32 * 107.7) * (f2 + 737.9f32 * 737.9)).sqrt()
                * (f2 + 12_194.0f32 * 12_194.0);
            20.0 * (numerator / denominator).max(1.0e-12).log10() + 2.0
        }
        "c" => {
            let numerator = (12_194.0f32 * 12_194.0) * f2;
            let denominator = (f2 + 20.6f32 * 20.6) * (f2 + 12_194.0f32 * 12_194.0);
            20.0 * (numerator / denominator).max(1.0e-12).log10() + 0.06
        }
        _ => 0.0,
    }
}

/// Corrects the finite-rate digital weighting filter at a band's center to
/// the standardized A/C reference curve. This is especially important near
/// Nyquist, where the bilinear filter necessarily bends toward zero.
pub fn weighting_power_correction(mode: &str, freq_hz: f32, sample_rate: u32) -> f64 {
    if normalize_weighting(mode) == "z" {
        return 1.0;
    }
    let difference =
        weighting_reference_db(mode, freq_hz) - weighting_response_db(mode, freq_hz, sample_rate);
    10.0f64.powf(f64::from(difference) / 10.0)
}

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

    #[test]
    fn third_octave_butterworth_has_correct_center_and_edges() {
        assert_eq!(RTW_THIRD_OCTAVE_CENTERS.len(), 31);
        for &nominal_center in RTW_THIRD_OCTAVE_CENTERS {
            let center = exact_fractional_octave_center(nominal_center, 3);
            let (lower, upper) = fractional_octave_edges(center, 3);
            let sections = design_fractional_octave_band(center, lower, upper, 48_000, 3);
            assert_eq!(sections.len(), 3);
            assert!(cascade_db(&sections, center, 48_000).abs() < 0.01);
            let lower_db = cascade_db(&sections, lower, 48_000);
            let upper_db = cascade_db(&sections, upper, 48_000);
            assert!(
                (lower_db + 3.0103).abs() < 0.08,
                "{center}: lower {lower_db}"
            );
            assert!(
                (upper_db + 3.0103).abs() < 0.08,
                "{center}: upper {upper_db}"
            );
        }
    }

    #[test]
    fn preferred_labels_map_to_exact_iec_centers() {
        assert!((exact_fractional_octave_center(31.5, 3) - 31.622_776).abs() < 0.000_1);
        assert_eq!(exact_fractional_octave_center(1_000.0, 3), 1_000.0);
        assert!((exact_fractional_octave_center(20_000.0, 3) - 19_952.623).abs() < 0.01);
        let center = exact_fractional_octave_center(1_250.0, 3);
        let (lower, upper) = fractional_octave_edges(center, 3);
        assert!(((lower * upper).sqrt() - center).abs() < 0.001);
    }

    #[test]
    fn neighboring_third_octave_centers_are_suppressed() {
        let factor = 2.0f32.powf(1.0 / 6.0);
        let sections =
            design_fractional_octave_band(1_000.0, 1_000.0 / factor, 1_000.0 * factor, 48_000, 3);
        assert!(cascade_db(&sections, 1_000.0 * 2.0f32.powf(1.0 / 3.0), 48_000) < -18.0);
        assert!(cascade_db(&sections, 1_000.0 * 2.0f32.powf(-1.0 / 3.0), 48_000) < -18.0);
    }

    #[test]
    fn design_tracks_sample_rate() {
        let factor = 2.0f32.powf(1.0 / 6.0);
        for sample_rate in [44_100, 48_000, 96_000] {
            let sections = design_fractional_octave_band(
                10_000.0,
                10_000.0 / factor,
                10_000.0 * factor,
                sample_rate,
                3,
            );
            assert!(cascade_db(&sections, 10_000.0, sample_rate).abs() < 0.01);
        }
    }

    #[test]
    fn power_integration_is_block_size_independent() {
        fn run(block_size: usize) -> f64 {
            let sample_rate = 48_000;
            let mut value = 0.0;
            let mut processed = 0usize;
            while processed < sample_rate as usize {
                let count = block_size.min(sample_rate as usize - processed);
                value = integrate_power(value, 1.0, count, sample_rate, 0.125);
                processed += count;
            }
            value
        }
        let reference = run(1);
        for block_size in [64, 128, 192, 512, 1_024] {
            let actual = run(block_size);
            assert!(
                (actual - reference).abs() < 1.0e-12,
                "block {block_size}: {actual} vs {reference}"
            );
        }
    }

    #[test]
    fn weighting_is_normalized_and_directionally_correct() {
        for mode in ["a", "c"] {
            assert!(weighting_response_db(mode, 1_000.0, 48_000).abs() < 0.01);
        }
        assert!(weighting_response_db("a", 31.5, 48_000) < -35.0);
        assert!(weighting_response_db("c", 31.5, 48_000) < -2.0);
        assert!(weighting_response_db("a", 8_000.0, 48_000) < 0.0);
        assert_eq!(weighting_response_db("z", 31.5, 48_000), 0.0);
    }

    #[test]
    fn weighting_center_calibration_tracks_standard_reference_points() {
        let a_points = [
            (31.5, -39.5),
            (63.0, -26.2),
            (125.0, -16.1),
            (1_000.0, 0.0),
            (8_000.0, -1.1),
            (16_000.0, -6.6),
        ];
        let c_points = [
            (31.5, -3.0),
            (63.0, -0.8),
            (1_000.0, 0.0),
            (8_000.0, -3.0),
            (16_000.0, -8.5),
        ];
        for (freq, expected) in a_points {
            let raw = weighting_response_db("a", freq, 48_000);
            let correction = 10.0 * weighting_power_correction("a", freq, 48_000).log10() as f32;
            let actual = raw + correction;
            assert!((actual - expected).abs() < 1.0, "A {freq} Hz: {actual} dB");
        }
        for (freq, expected) in c_points {
            let raw = weighting_response_db("c", freq, 48_000);
            let correction = 10.0 * weighting_power_correction("c", freq, 48_000).log10() as f32;
            let actual = raw + correction;
            assert!((actual - expected).abs() < 1.0, "C {freq} Hz: {actual} dB");
        }
    }

    #[test]
    fn weighting_time_domain_is_finite_and_normalized() {
        let sample_rate = 48_000;
        for freq in [31.5, 1_000.0, 16_000.0] {
            let mut weighting = FrequencyWeighting::new("a", sample_rate);
            let mut input_power = 0.0f64;
            let mut output_power = 0.0f64;
            for index in 0..sample_rate as usize * 2 {
                let input =
                    (2.0 * std::f32::consts::PI * freq * index as f32 / sample_rate as f32).sin();
                let (output, _) = weighting.process(input, input);
                assert!(output.is_finite());
                if index >= sample_rate as usize {
                    input_power += f64::from(input * input);
                    output_power += f64::from(output * output);
                }
            }
            let raw_db = 10.0 * (output_power / input_power).log10();
            let correction_db = 10.0 * weighting_power_correction("a", freq, sample_rate).log10();
            let calibrated_db = raw_db + correction_db;
            let expected = f64::from(weighting_reference_db("a", freq));
            assert!(
                (calibrated_db - expected).abs() < 0.05,
                "A {freq} Hz: {calibrated_db} dB vs {expected} dB"
            );
        }
    }

    fn measured_sine_gain(filter: &mut StereoCascade, freq: f32, sample_rate: u32) -> f32 {
        let total = sample_rate as usize * 3;
        let settle = sample_rate as usize * 2;
        let mut input_power = 0.0f64;
        let mut output_power = 0.0f64;
        for index in 0..total {
            let phase = 2.0 * std::f32::consts::PI * freq * index as f32 / sample_rate as f32;
            let input = phase.sin();
            let (output, _) = filter.process(input, input);
            if index >= settle {
                input_power += f64::from(input * input);
                output_power += f64::from(output * output);
            }
        }
        (10.0 * (output_power / input_power).log10()) as f32
    }

    #[test]
    fn time_domain_filter_matches_designed_response() {
        let factor = 2.0f32.powf(1.0 / 6.0);
        let coefficients =
            design_fractional_octave_band(1_000.0, 1_000.0 / factor, 1_000.0 * factor, 48_000, 3);
        let center = measured_sine_gain(
            &mut StereoCascade::new(coefficients.clone()),
            1_000.0,
            48_000,
        );
        let edge = measured_sine_gain(
            &mut StereoCascade::new(coefficients),
            1_000.0 * factor,
            48_000,
        );
        assert!(center.abs() < 0.02, "center gain {center}");
        assert!((edge + 3.0103).abs() < 0.08, "edge gain {edge}");
    }
}