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;
/// Length of the FIR support, i.e. how many past input frames an evaluation reads. A caller
/// bounding the true peak needs the max over exactly this many frames.
pub const SUPPORT_LEN: usize = TAPS_PER_PHASE;
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
}

/// Largest L1 norm across phases: `max_phase sum |c|`. Bounds the interpolator's output for a
/// bounded input, which is what makes the [`Oversampler::peak_gain`] early-out sound.
fn build_peak_gain(coeffs: &[[f32; TAPS_PER_PHASE]; PHASES]) -> f32 {
    coeffs
        .iter()
        .map(|row| row.iter().map(|c| c.abs()).sum::<f32>())
        .fold(0.0f32, f32::max)
}

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,
    /// Largest L1 norm across the phases. Since every interpolated sample is a dot product of
    /// one phase with the history, `|interpolated| <= peak_gain * max|history|` - a hard bound,
    /// which lets a caller prove the true peak cannot reach a ceiling without evaluating it.
    peak_gain: f32,
}

impl Default for Oversampler {
    fn default() -> Self {
        let coeffs = build_coefficients();
        Self {
            peak_gain: build_peak_gain(&coeffs),
            coeffs,
            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).
    /// Upper bound on `|interpolated|` for an input bounded by 1.0. See [`Self::peak_gain`].
    #[inline(always)]
    pub fn peak_gain(&self) -> f32 {
        self.peak_gain
    }

    /// Feed one input frame into the history. Cheap - a write per channel plus a counter.
    /// Must be called for EVERY frame even when [`Self::evaluate`] is skipped, or the FIR
    /// history develops holes and later evaluations read stale samples.
    #[inline]
    pub fn push(&mut self, input: &[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];
        }
        self.pos += 1;
    }

    /// The actual polyphase evaluation: `PHASES * TAPS_PER_PHASE` multiply-adds per channel.
    /// This is the expensive half, and the only half a caller can skip.
    #[inline]
    pub fn evaluate(&self, channels: usize) -> f32 {
        let n = channels.min(self.history.len());
        // 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.
        // `pos` already advanced past the newest sample in push(), hence the -1.
        let base = self.pos + TAPS_PER_PHASE - 1;
        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 = (base - 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());
            }
        }
        peak
    }

    /// Push a frame and evaluate it unconditionally. The real path splits these so the
    /// evaluation can be skipped (see `Limiter::process`); this is kept for tests that want
    /// the true peak of every frame regardless of any ceiling.
    #[cfg(test)]
    pub fn max_true_peak(&mut self, input: &[f32]) -> f32 {
        let n = input.len().min(self.history.len());
        self.push(input);
        self.evaluate(n)
    }
}

#[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}");
    }

    /// The early-out in `Limiter::process` is only sound if `evaluate()` can never exceed
    /// `peak_gain * max|history|`. Assert that directly against adversarial input - full-scale
    /// square waves and alternating impulses maximise the interpolator's overshoot.
    #[test]
    fn evaluate_never_exceeds_peak_gain_bound() {
        let mut os = Oversampler::default();
        os.prepare(2);
        let mut rng: u32 = 0xC0FF_EE01;
        let mut worst_ratio = 0.0f32;
        for i in 0..20_000 {
            rng = rng.wrapping_mul(1_664_525).wrapping_add(1_013_904_223);
            // Mix of adversarial shapes: alternating full scale, impulses, and noise.
            let x = match i % 3 {
                0 => if (i / 3) % 2 == 0 { 1.0 } else { -1.0 },
                1 => if i % 97 == 0 { 1.0 } else { 0.0 },
                _ => (rng >> 8) as f32 / (1 << 23) as f32 - 1.0,
            };
            let frame = [x, -x];
            os.push(&frame);
            let got = os.evaluate(2);
            // Bound uses the max over the FIR support, which for these inputs is at most 1.0.
            let bound = os.peak_gain() * 1.0;
            assert!(
                got <= bound + 1e-4,
                "evaluate() = {got} exceeded peak_gain bound {bound} at i={i}"
            );
            if bound > 0.0 {
                worst_ratio = worst_ratio.max(got / bound);
            }
        }
        // Sanity: the bound must be reachable enough to be useful, not vacuously huge.
        assert!(worst_ratio > 0.3, "bound looks far too loose (worst ratio {worst_ratio})");
    }
}