Goertzel.h

// DSPark - Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi - MIT License
 
#pragma once
 
#include "../Core/DspMath.h"
 
#include <algorithm>
#include <cassert>
#include <cmath>
#include <numbers>
 
namespace dspark {
 
template <FloatType T>
class Goertzel
{
public:
    void prepare(double sampleRate, double targetFreqHz, int blockSize) noexcept
    {
        assert(sampleRate > 0.0 && "Sample rate must be positive");
        assert(blockSize > 0 && "Block size must be strictly positive");
        assert(targetFreqHz >= 0.0 && targetFreqHz <= (sampleRate * 0.5) && "Frequency must be within Nyquist limit");
 
        if (!std::isfinite(sampleRate) || sampleRate <= 0.0
            || !std::isfinite(targetFreqHz) || blockSize <= 0)
            return;
 
        sampleRate_ = sampleRate;
        targetFreq_ = std::clamp(targetFreqHz, 0.0, sampleRate * 0.5);
        blockSize_ = blockSize;
 
        // Exact target frequency omega calculation (Generalized Goertzel)
        double omega = 2.0 * std::numbers::pi * targetFreq_ / sampleRate_;
 
        coeff_ = static_cast<T>(2.0 * std::cos(omega));
        cosOmega_ = static_cast<T>(std::cos(omega));
        sinOmega_ = static_cast<T>(std::sin(omega));
 
        // Magnitude normalization: 2/N for interior bins, 1/N for DC and for
        // exact Nyquist (neither has a mirrored bin; the old 2/N at Nyquist
        // read a full-scale alternating signal 2x / +6 dB high).
        const bool isDc      = targetFreq_ <= 0.001;
        const bool isNyquist = targetFreq_ >= sampleRate_ * 0.5 * (1.0 - 1e-12);
        normalisationFactor_ = (isDc || isNyquist)
                                ? static_cast<T>(1.0 / blockSize_)
                                : static_cast<T>(2.0 / blockSize_);
 
        reset();
    }
 
    void processBlock(const T* data, int numSamples) noexcept
    {
        if (data == nullptr || numSamples <= 0) return;
 
        // Bring state to local variables for compiler optimization (register allocation)
        T s1 = s1_;
        T s2 = s2_;
        const T coeff = coeff_;
 
        for (int i = 0; i < numSamples; ++i)
        {
            T s0 = data[i] + coeff * s1 - s2;
            s2 = s1;
            s1 = s0;
 
            if (++sampleCount_ >= blockSize_)
            {
                computeResult(s1, s2);
                s1 = T(0);
                s2 = T(0);
                sampleCount_ = 0;
            }
        }
 
        // Save local state back to member variables
        s1_ = s1;
        s2_ = s2;
    }
 
    bool pushSample(T sample) noexcept
    {
        T s0 = sample + coeff_ * s1_ - s2_;
        s2_ = s1_;
        s1_ = s0;
 
        if (++sampleCount_ >= blockSize_)
        {
            computeResult(s1_, s2_);
            s1_ = T(0);
            s2_ = T(0);
            sampleCount_ = 0;
            return true;
        }
        return false;
    }
 
    void forceCompute() noexcept
    {
        computeResult(s1_, s2_);
        s1_ = T(0);
        s2_ = T(0);
        sampleCount_ = 0;
    }
 
    // -- Results ------------------------------------------------------------------
 
    [[nodiscard]] bool checkNewResultAvailable() noexcept
    {
        if (hasNewResult_)
        {
            hasNewResult_ = false;
            return true;
        }
        return false;
    }
 
    [[nodiscard]] T getMagnitude() const noexcept
    {
        return std::sqrt(real_ * real_ + imag_ * imag_);
    }
 
    [[nodiscard]] T getPower() const noexcept
    {
        return real_ * real_ + imag_ * imag_;
    }
 
    [[nodiscard]] T getMagnitudeDb() const noexcept
    {
        return gainToDecibels(getMagnitude());
    }
 
    [[nodiscard]] T getPhase() const noexcept
    {
        return std::atan2(imag_, real_);
    }
 
    [[nodiscard]] double getTargetFrequency() const noexcept { return targetFreq_; }
 
    [[nodiscard]] double getSampleRate() const noexcept { return sampleRate_; }
 
    [[nodiscard]] int getBlockSize() const noexcept { return blockSize_; }
 
    void reset() noexcept
    {
        s1_ = T(0);
        s2_ = T(0);
        sampleCount_ = 0;
        real_ = T(0);
        imag_ = T(0);
        hasNewResult_ = false;
    }
 
private:
    void computeResult(T s1, T s2) noexcept
    {
        real_ = (s1 - s2 * cosOmega_) * normalisationFactor_;
        imag_ = (s2 * sinOmega_) * normalisationFactor_;
        hasNewResult_ = true;
    }
 
    double sampleRate_ = 48000.0;
    double targetFreq_ = 440.0;
    int blockSize_ = 2048;
 
    T coeff_ = T(0);
    T cosOmega_ = T(0);
    T sinOmega_ = T(0);
    T normalisationFactor_ = T(0);
 
    // Streaming state
    T s1_ = T(0);
    T s2_ = T(0);
    int sampleCount_ = 0;
 
    // Results
    T real_ = T(0);
    T imag_ = T(0);
    bool hasNewResult_ = false;
};
 
} // namespace dspark