Oversampling.h

// DSPark -- Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi -- MIT License
 
#pragma once
 
#include "AudioBuffer.h"
#include "AudioSpec.h"
#include "FIRFilter.h"
#include "SimdOps.h"
 
#include <algorithm>
#include <array>
#include <cassert>
#include <cstring>
#include <vector>
 
namespace dspark {
 
template <typename T, int MaxChannels = 16>
class Oversampling
{
public:
    enum class Quality
    {
        Low,      
        Medium,   
        High,     
        Maximum   
    };
 
    explicit Oversampling(int factor = 2, Quality quality = Quality::High)
        : quality_(quality)
    {
        assert(factor >= 1 && (factor & (factor - 1)) == 0 && "Factor must be a power of 2");
        assert(factor <= (1 << kMaxStages) && "Factor must be <= 16 (kMaxStages stages)");
        // Release-safe normalisation: clamp into [1, 2^kMaxStages], then derive
        // the stage count and re-derive factor_ = 2^numStages_ so the pair stays
        // coherent even for a non-power-of-two request. An incoherent pair
        // (e.g. factor_ = 3 driving one 2x stage) would make downsample() emit
        // factor_/2 samples per base sample -- overrunning the caller's output
        // buffer -- and overflow the per-stage histories sized for 2^numStages_.
        factor = std::clamp(factor, 1, 1 << kMaxStages);
        numStages_ = 0;
        while ((1 << (numStages_ + 1)) <= factor)
            ++numStages_;
        factor_ = 1 << numStages_;
    }
 
    void prepare(const AudioSpec& spec)
    {
        baseSpec_ = spec;
        upBuffer_.resize(spec.numChannels, spec.maxBlockSize * factor_);
 
        const int taps = tapsForQuality(quality_);
        const T beta = betaForQuality(quality_);
 
        for (int stage = 0; stage < numStages_; ++stage)
        {
            // The max block size entering a stage is baseSize * 2^stage
            int stageMaxSamples = spec.maxBlockSize * (1 << stage);
            filters_[stage].design(taps, beta, spec.numChannels, stageMaxSamples);
        }
 
        reset();
    }
 
    void reset() noexcept
    {
        upBuffer_.clear();
        for (int i = 0; i < numStages_; ++i)
            filters_[i].reset();
    }
 
    [[nodiscard]] int getFactor() const noexcept { return factor_; }
 
    [[nodiscard]] Quality getQuality() const noexcept { return quality_; }
 
    [[nodiscard]] int getLatency() const noexcept
    {
        if (numStages_ == 0) return 0;
        const int halfOrder = (tapsForQuality(quality_) - 1) / 2;
        // Exact up->down round-trip group delay at the base rate. Each 2x half-band
        // stage contributes (halfOrder+1) high-rate samples on the way up and the
        // same on the way down; cascaded over numStages_ and referred to the base
        // rate this telescopes to 2*(halfOrder+1)*(factor-1)/factor. Verified
        // sample-exact against an impulse round-trip for factors 2/4/8/16 and all
        // quality presets. (halfOrder+1 and factor are powers of two -> exact.)
        return 2 * (halfOrder + 1) * (factor_ - 1) / factor_;
    }
 
    [[nodiscard]] AudioBufferView<T> upsample(AudioBufferView<const T> input) noexcept
    {
        const int nCh = std::min(input.getNumChannels(), upBuffer_.getNumChannels());
        // Release-safe clamp: a block larger than prepare()'s maxBlockSize would
        // overflow the per-stage histories and the internal high-rate buffer.
        const int nS  = std::min(input.getNumSamples(), baseSpec_.maxBlockSize);
 
        if (numStages_ == 0)
        {
            for (int ch = 0; ch < nCh; ++ch)
                std::memcpy(upBuffer_.getChannel(ch), input.getChannel(ch), static_cast<std::size_t>(nS) * sizeof(T));
            return upBuffer_.toView().getSubView(0, nS);
        }
 
        // Stage 0: Read directly from input view
        filters_[0].processUpsample(input, upBuffer_.toView(), nCh, nS);
 
        // Subsequent stages: in-place upsampling inside upBuffer_
        int currentLen = nS * 2;
        for (int stage = 1; stage < numStages_; ++stage)
        {
            filters_[stage].processUpsampleInPlace(upBuffer_.toView(), nCh, currentLen);
            currentLen *= 2;
        }
 
        return upBuffer_.toView().getSubView(0, nS * factor_);
    }
 
    void downsample(AudioBufferView<T> output) noexcept
    {
        const int nCh = std::min(output.getNumChannels(), upBuffer_.getNumChannels());
        const int nS  = std::min(output.getNumSamples(), baseSpec_.maxBlockSize);
 
        if (numStages_ == 0)
        {
            for (int ch = 0; ch < nCh; ++ch)
                std::memcpy(output.getChannel(ch), upBuffer_.getChannel(ch), static_cast<std::size_t>(nS) * sizeof(T));
            return;
        }
 
        int currentLen = nS * factor_;
 
        for (int stage = numStages_ - 1; stage > 0; --stage)
        {
            filters_[stage].processDownsampleInPlace(upBuffer_.toView(), nCh, currentLen);
            currentLen /= 2;
        }
 
        // Stage 0: Write directly to final output
        filters_[0].processDownsample(upBuffer_.toView(), output, nCh, currentLen);
 
        // Level transparency note: no gain compensation is applied (or needed).
        // An impulse round-trip peaks slightly below 1.0 only because the
        // impulse contains supra-Nyquist energy the anti-alias filters must
        // remove; in-band sine-wave gain is already ~1.000 (verified to better
        // than 0.01 dB across all quality presets).
    }
 
private:
    static constexpr int kMaxStages = 4;
 
    // ========================================================================
    // Polyphase Block Half-Band FIR
    // ========================================================================
    struct PolyphaseHalfBand
    {
        std::vector<T> evenTaps;   
        T centerTap = T(0);
        int halfOrder = 0;
        int delaySamples = 0;
 
        struct ChannelState {
            std::vector<T> history;    
            std::vector<T> evenHist;   
            std::vector<T> oddHist;    
        };
        std::vector<ChannelState> upChannels;
        std::vector<ChannelState> downChannels;
        std::vector<T> scratch;        
 
        void design(int taps, T beta, int numChannels, int maxBlockSamples)
        {
            halfOrder = (taps - 1) / 2;
            // INVARIANT: the polyphase split below collects the array-even taps,
            // which coincide with the non-zero (odd-from-centre) half-band taps
            // ONLY when halfOrder is odd. All quality presets (taps 31/63/127/255
            // -> halfOrder 15/31/63/127) satisfy this. If you add a preset, keep
            // (taps-1)/2 odd or the filter degenerates to silence.
            assert((halfOrder & 1) == 1 && "half-band polyphase requires odd halfOrder");
            // Pure-delay alignment for the centre-tap (odd) phase. With the full
            // even branch (halfOrder+1 taps, group delay (halfOrder)/2 = numTaps/2
            // base samples) the centre sample must sit numTaps/2 = (halfOrder+1)/2
            // samples into the window to stay phase-aligned with the even phase.
            delaySamples = (halfOrder + 1) / 2;
 
            auto fullCoeffs = FIRDesign<T>::lowPass(1.0, 0.25, taps, beta);
            centerTap = fullCoeffs[static_cast<std::size_t>(halfOrder)];
 
            // The non-zero taps: all even array indices (the centre sits at the
            // odd index halfOrder, and the other odd indices are the half-band
            // zeros).
            evenTaps.clear();
            for (int i = 0; i < taps; i += 2)
                evenTaps.push_back(fullCoeffs[static_cast<std::size_t>(i)]);
 
            // Up history: base-rate (numTaps + block). Down histories: halfOrder
            // samples of prefix plus up to maxBlockSamples new samples per phase
            // (a stage's decimator input is 2 * maxBlockSamples long).
            const int upHistorySize   = static_cast<int>(evenTaps.size()) + maxBlockSamples;
            const int downHistorySize = halfOrder + maxBlockSamples;
 
            upChannels.resize(static_cast<std::size_t>(numChannels));
            downChannels.resize(static_cast<std::size_t>(numChannels));
 
            for (int ch = 0; ch < numChannels; ++ch)
            {
                upChannels[ch].history.assign(static_cast<std::size_t>(upHistorySize), T(0));
                downChannels[ch].evenHist.assign(static_cast<std::size_t>(downHistorySize), T(0));
                downChannels[ch].oddHist.assign(static_cast<std::size_t>(downHistorySize), T(0));
            }
            scratch.assign(static_cast<std::size_t>(maxBlockSamples), T(0));
        }
 
        void reset() noexcept
        {
            for (auto& ch : upChannels)
                std::fill(ch.history.begin(), ch.history.end(), T(0));
            for (auto& ch : downChannels)
            {
                std::fill(ch.evenHist.begin(), ch.evenHist.end(), T(0));
                std::fill(ch.oddHist.begin(), ch.oddHist.end(), T(0));
            }
        }
 
        void upsampleChannel(const T* src, T* dst, ChannelState& state, int n) noexcept
        {
            constexpr int W = simd::kVecWidth<T>;
            using O = simd::Vec<T, W>;
            const int numTaps = static_cast<int>(evenTaps.size());
            auto& hist = state.history;
 
            // Append the new block after the fixed numTaps-sample history prefix.
            std::memmove(hist.data() + numTaps, src, static_cast<std::size_t>(n) * sizeof(T));
 
            simd::firCorrelate(hist.data(), evenTaps.data(), numTaps, scratch.data(), n, T(2));
 
            const T* centre = hist.data() + numTaps - delaySamples;
            const T centre2 = centerTap * T(2);
            const auto c2 = O::set1(centre2);
            int i = 0;
            for (; i + W <= n; i += W)
                O::storeInterleave2(dst + 2 * i, O::load(scratch.data() + i),
                                    O::mul(O::load(centre + i), c2));
            for (; i < n; ++i)
            {
                dst[2 * i] = scratch[static_cast<std::size_t>(i)];
                dst[2 * i + 1] = centre[i] * centre2;
            }
 
            // Save the trailing numTaps input samples for the next block. Using
            // the current n keeps this correct under variable block sizes.
            std::memmove(hist.data(), hist.data() + n, static_cast<std::size_t>(numTaps) * sizeof(T));
        }
 
        void downsampleChannel(const T* src, T* dst, ChannelState& state, int len) noexcept
        {
            constexpr int W = simd::kVecWidth<T>;
            using O = simd::Vec<T, W>;
            const int outLen = len / 2;
            const int numTaps = static_cast<int>(evenTaps.size());
            T* even = state.evenHist.data();
            T* odd = state.oddHist.data();
 
            // Split the new samples after the halfOrder-sample prefixes.
            int m = 0;
            for (; m + W <= outLen; m += W)
            {
                typename O::V e, o;
                O::loadDeinterleave(src + 2 * m, e, o);
                O::store(even + halfOrder + m, e);
                O::store(odd + halfOrder + m, o);
            }
            for (; m < outLen; ++m)
            {
                even[halfOrder + m] = src[2 * m];
                odd[halfOrder + m] = src[2 * m + 1];
            }
 
            simd::firCorrelate(even, evenTaps.data(), numTaps, dst, outLen, T(1));
 
            // The centre tap meets x[2k + halfOrder] = odd[k + (halfOrder - 1) / 2].
            const T* centre = odd + (halfOrder - 1) / 2;
            const auto c = O::set1(centerTap);
            int k = 0;
            for (; k + W <= outLen; k += W)
                O::store(dst + k, O::madd(c, O::load(centre + k), O::load(dst + k)));
            for (; k < outLen; ++k)
            {
                // Keep the vector multiply-add rounding at block tails too.
                T lanes[W];
                O::store(lanes, O::madd(c, O::set1(centre[k]), O::set1(dst[k])));
                dst[k] = lanes[0];
            }
 
            std::memmove(even, even + outLen, static_cast<std::size_t>(halfOrder) * sizeof(T));
            std::memmove(odd, odd + outLen, static_cast<std::size_t>(halfOrder) * sizeof(T));
        }
 
        void processUpsample(AudioBufferView<const T> input, AudioBufferView<T> output, int nCh, int nS) noexcept
        {
            for (int ch = 0; ch < nCh; ++ch)
                upsampleChannel(input.getChannel(ch), output.getChannel(ch),
                                upChannels[static_cast<std::size_t>(ch)], nS);
        }
 
        void processUpsampleInPlace(AudioBufferView<T> buffer, int nCh, int currentLen) noexcept
        {
            for (int ch = 0; ch < nCh; ++ch)
                upsampleChannel(buffer.getChannel(ch), buffer.getChannel(ch),
                                upChannels[static_cast<std::size_t>(ch)], currentLen);
        }
 
        void processDownsample(AudioBufferView<const T> input, AudioBufferView<T> output, int nCh, int currentLen) noexcept
        {
            for (int ch = 0; ch < nCh; ++ch)
                downsampleChannel(input.getChannel(ch), output.getChannel(ch),
                                  downChannels[static_cast<std::size_t>(ch)], currentLen);
        }
 
        void processDownsampleInPlace(AudioBufferView<T> buffer, int nCh, int currentLen) noexcept
        {
            for (int ch = 0; ch < nCh; ++ch)
                downsampleChannel(buffer.getChannel(ch), buffer.getChannel(ch),
                                  downChannels[static_cast<std::size_t>(ch)], currentLen);
        }
    };
 
    static constexpr int tapsForQuality(Quality q) noexcept
    {
        switch (q)
        {
            case Quality::Low:     return 31;
            case Quality::Medium:  return 63;
            case Quality::High:    return 127;
            case Quality::Maximum: return 255;
        }
        return 127;
    }
 
    static constexpr T betaForQuality(Quality q) noexcept
    {
        switch (q)
        {
            case Quality::Low:     return T(3.395);
            case Quality::Medium:  return T(5.653);
            case Quality::High:    return T(7.857);
            case Quality::Maximum: return T(10.056);
        }
        return T(7.857);
    }
 
    int factor_;
    int numStages_;
    Quality quality_;
    AudioSpec baseSpec_ {};
    AudioBuffer<T, MaxChannels> upBuffer_;
 
    std::array<PolyphaseHalfBand, kMaxStages> filters_;
};
 
} // namespace dspark