LadderFilter.h

// DSPark - Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi - MIT License
 
#pragma once
 
#include "DspMath.h"
#include "AudioSpec.h"
#include "AudioBuffer.h"
#include "DenormalGuard.h"
 
#include <algorithm>
#include <array>
#include <atomic>
#include <cmath>
 
namespace dspark {
 
template <FloatType T>
class LadderFilter
{
public:
    enum class Mode
    {
        LP6,    
        LP12,   
        LP18,   
        LP24,   
        BP12,   
        HP24    
    };
 
    ~LadderFilter() = default;
 
    // -- Lifecycle --------------------------------------------------------------
 
    void prepare(const AudioSpec& spec) noexcept
    {
        if (!(spec.sampleRate > 0.0)) return;
        spec_ = spec;
        updateCoefficients();
        reset();
    }
 
    void processBlock(AudioBufferView<T> buffer) noexcept
    {
        DenormalGuard guard;
        const int nCh = std::min(buffer.getNumChannels(), kMaxChannels);
        const int nS  = buffer.getNumSamples();
 
        // Local snapshot to guarantee thread-safety and avoid mid-block changes
        const T currentG = g_.load(std::memory_order_relaxed);
        const T currentRes = resonance_.load(std::memory_order_relaxed);
        const T currentDrive = drive_.load(std::memory_order_relaxed);
        const Mode currentMode = mode_.load(std::memory_order_relaxed);
 
        // Template dispatch to eliminate per-sample branching
        switch (currentMode)
        {
            case Mode::LP6:  processBlockInternal<Mode::LP6>(buffer, nCh, nS, currentG, currentRes, currentDrive); break;
            case Mode::LP12: processBlockInternal<Mode::LP12>(buffer, nCh, nS, currentG, currentRes, currentDrive); break;
            case Mode::LP18: processBlockInternal<Mode::LP18>(buffer, nCh, nS, currentG, currentRes, currentDrive); break;
            case Mode::LP24: processBlockInternal<Mode::LP24>(buffer, nCh, nS, currentG, currentRes, currentDrive); break;
            case Mode::BP12: processBlockInternal<Mode::BP12>(buffer, nCh, nS, currentG, currentRes, currentDrive); break;
            case Mode::HP24: processBlockInternal<Mode::HP24>(buffer, nCh, nS, currentG, currentRes, currentDrive); break;
        }
    }
 
    [[nodiscard]] T processSample(T input, int channel) noexcept
    {
        if (channel < 0 || channel >= kMaxChannels) return input;
 
        const T g     = g_.load(std::memory_order_relaxed);
        const T res   = resonance_.load(std::memory_order_relaxed);
        const T drive = drive_.load(std::memory_order_relaxed);
        const BlockCoeffs c = makeBlockCoeffs(g, res);
        ChannelState& s = state_[channel];
 
        switch (mode_.load(std::memory_order_relaxed))
        {
            case Mode::LP6:  return processSampleInternal<Mode::LP6>(input, s, c, drive);
            case Mode::LP12: return processSampleInternal<Mode::LP12>(input, s, c, drive);
            case Mode::LP18: return processSampleInternal<Mode::LP18>(input, s, c, drive);
            case Mode::LP24: return processSampleInternal<Mode::LP24>(input, s, c, drive);
            case Mode::BP12: return processSampleInternal<Mode::BP12>(input, s, c, drive);
            case Mode::HP24: return processSampleInternal<Mode::HP24>(input, s, c, drive);
        }
        return input;
    }
 
    void reset() noexcept
    {
        for (auto& s : state_)
            s.z.fill(T(0));
    }
 
    // -- Parameters -------------------------------------------------------------
 
    void setCutoff(T hz) noexcept
    {
        if (!std::isfinite(hz)) return;
        cutoff_.store(std::max(hz, T(0)), std::memory_order_relaxed);
        updateCoefficients();
    }
 
    void setResonance(T amount) noexcept
    {
        if (!std::isfinite(amount)) return;  // NaN would poison the loop via k
        resonance_.store(std::clamp(amount, T(0), T(1)), std::memory_order_relaxed);
    }
 
    void setDrive(T amount) noexcept
    {
        if (!std::isfinite(amount)) return;  // Inf turns 0 * drive into NaN
        drive_.store(std::max(amount, T(0.1)), std::memory_order_relaxed);
    }
 
    void setMode(Mode mode) noexcept
    {
        mode_.store(mode, std::memory_order_relaxed);
    }
 
    [[nodiscard]] T getCutoff() const noexcept { return cutoff_.load(std::memory_order_relaxed); }
    [[nodiscard]] T getResonance() const noexcept { return resonance_.load(std::memory_order_relaxed); }
    [[nodiscard]] T getDrive() const noexcept { return drive_.load(std::memory_order_relaxed); }
    [[nodiscard]] Mode getMode() const noexcept { return mode_.load(std::memory_order_relaxed); }
 
protected:
    static constexpr int kMaxChannels = 16;
 
    // Scalar per-channel state - no SIMD kernel ever touches it, and adjacent
    // channels sharing a cache line is better locality than padded isolation,
    // so it is deliberately not over-aligned. The stage tap outputs are NOT
    // state (each sample overwrites all of them), so they live in registers
    // inside processSampleInternal().
    struct ChannelState
    {
        std::array<T, 4> z {};  // TPT integrator states (z^-1 equivalents)
    };
 
    std::array<ChannelState, kMaxChannels> state_ {};
    AudioSpec spec_ {};
 
    // Atomic variables for thread-safety between Audio Thread and UI/Main Thread
    std::atomic<T> cutoff_ {T(1000)};
    std::atomic<T> resonance_ {T(0)};
    std::atomic<T> drive_ {T(1)};
    std::atomic<T> g_ {T(0)};
    std::atomic<Mode> mode_ {Mode::LP24};
 
private:
    void updateCoefficients() noexcept
    {
        if (!(spec_.sampleRate > 0.0)) return;
        const T clamped = std::clamp(cutoff_.load(std::memory_order_relaxed),
                                     T(20), static_cast<T>(spec_.sampleRate) * T(0.499));
        cutoff_.store(clamped, std::memory_order_relaxed);
        const T preWarpedGain = static_cast<T>(std::tan(pi<double> * static_cast<double>(clamped) / spec_.sampleRate));
        g_.store(preWarpedGain, std::memory_order_relaxed);
    }
 
    struct BlockCoeffs
    {
        T G, G2, G3, G4, ig, k, invFbDen;
    };
 
    [[nodiscard]] static BlockCoeffs makeBlockCoeffs(T g, T res) noexcept
    {
        BlockCoeffs c;
        c.G   = g / (T(1) + g);
        c.G2  = c.G * c.G;
        c.G3  = c.G2 * c.G;
        c.G4  = c.G3 * c.G;
        c.ig  = T(1) / (T(1) + g);
        c.k   = res * T(4);                         // k = 4 -> self-oscillation
        c.invFbDen = T(1) / (T(1) + c.k * c.G4);    // zero-delay loop denominator
        return c;
    }
 
    template <Mode FilterMode>
    void processBlockInternal(AudioBufferView<T> buffer, int nCh, int nS, T g, T res, T drive) noexcept
    {
        // Precompute the block-constant coefficients ONCE (previously these two
        // divisions ran for every sample of every channel).
        const BlockCoeffs c = makeBlockCoeffs(g, res);
 
        for (int ch = 0; ch < nCh; ++ch)
        {
            auto* channelData = buffer.getChannel(ch);
            auto& state = state_[ch];
 
            for (int i = 0; i < nS; ++i)
            {
                channelData[i] = processSampleInternal<FilterMode>(channelData[i], state, c, drive);
            }
        }
    }
 
    template <Mode FilterMode>
    [[nodiscard]] T processSampleInternal(T input, ChannelState& s, const BlockCoeffs& c, T drive) noexcept
    {
        // Estimate the LP24 output from the integrator states (zero-delay logic)
        const T Sest = c.G3 * c.ig * s.z[0]
                     + c.G2 * c.ig * s.z[1]
                     + c.G  * c.ig * s.z[2]
                     +        c.ig * s.z[3];
 
        // Saturate the estimated feedback (cheap ZDF nonlinearity: the state
        // contribution is shaped, the direct G4*u term stays linear).
        // fastTanh clamps its input, so the saturated loop stays bounded.
        T fbSignal = Sest;
        if (drive > T(1))
            fbSignal = fastTanh(fbSignal * drive) / drive;
 
        // Resolve zero-delay loop (denominator precomputed once per block)
        const T u = (input - c.k * fbSignal) * c.invFbDen;
 
        // Four cascaded TPT one-pole stages. Tap outputs carry no state
        // between samples, so they stay in registers ({} avoids a spurious
        // uninitialised-variable warning; the stores are dead after unroll).
        std::array<T, 4> y {};
        T x = u;
        for (int i = 0; i < 4; ++i)
        {
            const T v = (x - s.z[i]) * c.G;
            y[i] = v + s.z[i];
            s.z[i] = y[i] + v;
            x = y[i];
        }
 
        // Output tap mix (resolved at compile time per mode instantiation)
        if constexpr (FilterMode == Mode::LP6)       return y[0];
        else if constexpr (FilterMode == Mode::LP12) return y[1];
        else if constexpr (FilterMode == Mode::LP18) return y[2];
        else if constexpr (FilterMode == Mode::LP24) return y[3];
        else if constexpr (FilterMode == Mode::BP12) return y[0] - y[2];
        else
        {
            // HP24: apply the binomial (1-L)^4 to `u` (the ladder's true input,
            // post feedback), NOT to `input`. With resonance, input = u*(1+k) at
            // DC, so the input-based form leaked DC with gain k/(1+k) - e.g. 67%
            // of the DC passed straight through a "high-pass" at resonance 0.5.
            // With u the DC gain is exactly 0 and the resonant peak is preserved.
            return u - T(4)*y[0] + T(6)*y[1] - T(4)*y[2] + y[3];
        }
    }
};
 
} // namespace dspark