StateVariableFilter.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 "detail/TptSvf.h"
 
#include <algorithm>
#include <array>
#include <cmath>
 
namespace dspark {
 
template <FloatType T>
class StateVariableFilter
{
public:
    enum class Mode
    {
        LowPass,    
        HighPass,   
        BandPass,   
        Notch,      
        AllPass,    
        Bell,       
        LowShelf,   
        HighShelf   
    };
 
    struct MultiOutput
    {
        T lowpass;
        T highpass;
        T bandpass;
    };
 
    ~StateVariableFilter() = 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();
 
        // Channel-outer / sample-inner: keeps one channel's state and buffer hot
        // in cache for the whole inner loop (cache-friendly, matches LadderFilter).
        for (int ch = 0; ch < nCh; ++ch)
        {
            T* data = buffer.getChannel(ch);
            for (int i = 0; i < nS; ++i)
                data[i] = processOne(data[i], ch);
        }
    }
 
    [[nodiscard]] T processSample(T input, int channel) noexcept
    {
        if (channel < 0 || channel >= kMaxChannels) return input;
        return processOne(input, channel);
    }
 
    [[nodiscard]] MultiOutput processMultiOutput(T input, int channel) noexcept
    {
        if (channel < 0 || channel >= kMaxChannels) return { input, T(0), T(0) };
        return processCore(input, channel);
    }
 
    void reset() noexcept
    {
        for (auto& s : state_)
        {
            s.ic1eq = T(0);
            s.ic2eq = T(0);
        }
    }
 
    // -- Parameters (Level 1) ---------------------------------------------------
 
    void setCutoff(T hz) noexcept
    {
        if (!std::isfinite(hz)) return;
        cutoff_ = std::max(hz, T(0));
        updateCoefficients();
    }
 
    void setResonance(T resonance) noexcept
    {
        if (!std::isfinite(resonance)) return;  // NaN would poison R_
        resonance_ = std::clamp(resonance, T(0), T(1));
        // Map 0-1 to Q: 0.5 (wide) to 50 (near self-osc). R = 1/(2*Q).
        const T Q = T(0.5) + resonance_ * T(49.5);
        R_ = T(1) / (T(2) * Q);
        updateBellR();
        updateDerivedCoeffs();
    }
 
    void setQ(T q) noexcept
    {
        if (!std::isfinite(q)) return;  // NaN would poison R_
        q = std::max(q, T(0.01));
        R_ = T(1) / (T(2) * q);
        resonance_ = std::clamp((q - T(0.5)) / T(49.5), T(0), T(1));
        updateBellR();
        updateDerivedCoeffs();
    }
 
    void setMode(Mode mode) noexcept
    {
        mode_ = mode;
        updateDerivedCoeffs();
    }
 
    void setGain(T dB) noexcept
    {
        if (!std::isfinite(dB)) return;  // NaN/Inf would poison A_
        gainDb_ = dB;
        A_ = std::pow(T(10), std::abs(dB) / T(40)); // sqrt(linear gain)
        updateBellR();
        updateDerivedCoeffs();
    }
 
    // -- Getters ----------------------------------------------------------------
 
    [[nodiscard]] T getCutoff() const noexcept { return cutoff_; }
    [[nodiscard]] T getResonance() const noexcept { return resonance_; }
    [[nodiscard]] T getQ() const noexcept { return T(1) / (T(2) * R_); }
    [[nodiscard]] T getGain() const noexcept { return gainDb_; }
    [[nodiscard]] Mode getMode() const noexcept { return mode_; }
 
protected:
    static constexpr int kMaxChannels = 16;
 
    using ChannelState = detail::TptSvfState<T>;
 
    std::array<ChannelState, kMaxChannels> state_ {};
    AudioSpec spec_ {};
    T cutoff_    = T(1000);
    T resonance_ = T(0);
    T gainDb_    = T(0);
    T g_  = T(0);    // tan(pi * fc / fs) - TPT coefficient
    T R_  = T(1);    // 1/(2*Q) - damping
    T Rbell_ = T(1); // Mode-specific R for Bell (Zavalishin gain-dependent damping)
    T A_  = T(1);    // sqrt(gain) for shelving/bell
    Mode mode_ = Mode::LowPass;
 
    // Cached derived coefficients (recomputed by updateDerivedCoeffs()).
    T effG_ = T(0);  // possibly shelf-prewarped g
    T effR_ = T(1);  // mode-effective damping (R_ or Rbell_)
    T a1_ = T(1), a2_ = T(0), a3_ = T(0);
 
private:
    void updateCoefficients() noexcept
    {
        if (spec_.sampleRate > 0)
        {
            cutoff_ = std::clamp(cutoff_, T(20),
                                 static_cast<T>(spec_.sampleRate) * T(0.499));
            g_ = static_cast<T>(std::tan(pi<double> * static_cast<double>(cutoff_)
                                         / spec_.sampleRate));
        }
        updateBellR();
        updateDerivedCoeffs();
    }
 
    void updateBellR() noexcept
    {
        // Zavalishin Bell EQ topology: modify damping by gain factor A
        // (A_ >= 1 always - setGain uses |dB| and rejects non-finite input).
        // Boost: decrease R (increase Q) => narrower peak compensates for gain spread
        // Cut:   increase R (decrease Q) => wider notch compensates for gain spread
        if (gainDb_ >= T(0))
            Rbell_ = R_ / A_;
        else
            Rbell_ = R_ * A_;
    }
 
    void updateDerivedCoeffs() noexcept
    {
        effR_ = (mode_ == Mode::Bell) ? Rbell_ : R_;
 
        T gEff = g_;
        if (mode_ == Mode::LowShelf || mode_ == Mode::HighShelf)
        {
            const T sqrtAs = std::sqrt((gainDb_ >= T(0)) ? A_ : T(1) / A_);
            gEff = (mode_ == Mode::LowShelf) ? g_ / sqrtAs : g_ * sqrtAs;
        }
 
        effG_ = gEff;
        a1_ = T(1) / (T(1) + T(2) * effR_ * gEff + gEff * gEff);
        a2_ = gEff * a1_;
        a3_ = gEff * a2_;
    }
 
    [[nodiscard]] T processOne(T input, int channel) noexcept
    {
        const MultiOutput m = processCore(input, channel);
        return selectOutput(input, m.lowpass, m.highpass, m.bandpass);
    }
 
    [[nodiscard]] MultiOutput processCore(T input, int channel) noexcept
    {
        const auto output = detail::tptSvfStep(input, state_[channel], a1_, a2_, a3_);
        return { output.lowpass, input - T(2) * effR_ * output.bandpass - output.lowpass,
                 output.bandpass };
    }
 
    [[nodiscard]] T selectOutput(T input, T lp, T hp, T bp) const noexcept
    {
        switch (mode_)
        {
            case Mode::LowPass:  return lp;
            case Mode::HighPass: return hp;
            case Mode::BandPass: return bp;
            case Mode::Notch:    return lp + hp;       // LP + HP = Notch
            case Mode::AllPass:  return lp + hp - T(2) * R_ * bp;  // HP - 2R*BP + LP
            case Mode::Bell:
            {
                // Bell EQ (Zavalishin ch. 4): bandwidth-correct topology.
                // processCore already used Rbell_ for the SVF damping,
                // so the BP bandwidth is correct. Apply gain via mixing:
                //   boost: output = input + (A^2 - 1) * 2*Rbell * BP
                //   cut:   output = input + (1/A^2 - 1) * 2*Rbell * BP
                // At the centre frequency 2*Rbell*bp == input, so the mix factor
                // must be (A^2 - 1) to reach gain A^2 (boost) and (1/A^2 - 1) to
                // reach gain 1/A^2 (cut). The cut term must be NEGATIVE - the old
                // (1 - 1/A^2) was positive and boosted instead of cutting.
                const T k = T(2) * Rbell_;
                return (gainDb_ >= T(0))
                    ? input + (A_ * A_ - T(1)) * k * bp
                    : input + (T(1) / (A_ * A_) - T(1)) * k * bp;
            }
            case Mode::LowShelf:
            {
                // Canonical Simper low shelf. With As = A_ (boost) or 1/A_
                // (cut), where A_ = 10^(|dB|/40) = sqrt(linear gain), and the
                // pre-warped g from updateDerivedCoeffs():
                //   out = As^2 * LP + 2R * As * BP + HP
                // DC gain = As^2 (= linear gain), Nyquist gain = 1, and the
                // half-gain point sits exactly on the nominal frequency.
                const T As   = (gainDb_ >= T(0)) ? A_ : T(1) / A_;
                const T twoR = T(2) * R_;
                return As * As * lp + As * twoR * bp + hp;
            }
            case Mode::HighShelf:
            {
                // Canonical Simper high shelf (mirror of the low shelf):
                //   out = LP + 2R * As * BP + As^2 * HP
                const T As   = (gainDb_ >= T(0)) ? A_ : T(1) / A_;
                const T twoR = T(2) * R_;
                return lp + As * twoR * bp + As * As * hp;
            }
        }
        return lp;
    }
};
 
} // namespace dspark