DCBlocker.h

// DSPark - Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi - MIT License
 
#pragma once
 
#include "../Core/DspMath.h"
#include "../Core/Biquad.h"
#include "../Core/AudioSpec.h"
#include "../Core/AudioBuffer.h"
#include "../Core/StateBlob.h"
#include "../Core/detail/DcBlock.h"
 
#include <algorithm>
#include <array>
#include <atomic>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <numbers>
#include <vector>
 
namespace dspark {
 
template <FloatType T>
class DCBlocker
{
public:
    static constexpr int kMaxBiquadStages = 5;
    static constexpr int kMaxChannels = 16;
 
    DCBlocker() noexcept { forceUpdateCoefficients(); }
 
    ~DCBlocker() = default; // Non-virtual to avoid vtable injection
 
    void prepare(double sampleRate, int numChannels = 2, double cutoffHz = -1.0)
    {
        if (!(sampleRate > 0.0)) return;
        sampleRate_ = sampleRate;
        numChannels_ = std::clamp(numChannels, 1, kMaxChannels);
 
        reset();
 
        // Use the thread-safe setter to clamp and initialize safely
        if (cutoffHz > 0.0)
            setCutoff(static_cast<T>(cutoffHz));
        forceUpdateCoefficients();
    }
 
    void prepare(const AudioSpec& spec)
    {
        prepare(spec.sampleRate, spec.numChannels);
    }
 
    void setOrder(int order) noexcept
    {
        order_.store(std::clamp(order, 1, 10), std::memory_order_relaxed);
    }
 
    [[nodiscard]] int getOrder() const noexcept
    {
        return order_.load(std::memory_order_relaxed);
    }
 
    void setCutoff(T hz) noexcept
    {
        if (!std::isfinite(hz)) return;
        cutoffHz_.store(std::max(hz, T(1)), std::memory_order_relaxed);
    }
 
    [[nodiscard]] T getCutoff() const noexcept
    {
        return cutoffHz_.load(std::memory_order_relaxed);
    }
 
    void processBlock(AudioBufferView<T> buffer) noexcept
    {
        updateCoefficientsIfNeeded();
 
        // lastOrder_ is the order the live coefficients were designed for.
        // Reloading the atomic here could pick up a setOrder() that landed
        // after the rebuild and enable stages that hold no coefficients yet.
        const int currentOrder = lastOrder_;
        const int nCh = std::min(buffer.getNumChannels(), numChannels_);
        const int nS = buffer.getNumSamples();
        if (nS <= 0) return;
 
        if (currentOrder <= 1)
        {
            for (int ch = 0; ch < nCh; ++ch)
                runOnePole(buffer.getChannel(ch), nS, ch);
        }
        else
        {
            const int numStages = std::clamp(currentOrder / 2, 1, kMaxBiquadStages);
            for (int ch = 0; ch < nCh; ++ch)
                runCascade(buffer.getChannel(ch), nS, ch, numStages);
        }
    }
 
    void processBlock(int channel, T* data, int numSamples) noexcept
    {
        if (channel < 0 || channel >= kMaxChannels) return;
        if (data == nullptr || numSamples <= 0) return;
        updateCoefficientsIfNeeded();
        const int currentOrder = lastOrder_;
 
        if (currentOrder <= 1)
            runOnePole(data, numSamples, channel);
        else
            runCascade(data, numSamples, channel,
                       std::clamp(currentOrder / 2, 1, kMaxBiquadStages));
    }
 
    [[nodiscard]] T processSample(int channel, T input) noexcept
    {
        if (channel < 0 || channel >= kMaxChannels) return input;
        updateCoefficientsIfNeeded();
        const auto ch = static_cast<size_t>(channel);
        const double x = static_cast<double>(input);
 
        if (lastOrder_ <= 1)
            return static_cast<T>(stepOnePole(onePoleR_, onePole_[ch], x));
 
        const int numStages = std::clamp(lastOrder_ / 2, 1, kMaxBiquadStages);
        double v = x;
        for (int s = 0; s < numStages; ++s)
        {
            auto& sec = sections_[static_cast<size_t>(s)];
            v = stepSection(sec.b0, sec.a1, sec.a2, sec.state[ch], v);
        }
        return static_cast<T>(v);
    }
 
    void reset() noexcept
    {
        onePole_.fill(OnePoleState{});
        for (auto& s : sections_)
            s.state.fill(SectionState{});
    }
 
 
    [[nodiscard]] std::vector<uint8_t> getState() const
    {
        StateWriter w(stateId("DCBL"), 1);
        w.write("order", order_.load(std::memory_order_relaxed));
        w.write("cutoff", static_cast<float>(cutoffHz_.load(std::memory_order_relaxed)));
        return w.blob();
    }
 
    bool setState(const uint8_t* data, size_t size)
    {
        StateReader r(data, size);
        if (!r.isValid() || r.processorId() != stateId("DCBL")) return false;
        setOrder(r.read("order", 1));
        setCutoff(static_cast<T>(r.read("cutoff", 5.0f)));
        return true;
    }
 
protected:
    void updateCoefficientsIfNeeded() noexcept
    {
        // The ORDER is part of the design, not just a stage count: each order
        // has its own Q table row, and a larger order enables stages that may
        // never have received coefficients. Rebuilding only on cutoff changes
        // left a live setOrder() running a broken hybrid (old-Q first stage +
        // identity stages) until the cutoff happened to move.
        const T cutoff  = cutoffHz_.load(std::memory_order_relaxed);
        const int order = order_.load(std::memory_order_relaxed);
        if (cutoff != lastCutoff_ || order != lastOrder_)
        {
            forceUpdateCoefficients(cutoff);
        }
    }
 
    void forceUpdateCoefficients(T explicitCutoff = T(0)) noexcept
    {
        const T cutoff = explicitCutoff > T(0) ? explicitCutoff : cutoffHz_.load(std::memory_order_relaxed);
        const double fc = static_cast<double>(cutoff);
        // Loaded ONCE: reading order_ again below could pick up a concurrent
        // setOrder() and record a design (lastOrder_) whose stages were never
        // built, which the processing paths would then run as silent sections.
        const int order = order_.load(std::memory_order_relaxed);
 
        // 1-Pole update
        onePoleR_ = std::exp(-std::numbers::pi * 2.0 * fc / sampleRate_);
 
        // Biquad updates
        static constexpr float qTable[6][kMaxBiquadStages] = {
            {},                                                // index 0 (unused)
            { 0.7071f },                                       // order 2
            { 0.5412f, 1.3066f },                              // order 4
            { 0.5177f, 0.7071f, 1.9319f },                     // order 6
            { 0.5098f, 0.6013f, 0.8999f, 2.5628f },            // order 8
            { 0.5062f, 0.5612f, 0.7071f, 1.1013f, 3.1962f }    // order 10
        };
 
        const int tableIdx = std::clamp(order / 2, 1, 5);
        for (int s = 0; s < tableIdx; ++s)
        {
            // Designed in double and used as b0 * (1 - z^-1)^2 / A(z): the RBJ
            // high-pass numerator IS b0 * (1, -2, 1) - exactly so in binary
            // floating point, since halving and doubling are exact and all
            // three share the same 1/a0 factor - which is what lets the DC
            // zero be applied structurally in the inner loop.
            const auto c = BiquadCoeffs::makeHighPass(
                sampleRate_, fc, static_cast<double>(qTable[tableIdx][s]));
            assert(c.b1 == -2.0 * c.b0 && c.b2 == c.b0
                   && "RBJ high-pass numerator is no longer b0 * (1, -2, 1)");
 
            auto& sec = sections_[static_cast<size_t>(s)];
            sec.b0 = c.b0;
            sec.a1 = c.a1;
            sec.a2 = c.a2;
        }
 
        lastCutoff_ = cutoff;
        lastOrder_  = order;
    }
 
    // -- Double-precision filter core -----------------------------------------
 
    using OnePoleState = detail::DcBlockState;
    struct SectionState { double x1 = 0.0, x2 = 0.0, y1 = 0.0, y2 = 0.0; };
 
    struct Section
    {
        double b0 = 0.0, a1 = 0.0, a2 = 0.0;
        std::array<SectionState, kMaxChannels> state {};
    };
 
    static double stepOnePole(double r, OnePoleState& z, double x) noexcept
    {
        return detail::dcBlockStep(r, z, x);
    }
 
    static double stepSection(double b0, double a1, double a2, SectionState& z, double x) noexcept
    {
        // (x - x1) - (x1 - x2) is exactly zero for a constant input in IEEE
        // arithmetic, so steady DC never reaches the recursion at all.
        const double d = (x - z.x1) - (z.x1 - z.x2);
        const double y = b0 * d - a1 * z.y1 - a2 * z.y2;
        z.x2 = z.x1; z.x1 = x;
        z.y2 = z.y1; z.y1 = y;
        return y;
    }
 
    static constexpr double kDenormalFloor = 1e-30;
 
    static void flushDenormals(OnePoleState& z) noexcept
    {
        if (std::abs(z.y1) + std::abs(z.x1) < kDenormalFloor)
            z = OnePoleState{};
    }
 
    static void flushDenormals(SectionState& z) noexcept
    {
        if (std::abs(z.y1) + std::abs(z.y2) + std::abs(z.x1) + std::abs(z.x2) < kDenormalFloor)
            z = SectionState{};
    }
 
    void runOnePole(T* data, int numSamples, int channel) noexcept
    {
        const double r = onePoleR_;
        OnePoleState z = onePole_[static_cast<size_t>(channel)];
        for (int i = 0; i < numSamples; ++i)
            data[i] = static_cast<T>(stepOnePole(r, z, static_cast<double>(data[i])));
        flushDenormals(z);
        onePole_[static_cast<size_t>(channel)] = z;
    }
 
    void runCascade(T* data, int numSamples, int channel, int numStages) noexcept
    {
        // Coefficients and history are lifted into locals for the whole block:
        // the intermediate signal then stays in double across the cascade and
        // the compiler is free to keep the state in registers (writing to
        // data[i] would otherwise be assumed to alias the member state).
        double b0[kMaxBiquadStages] {}, a1[kMaxBiquadStages] {}, a2[kMaxBiquadStages] {};
        SectionState z[kMaxBiquadStages] {};
        const auto ch = static_cast<size_t>(channel);
        for (int s = 0; s < numStages; ++s)
        {
            const auto& sec = sections_[static_cast<size_t>(s)];
            b0[s] = sec.b0; a1[s] = sec.a1; a2[s] = sec.a2;
            z[s]  = sec.state[ch];
        }
 
        for (int i = 0; i < numSamples; ++i)
        {
            double v = static_cast<double>(data[i]);
            for (int s = 0; s < numStages; ++s)
                v = stepSection(b0[s], a1[s], a2[s], z[s], v);
            data[i] = static_cast<T>(v);
        }
 
        for (int s = 0; s < numStages; ++s)
        {
            flushDenormals(z[s]);
            sections_[static_cast<size_t>(s)].state[ch] = z[s];
        }
    }
 
    double sampleRate_ = 48000.0;
    int numChannels_ = 2;
    double onePoleR_ = 0.0;   
 
    std::atomic<int> order_ { 1 };
    std::atomic<T> cutoffHz_ { T(5) };
    T lastCutoff_ = T(-1);
    int lastOrder_ = -1;
 
    // Fixed-size per-channel state (scalar loops: no SIMD alignment needed).
    std::array<OnePoleState, kMaxChannels> onePole_ {};
    std::array<Section, kMaxBiquadStages> sections_ {};
};
 
} // namespace dspark