bass-foundry/plugins/common/dsp/src/oversample.rs

//! 4× oversampling with two linear-phase half-band FIR stages. The round trip (up, then
//! down) delays the signal by a whole number of base-rate samples, `latency()`.
const STAGE1_HALF: usize = 31; // 63 taps at 2×: steep, carries the audio band edge
const STAGE2_HALF: usize = 11; // 23 taps at 4×: the band edge is far from its images
const BETA: f32 = 9.;

/// Kaiser-windowed half-band lowpass with gain `gain`, `2 * half + 1` taps.
fn halfband(half: usize, gain: f32) -> Vec<f32> {
    fn i0(x: f32) -> f32 {
        let (mut sum, mut term) = (1f64, 1f64);
        let y = (x as f64 / 2.).powi(2);
        for k in 1..50 {
            term *= y / (k * k) as f64;
            sum += term;
        }
        sum as f32
    }
    let n = 2 * half + 1;
    let norm = i0(BETA);
    let mut h: Vec<f32> = (0..n)
        .map(|i| {
            let m = i as f32 - half as f32;
            let sinc = if m == 0. {
                0.5
            } else {
                (std::f32::consts::FRAC_PI_2 * m).sin() / (std::f32::consts::PI * m)
            };
            let r = m / half as f32;
            sinc * i0(BETA * (1. - r * r).max(0.).sqrt()) / norm
        })
        .collect();
    let sum: f32 = h.iter().sum();
    for v in &mut h {
        *v *= gain / sum;
    }
    h
}

/// FIR with a doubled circular buffer so every convolution reads one contiguous slice.
struct Fir {
    taps: Vec<f32>,
    buf: Vec<f32>,
    at: usize,
}
impl Fir {
    fn new(taps: Vec<f32>) -> Self {
        let n = taps.len();
        Self {
            taps,
            buf: vec![0.; 2 * n],
            at: 0,
        }
    }
    #[inline]
    fn process(&mut self, x: f32) -> f32 {
        let n = self.taps.len();
        self.at = if self.at == 0 { n - 1 } else { self.at - 1 };
        self.buf[self.at] = x;
        self.buf[self.at + n] = x;
        let window = &self.buf[self.at..self.at + n];
        window.iter().zip(&self.taps).map(|(a, b)| a * b).sum()
    }
    fn reset(&mut self) {
        self.buf.fill(0.);
        self.at = 0;
    }
}

/// One channel of 4× oversampling. Allocation-free after `new`.
pub struct Oversampler {
    up1: Fir,
    up2: Fir,
    down2: Fir,
    down1: Fir,
    /// Two samples at 4× make the round trip a whole number of base samples.
    pad: [f32; 2],
}
impl Default for Oversampler {
    fn default() -> Self {
        Self::new()
    }
}
impl Oversampler {
    pub const FACTOR: usize = 4;
    pub fn new() -> Self {
        Self {
            up1: Fir::new(halfband(STAGE1_HALF, 2.)),
            up2: Fir::new(halfband(STAGE2_HALF, 2.)),
            down2: Fir::new(halfband(STAGE2_HALF, 1.)),
            down1: Fir::new(halfband(STAGE1_HALF, 1.)),
            pad: [0.; 2],
        }
    }
    /// Round-trip delay in base-rate samples.
    pub const fn latency() -> usize {
        STAGE1_HALF + STAGE2_HALF.div_ceil(2)
    }
    #[inline]
    pub fn up(&mut self, x: f32) -> [f32; 4] {
        let a = self.up1.process(x);
        let b = self.up1.process(0.);
        let mut out = [
            self.up2.process(a),
            self.up2.process(0.),
            self.up2.process(b),
            self.up2.process(0.),
        ];
        // Delay by two 4× samples.
        let [p0, p1] = self.pad;
        self.pad = [out[2], out[3]];
        out = [p0, p1, out[0], out[1]];
        out
    }
    #[inline]
    pub fn down(&mut self, y: [f32; 4]) -> f32 {
        let a = self.down2.process(y[0]);
        self.down2.process(y[1]);
        let b = self.down2.process(y[2]);
        self.down2.process(y[3]);
        let out = self.down1.process(a);
        self.down1.process(b);
        out
    }
    pub fn reset(&mut self) {
        self.up1.reset();
        self.up2.reset();
        self.down2.reset();
        self.down1.reset();
        self.pad = [0.; 2];
    }
}