FIRFilter.h

// DSPark -- Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi -- MIT License
 
#pragma once
 
#include "AudioBuffer.h"
#include "SimdOps.h"
#include "WindowFunctions.h"
 
#include <algorithm>
#include <array>
#include <atomic>
#include <cassert>
#include <cmath>
#include <numbers>
#include <span>
#include <vector>
 
namespace dspark {
 
// ============================================================================
// FIRDesign -- Coefficient design via windowed-sinc method
// ============================================================================
 
template <typename T>
class FIRDesign
{
public:
    [[nodiscard]] static std::vector<T> lowPass(double sampleRate, double cutoffHz,
                                                 int numTaps, T beta = T(5))
    {
        assert(numTaps >= 3 && (numTaps % 2 == 1));
        numTaps = sanitizeTaps(numTaps);
        return designSinc(cutoffHz / sampleRate, numTaps, beta, false);
    }
 
    [[nodiscard]] static std::vector<T> highPass(double sampleRate, double cutoffHz,
                                                  int numTaps, T beta = T(5))
    {
        assert(numTaps >= 3 && (numTaps % 2 == 1));
        numTaps = sanitizeTaps(numTaps);
        return designSinc(cutoffHz / sampleRate, numTaps, beta, true);
    }
 
    [[nodiscard]] static std::vector<T> bandPass(double sampleRate, double lowCutoffHz,
                                                  double highCutoffHz, int numTaps,
                                                  T beta = T(5))
    {
        assert(numTaps >= 3 && (numTaps % 2 == 1));
        assert(lowCutoffHz < highCutoffHz);
        numTaps = sanitizeTaps(numTaps);
 
        auto lp1 = designSinc(highCutoffHz / sampleRate, numTaps, beta, false);
        auto lp2 = designSinc(lowCutoffHz / sampleRate, numTaps, beta, false);
 
        for (int i = 0; i < numTaps; ++i)
            lp1[static_cast<size_t>(i)] -= lp2[static_cast<size_t>(i)];
 
        return lp1;
    }
 
    [[nodiscard]] static std::vector<T> bandStop(double sampleRate, double lowCutoffHz,
                                                  double highCutoffHz, int numTaps,
                                                  T beta = T(5))
    {
        assert(numTaps >= 3 && (numTaps % 2 == 1));
        // Sanitise HERE too: bandPass() sanitises its own copy, so an even
        // request would return numTaps+1 coefficients while the loops below
        // still ran over the caller's numTaps -- leaving the last tap
        // un-negated (a broken, asymmetric filter) in release builds.
        numTaps = sanitizeTaps(numTaps);
 
        auto bp = bandPass(sampleRate, lowCutoffHz, highCutoffHz, numTaps, beta);
 
        // Spectral inversion: negate all and add 1 to centre tap
        int centre = numTaps / 2;
        for (int i = 0; i < numTaps; ++i)
            bp[static_cast<size_t>(i)] = -bp[static_cast<size_t>(i)];
        bp[static_cast<size_t>(centre)] += T(1);
 
        return bp;
    }
 
    [[nodiscard]] static int estimateTaps(double sampleRate, double transitionHz,
                                           double attenuationDb) noexcept
    {
        assert(sampleRate > 0.0 && transitionHz > 0.0);
        // Release-safe floor: a zero/negative transition width would push the
        // estimate through ceil(inf) into undefined integer conversion.
        double normTransition = std::max(transitionHz / sampleRate, 1e-6);
        int n = static_cast<int>(std::ceil((attenuationDb - 7.95) / (14.36 * normTransition)));
        if (n < 3) n = 3;
        if (n % 2 == 0) ++n; // Ensure odd
        return n;
    }
 
    [[nodiscard]] static T estimateKaiserBeta(double attenuationDb) noexcept
    {
        if (attenuationDb > 50.0)
            return static_cast<T>(0.1102 * (attenuationDb - 8.7));
        else if (attenuationDb >= 21.0)
            return static_cast<T>(0.5842 * std::pow(attenuationDb - 21.0, 0.4)
                                + 0.07886 * (attenuationDb - 21.0));
        else
            return T(0);
    }
 
private:
    [[nodiscard]] static int sanitizeTaps(int numTaps) noexcept
    {
        if (numTaps < 3) numTaps = 3;
        return numTaps | 1;
    }
 
    [[nodiscard]] static std::vector<T> designSinc(double normFreq, int numTaps,
                                                    T beta, bool invert)
    {
        std::vector<double> design(static_cast<size_t>(numTaps));
        std::vector<double> window(static_cast<size_t>(numTaps));
 
        // Generate Kaiser window (double engine regardless of T)
        WindowFunctions<double>::kaiser(window.data(), numTaps,
                                        static_cast<double>(beta), false);
 
        const int centre = numTaps / 2;
        const double fc = normFreq * 2.0; // Normalised to [0, 1] for sinc
        constexpr double kPi = std::numbers::pi;
 
        // Compute windowed sinc and its DC sum in one pass
        double sum = 0.0;
        for (int i = 0; i < numTaps; ++i)
        {
            const int n = i - centre;
            const double x = static_cast<double>(n) * kPi;
            const double s = (n == 0) ? fc : std::sin(fc * x) / x;
            const double c = s * window[static_cast<size_t>(i)];
            design[static_cast<size_t>(i)] = c;
            sum += c;
        }
 
        // Normalise for unity gain at DC (low-pass) or Nyquist (high-pass)
        if (std::abs(sum) > 1e-10)
        {
            const double invSum = 1.0 / sum;
            for (auto& c : design) c *= invSum;
        }
 
        // Spectral inversion for high-pass
        if (invert)
        {
            for (auto& c : design) c = -c;
            design[static_cast<size_t>(centre)] += 1.0;
        }
 
        std::vector<T> coeffs(static_cast<size_t>(numTaps));
        for (int i = 0; i < numTaps; ++i)
            coeffs[static_cast<size_t>(i)] = static_cast<T>(design[static_cast<size_t>(i)]);
        return coeffs;
    }
};
 
// ============================================================================
// FIRFilter -- FIR filter processor (SIMD direct convolution, thread-safe)
// ============================================================================
 
template <typename T>
class FIRFilter
{
public:
    FIRFilter() = default;
    ~FIRFilter() = default;
 
    void prepare(int maxTaps, int numChannels)
    {
        assert(maxTaps > 0 && numChannels > 0);
 
        maxTaps_ = maxTaps;
        numChannels_ = numChannels;
 
        // SPSC coefficient publication: a shared staging buffer (written by the
        // control thread under a seqlock) and an audio-thread-private active
        // buffer (copied once per update, never overwritten mid-block). The
        // SHARED staging words are std::atomic<T> so each cross-thread word
        // access is defined (no data-race UB); the seqlock counter still guards
        // against reading a half-published set. Allocated here, at setup time.
        stagingCoeffs_ = std::vector<std::atomic<T>>(static_cast<size_t>(maxTaps_));
        for (auto& s : stagingCoeffs_) s.store(T(0), std::memory_order_relaxed);
        activeCoeffs_[0].assign(static_cast<size_t>(maxTaps_), T(0));
        activeCoeffs_[1].assign(static_cast<size_t>(maxTaps_), T(0));
        stagingTaps_.store(0, std::memory_order_relaxed);
        activeIdx_    = 0;
        activeTaps_   = 0;
        coeffSeq_.store(0, std::memory_order_release);
        coeffDirty_.store(false, std::memory_order_release);
        reportedTaps_.store(0, std::memory_order_release);
 
        // Flattened delay line buffer using the "mirror" technique.
        // Size: numChannels * (maxTaps * 2)
        delayLineBuffer_.assign(static_cast<size_t>(numChannels_ * maxTaps_ * 2), T(0));
        writePositions_.assign(static_cast<size_t>(numChannels_), 0);
 
        isPrepared_.store(true, std::memory_order_release);
    }
 
    void setCoefficients(std::span<const T> coeffs) noexcept
    {
        if (!isPrepared_.load(std::memory_order_acquire)) return;
 
        const int numTaps = static_cast<int>(coeffs.size());
        if (numTaps == 0 || numTaps > maxTaps_) return;
 
        // Seqlock publish: odd sequence = write in progress.
        coeffSeq_.fetch_add(1, std::memory_order_acq_rel);
        // Release fence: pairs with the reader's acquire fence via fence-fence
        // synchronization ([atomics.fences]/2). If the audio thread's copy loop
        // reads ANY of the relaxed word stores below, that read forces its
        // post-fence re-read of coeffSeq_ to observe the odd count above, so the
        // retry loop rejects the torn copy. Without this fence the relaxed data
        // words are NOT in coeffSeq_'s release sequence and the standard permits
        // a torn copy to pass validation on weakly ordered targets (Boehm,
        // "Can Seqlocks Get Along With Programming Language Memory Models?",
        // MSPC 2012). Control-thread only: zero audio-path cost.
        std::atomic_thread_fence(std::memory_order_release);
        // Write REVERSED coefficients to the staging buffer for SIMD dot product.
        // Relaxed stores: the atomic words make each access race-free; the fence
        // above plus the seq counter order and publish the set.
        for (int k = 0; k < numTaps; ++k)
            stagingCoeffs_[static_cast<size_t>(k)].store(
                coeffs[static_cast<size_t>(numTaps - 1 - k)], std::memory_order_relaxed);
        stagingTaps_.store(numTaps, std::memory_order_relaxed);
        coeffSeq_.fetch_add(1, std::memory_order_release); // even = consistent
        coeffDirty_.store(true, std::memory_order_release);
        reportedTaps_.store(numTaps, std::memory_order_release);
    }
 
    void reset() noexcept
    {
        std::fill(delayLineBuffer_.begin(), delayLineBuffer_.end(), T(0));
        std::fill(writePositions_.begin(), writePositions_.end(), 0);
    }
 
    void processBlock(AudioBufferView<T> buffer) noexcept
    {
        if (!isPrepared_.load(std::memory_order_acquire)) return;
 
        // Pick up any pending coefficient update once per block (seqlock copy
        // into the audio-thread-private active buffer).
        pullCoeffsIfDirty();
 
        const int currentTaps = activeTaps_;
        if (currentTaps == 0) return; // Bypass if uninitialized
 
        // Hoisted exactly as the single-buffer .data() was: one private index
        // load outside the loops, nothing added inside them.
        const T* currentCoeffs = activeCoeffs_[static_cast<size_t>(activeIdx_)].data();
        const int numChannels = std::min(buffer.getNumChannels(), numChannels_);
        const int numSamples  = buffer.getNumSamples();
 
        for (int ch = 0; ch < numChannels; ++ch)
        {
            T* data = buffer.getChannel(ch);
            auto& wp = writePositions_[static_cast<size_t>(ch)];
            const int channelOffset = ch * (maxTaps_ * 2);
            T* dl = delayLineBuffer_.data() + channelOffset;
 
            for (int i = 0; i < numSamples; ++i)
            {
                // Write into mirror buffer based on fixed maxTaps_ bound
                dl[wp] = data[i];
                dl[wp + maxTaps_] = data[i];
 
                // Calculate oldest sample position for the contiguous read
                const T* readPtr = dl + wp + maxTaps_ - currentTaps + 1;
 
                // Execute SIMD convolution (unaligned load assumed for readPtr)
                data[i] = simd::dotProduct(currentCoeffs, readPtr, currentTaps);
 
                // Advance write position and wrap fixed bound
                if (++wp >= maxTaps_) wp = 0;
            }
        }
    }
 
    [[nodiscard]] T processSample(T input, int channel) noexcept
    {
        if (!isPrepared_.load(std::memory_order_acquire)) return input;
 
        // Release-safe channel bound (processBlock clamps; this entry point
        // must too, or an out-of-range channel indexes the flat delay line OOB).
        assert(channel >= 0 && channel < numChannels_);
        if (channel < 0 || channel >= numChannels_) return input;
 
        // Cheap relaxed check; the seqlock copy only runs when an update landed.
        if (coeffDirty_.load(std::memory_order_relaxed)) [[unlikely]]
            pullCoeffsIfDirty();
 
        const int currentTaps = activeTaps_;
        if (currentTaps == 0) return input;
 
        const T* currentCoeffs = activeCoeffs_[static_cast<size_t>(activeIdx_)].data();
 
        auto& wp = writePositions_[static_cast<size_t>(channel)];
        const int channelOffset = channel * (maxTaps_ * 2);
        T* dl = delayLineBuffer_.data() + channelOffset;
 
        dl[wp] = input;
        dl[wp + maxTaps_] = input;
 
        const T* readPtr = dl + wp + maxTaps_ - currentTaps + 1;
        const T output = simd::dotProduct(currentCoeffs, readPtr, currentTaps);
 
        if (++wp >= maxTaps_) wp = 0;
 
        return output;
    }
 
    [[nodiscard]] int getLatency() const noexcept
    {
        const int taps = reportedTaps_.load(std::memory_order_relaxed);
        return taps > 0 ? (taps - 1) / 2 : 0;
    }
 
private:
    static constexpr int kSeqlockMaxAttempts = 3;
 
    void pullCoeffsIfDirty() noexcept
    {
        if (!coeffDirty_.exchange(false, std::memory_order_acquire)) return;
 
        // Commit-on-validate: the copy lands in the SPARE active buffer, never
        // in the one the per-sample loop is reading. A give-up therefore leaves
        // the live set untouched instead of leaving a torn mixture behind it.
        const size_t spare = static_cast<size_t>(activeIdx_ ^ 1);
        T* const dest = activeCoeffs_[spare].data();
 
        bool adopted = false;
        int n = 0;
        for (int attempt = 0; attempt < kSeqlockMaxAttempts; ++attempt)
        {
            const unsigned s0 = coeffSeq_.load(std::memory_order_acquire);
            // Odd counter: the writer is mid-publish. Skip the copy entirely --
            // testing this BEFORE copying is what makes a collision cost one
            // relaxed load instead of up to maxTaps loads that would then be
            // thrown away, and is why the bound is affordable per sample.
            if ((s0 & 1u) != 0u) continue;
            // Defensive clamp: the loop bound must never exceed the buffers
            // even if this speculative read races a concurrent publish.
            n = std::min(stagingTaps_.load(std::memory_order_relaxed), maxTaps_);
            for (int k = 0; k < n; ++k)
                dest[k] = stagingCoeffs_[static_cast<size_t>(k)].load(std::memory_order_relaxed);
            // The fence orders the copy above BEFORE the re-read below. A plain
            // load-acquire is not enough: acquire only orders LATER accesses,
            // so on weakly-ordered CPUs (ARM) the copy could sink below the
            // second read and a torn copy would pass the equality check.
            std::atomic_thread_fence(std::memory_order_acquire);
            if (s0 == coeffSeq_.load(std::memory_order_relaxed))
            {
                adopted = true;
                break;
            }
        }
 
        if (!adopted)
        {
            // Adopt nothing: neither the index nor activeTaps_ moves, so the
            // set already in use stays live. Re-arm and pick the update up on a
            // later call.
            coeffDirty_.store(true, std::memory_order_release);
            return;
        }
 
        activeIdx_  ^= 1;   // flip: the validated copy becomes the live set
        activeTaps_  = n;
    }
 
    int maxTaps_{0};
    int numChannels_{0};
    std::atomic<bool> isPrepared_{false};
 
    // SPSC seqlock coefficient publication (writer: setCoefficients).
    // Atomic words: the staging buffer is read by the audio thread while the
    // control thread writes it, so each word is std::atomic<T> (relaxed) to keep
    // every concurrent access defined; the seq counter (acq_rel/release) orders
    // and tear-protects the set. std::atomic<float/double> is lock-free and
    // one machine word, so this is a plain load/store on every supported target.
    std::vector<std::atomic<T>> stagingCoeffs_;  
    std::atomic<int> stagingTaps_{0};       
    std::atomic<unsigned> coeffSeq_{0};     
    std::atomic<bool> coeffDirty_{false};   
    std::atomic<int> reportedTaps_{0};      
 
    // Audio-thread-private active coefficient sets (never overwritten
    // mid-block). TWO buffers plus a private index, not one: the bounded read
    // must be able to give up without having damaged the set in use, so it
    // copies into activeCoeffs_[activeIdx_ ^ 1] and only flips the index once
    // the copy validates. Both are audio-thread-private, so the index needs no
    // synchronization; the cost is one extra maxTaps buffer at prepare time.
    std::array<std::vector<T>, 2> activeCoeffs_;
    int activeIdx_{0};
    int activeTaps_{0};
 
    // Flattened, cache-contiguous delay lines
    std::vector<T> delayLineBuffer_;
    std::vector<int> writePositions_;
};
 
} // namespace dspark