codename-206/src/dsp/oversampler.rs

// SPDX-License-Identifier: Apache-2.0
//! 4× polyphase interpolation for **true-peak (inter-sample) detection only**.
//!
//! A band-limited signal can overshoot its sample values between samples, so the digital sample
//! peak under-reads the real (post-DAC) peak. We reconstruct the 4× grid with a polyphase
//! windowed-sinc interpolator and report only the maximum magnitude found — the interpolated
//! samples themselves are discarded. The audio path is untouched; this just feeds a better peak
//! estimate into the limiter.
//!
//! Speed: the prototype `PHASES * TAPS_PER_PHASE`-tap low-pass is split into `PHASES` sub-filters
//! of `TAPS_PER_PHASE` taps, each run at the base rate (no zero-stuffed multiplies). Cost is
//! `PHASES * TAPS_PER_PHASE` MACs per input sample per channel — about one base-rate FIR.
//!
//! The interpolation point sits at the centre of the tap window, so the estimate lags the input by
//! ~`TAPS_PER_PHASE/2` samples. That is far smaller than the limiter's look-ahead, which absorbs it
//! — so this adds no reported latency.

const PHASES: usize = 4;
const TAPS_PER_PHASE: usize = 12;
const MAX_CHANNELS: usize = 2;

/// Build the normalised polyphase coefficients: a windowed-sinc prototype split into `PHASES`
/// sub-filters, each normalised to unity DC gain so reconstruction preserves amplitude.
fn build_coefficients() -> [[f32; TAPS_PER_PHASE]; PHASES] {
    use std::f32::consts::PI;
    let n = PHASES * TAPS_PER_PHASE;
    let center = (n as f32 - 1.0) / 2.0;

    let mut proto = [0.0f32; PHASES * TAPS_PER_PHASE];
    for (m, p) in proto.iter_mut().enumerate() {
        // Sinc low-pass at the base-rate Nyquist (cutoff = 1/PHASES of the oversampled rate).
        let x = (m as f32 - center) / PHASES as f32;
        let sinc = if x.abs() < 1e-7 { 1.0 } else { (PI * x).sin() / (PI * x) };
        // Blackman window.
        let t = m as f32 / (n as f32 - 1.0);
        let window = 0.42 - 0.5 * (2.0 * PI * t).cos() + 0.08 * (4.0 * PI * t).cos();
        *p = sinc * window;
    }

    let mut coeffs = [[0.0f32; TAPS_PER_PHASE]; PHASES];
    for (phase, row) in coeffs.iter_mut().enumerate() {
        let mut sum = 0.0;
        for (k, c) in row.iter_mut().enumerate() {
            *c = proto[k * PHASES + phase];
            sum += *c;
        }
        if sum.abs() > 1e-12 {
            for c in row.iter_mut() {
                *c /= sum; // unity DC per phase -> amplitude-preserving
            }
        }
    }
    coeffs
}

pub struct Oversampler {
    coeffs: [[f32; TAPS_PER_PHASE]; PHASES],
    /// Per-channel circular history of the last `TAPS_PER_PHASE` input samples.
    history: Vec<[f32; TAPS_PER_PHASE]>,
    pos: usize,
}

impl Default for Oversampler {
    fn default() -> Self {
        Self {
            coeffs: build_coefficients(),
            history: Vec::new(),
            pos: 0,
        }
    }
}

impl Oversampler {
    pub fn new() -> Self {
        Self::default()
    }

    pub fn prepare(&mut self, num_channels: usize) {
        let channels = num_channels.clamp(1, MAX_CHANNELS);
        self.history = vec![[0.0; TAPS_PER_PHASE]; channels];
        self.reset();
    }

    pub fn reset(&mut self) {
        for ch in &mut self.history {
            *ch = [0.0; TAPS_PER_PHASE];
        }
        self.pos = 0;
    }

    /// Feed one input frame; return the maximum inter-sample magnitude across all channels and the
    /// 4× grid (the reconstructed samples are not kept).
    pub fn max_true_peak(&mut self, input: &[f32]) -> f32 {
        let n = input.len().min(self.history.len());
        let slot = self.pos % TAPS_PER_PHASE;
        for ch in 0..n {
            self.history[ch][slot] = input[ch];
        }

        // The tap index only depends on `pos` and `k`, not on `phase` or `ch` — hoist it out of
        // those loops instead of recomputing (and re-dividing) it PHASES*MAX_CHANNELS times over.
        let mut idx = [0usize; TAPS_PER_PHASE];
        for (k, slot) in idx.iter_mut().enumerate() {
            // k = 0 is the newest sample, increasing k goes back in time.
            *slot = (self.pos + TAPS_PER_PHASE - k) % TAPS_PER_PHASE;
        }

        let mut peak = 0.0f32;
        for ch in 0..n {
            let hist = &self.history[ch];
            for phase in &self.coeffs {
                let mut acc = 0.0f32;
                for (&c, &i) in phase.iter().zip(idx.iter()) {
                    acc += c * hist[i];
                }
                peak = peak.max(acc.abs());
            }
        }

        self.pos += 1;
        peak
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use std::f32::consts::PI;

    #[test]
    fn detects_inter_sample_overshoot() {
        // A unit sine at fs/4 phased so every sample lands at ±0.707 while the true peak is 1.0
        // (a classic ~3 dB inter-sample overshoot). The detector must see well above 0.707.
        let mut os = Oversampler::new();
        os.prepare(1);
        let mut detected = 0.0f32;
        for n in 0..2_000 {
            let x = (PI * n as f32 / 2.0 + PI / 4.0).sin(); // sin(πn/2 + π/4)
            detected = detected.max(os.max_true_peak(&[x]));
        }
        assert!(detected > 0.9, "missed inter-sample peak: {detected}");
        assert!(detected < 1.1, "implausible overshoot: {detected}");
    }

    #[test]
    fn preserves_amplitude_of_constant() {
        // Unity-DC normalisation: a constant signal reconstructs at its own level.
        let mut os = Oversampler::new();
        os.prepare(1);
        let mut last = 0.0f32;
        for _ in 0..200 {
            last = os.max_true_peak(&[0.5]);
        }
        assert!((last - 0.5).abs() < 0.02, "amplitude not preserved: {last}");
    }
}