FIRFilter.h

Go to the documentation of this file.

// 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 <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)) noexcept
    {
        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)) noexcept
    {
        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)) noexcept
    {
        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)) noexcept
    {
        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) noexcept
    {
        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).
        stagingCoeffs_.assign(static_cast<size_t>(maxTaps_), T(0));
        activeCoeffs_.assign(static_cast<size_t>(maxTaps_), T(0));
        stagingTaps_.store(0, std::memory_order_relaxed);
        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);
        // Write REVERSED coefficients to the staging buffer for SIMD dot product.
        for (int k = 0; k < numTaps; ++k)
            stagingCoeffs_[static_cast<size_t>(k)] = coeffs[static_cast<size_t>(numTaps - 1 - k)];
        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
 
        const T* currentCoeffs = activeCoeffs_.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_.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:
    void pullCoeffsIfDirty() noexcept
    {
        if (!coeffDirty_.exchange(false, std::memory_order_acquire)) return;
        unsigned s0, s1;
        int n;
        do {
            s0 = coeffSeq_.load(std::memory_order_acquire);
            // 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)
                activeCoeffs_[static_cast<size_t>(k)] = stagingCoeffs_[static_cast<size_t>(k)];
            // 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 s0 == s1 check.
            std::atomic_thread_fence(std::memory_order_acquire);
            s1 = coeffSeq_.load(std::memory_order_relaxed);
        } while ((s0 & 1u) != 0u || s0 != s1);
        activeTaps_ = n;
    }
 
    int maxTaps_{0};
    int numChannels_{0};
    std::atomic<bool> isPrepared_{false};
 
    // SPSC seqlock coefficient publication (writer: setCoefficients).
    std::vector<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 set (never overwritten mid-block).
    std::vector<T> activeCoeffs_;
    int activeTaps_{0};
 
    // Flattened, cache-contiguous delay lines
    std::vector<T> delayLineBuffer_;
    std::vector<int> writePositions_;
};
 
} // namespace dspark