LoudnessMeter.h

// DSPark - Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi - MIT License
 
#pragma once
 
#include "../Core/DspMath.h"
#include "../Core/AudioSpec.h"
#include "../Core/AudioBuffer.h"
#include "../Core/Biquad.h"
#include "../Core/TruePeakDetector.h"
 
#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstdint>
#include <limits>
#include <numbers>
#include <array>
 
namespace dspark {
 
template <FloatType T>
class LoudnessMeter
{
public:
    struct MeasurementInfo
    {
        std::int64_t committedFrames = 0;
        int pendingFrames = 0;
        int hopFrames = 0;
        std::uint64_t integratedBlocks = 0;
        std::uint64_t rangeBlocks = 0;
    };
 
    [[nodiscard]] MeasurementInfo getMeasurementInfo() const noexcept
    {
        MeasurementInfo info;
        if (!prepared_.load(std::memory_order_relaxed)) return info;
        info.committedFrames = totalCommittedBlocks_ * blockSamples_;
        info.pendingFrames = currentBlockSamples_;
        info.hopFrames = blockSamples_;
        for (int i = 0; i < kNumBins; ++i)
        {
            info.integratedBlocks += histogram_[i].load(std::memory_order_relaxed);
            info.rangeBlocks += lraHistogram_[i].load(std::memory_order_relaxed);
        }
        return info;
    }
 
    void prepare(double sampleRate, int numChannels = 2) noexcept
    {
        (void)numChannels;
        if (!std::isfinite(sampleRate) || sampleRate <= 0.0) return;
 
        sampleRate_ = sampleRate;
 
        computeKWeighting(sampleRate);
 
        // 100 ms block length (clamped in double before the cast: an absurd
        // finite rate must not overflow the int conversion)
        blockSamples_ = static_cast<int>(std::clamp(sampleRate * 0.1, 1.0, 1.0e9));
 
        reset();
        prepared_.store(true, std::memory_order_relaxed);
    }
 
    void prepare(const AudioSpec& spec) noexcept
    {
        prepare(spec.sampleRate, spec.numChannels);
    }
 
    void processBlock(AudioBufferView<const T> buffer) noexcept
    {
        const int nCh = buffer.getNumChannels();
        const int nS = buffer.getNumSamples();
        if (nCh >= 2)
            process(buffer.getChannel(0), buffer.getChannel(1), nS);
        else if (nCh == 1)
            process(buffer.getChannel(0), nS);
    }
 
    void process(const T* data, int numSamples) noexcept
    {
        if (!prepared_.load(std::memory_order_relaxed) || data == nullptr || numSamples <= 0)
            return;
 
        T tpMax = truePeakMax_.load(std::memory_order_relaxed);
        for (int i = 0; i < numSamples; ++i)
        {
            // Only fold FINITE estimates into the max-hold: a single Inf/NaN
            // sample would otherwise pin the peak at +Inf forever (std::max
            // with +Inf latches, and the raw |NaN| leaks through the ring),
            // leaving getTruePeakDb() stuck at +Inf dBTP until reset() -- the
            // K-filter / histogram path already self-recovers, so the true
            // peak must too (same non-finite robustness contract).
            const T tp0 = truePeak_.processSample(data[i], 0);
            if (std::isfinite(tp0))
                tpMax = std::max(tpMax, tp0);
            else
                invalidateMeasurement();
 
            double filtered = applyKWeighting(static_cast<double>(data[i]), 0);
            currentBlockPower_ += filtered * filtered;
            if (!std::isfinite(currentBlockPower_))
                invalidateMeasurement();
 
            if (++currentBlockSamples_ >= blockSamples_)
                commitBlock();
        }
        truePeakMax_.store(tpMax, std::memory_order_relaxed);
    }
 
    void process(const T* left, const T* right, int numSamples) noexcept
    {
        if (!prepared_.load(std::memory_order_relaxed)
            || left == nullptr || right == nullptr || numSamples <= 0)
            return;
 
        T tpMax = truePeakMax_.load(std::memory_order_relaxed);
        for (int i = 0; i < numSamples; ++i)
        {
            // Fold only FINITE true-peak estimates (see the mono path): a lone
            // Inf/NaN sample must not latch the max-hold at +Inf forever.
            const T tpL = truePeak_.processSample(left[i], 0);
            const T tpR = truePeak_.processSample(right[i], 1);
            if (std::isfinite(tpL))
                tpMax = std::max(tpMax, tpL);
            else
                invalidateMeasurement();
            if (std::isfinite(tpR))
                tpMax = std::max(tpMax, tpR);
            else
                invalidateMeasurement();
 
            double filtL = applyKWeighting(static_cast<double>(left[i]), 0);
            double filtR = applyKWeighting(static_cast<double>(right[i]), 1);
 
            currentBlockPower_ += (filtL * filtL + filtR * filtR);
            if (!std::isfinite(currentBlockPower_))
                invalidateMeasurement();
 
            if (++currentBlockSamples_ >= blockSamples_)
                commitBlock();
        }
        truePeakMax_.store(tpMax, std::memory_order_relaxed);
    }
 
    [[nodiscard]] T getMomentaryLUFS() const noexcept
    {
        return calculateLUFSFromBlocks(4);
    }
 
    [[nodiscard]] T getShortTermLUFS() const noexcept
    {
        return calculateLUFSFromBlocks(30);
    }
 
    [[nodiscard]] T getIntegratedLUFS() const noexcept
    {
        // Pass 1: Absolute Gate (-70 LUFS)
        double sumPowerUngated = 0.0;
        uint64_t countUngated = 0;
 
        for (int i = 0; i < kNumBins; ++i)
        {
            uint32_t binCount = histogram_[i].load(std::memory_order_relaxed);
            if (binCount > 0)
            {
                double binPower = lufsToPower(kMinHistogramLUFS + static_cast<double>(i) * kBinWidth);
                sumPowerUngated += binPower * binCount;
                countUngated += binCount;
            }
        }
 
        if (countUngated == 0) return T(-100);
 
        double meanPowerUngated = sumPowerUngated / countUngated;
        double ungatedLUFS = powerToLUFS(meanPowerUngated);
 
        // Pass 2: Relative Gate (-10 LU below ungated mean)
        double relativeGateLUFS = ungatedLUFS - 10.0;
        int relativeGateBin = static_cast<int>(std::floor((relativeGateLUFS - kMinHistogramLUFS) / kBinWidth));
        relativeGateBin = std::clamp(relativeGateBin, 0, kNumBins - 1);
 
        double sumPowerGated = 0.0;
        uint64_t countGated = 0;
 
        for (int i = relativeGateBin; i < kNumBins; ++i)
        {
            uint32_t binCount = histogram_[i].load(std::memory_order_relaxed);
            if (binCount > 0)
            {
                double binPower = lufsToPower(kMinHistogramLUFS + static_cast<double>(i) * kBinWidth);
                sumPowerGated += binPower * binCount;
                countGated += binCount;
            }
        }
 
        if (countGated == 0) return T(-100);
 
        return static_cast<T>(powerToLUFS(sumPowerGated / countGated));
    }
 
    [[nodiscard]] bool isMeasurementValid() const noexcept
    {
        return measurementValid_.load(std::memory_order_relaxed);
    }
 
    [[nodiscard]] T getTruePeakDb() const noexcept
    {
        return gainToDecibels(truePeakMax_.load(std::memory_order_relaxed));
    }
 
    void finalizeTruePeak() noexcept
    {
        if (!prepared_.load(std::memory_order_relaxed)) return;
        T peak = truePeakMax_.load(std::memory_order_relaxed);
        for (int channel = 0; channel < kMaxChannels; ++channel)
        {
            const T tail = truePeak_.getTailPeak(channel);
            if (std::isfinite(tail))
                peak = std::max(peak, tail);
            else
                invalidateMeasurement();
        }
        truePeakMax_.store(peak, std::memory_order_relaxed);
    }
 
    [[nodiscard]] T getLoudnessRange() const noexcept
    {
        // Pass 1: relative gate from the mean of absolute-gated ST values.
        double sumPower = 0.0;
        uint64_t count = 0;
        for (int i = 0; i < kNumBins; ++i)
        {
            const uint32_t c = lraHistogram_[i].load(std::memory_order_relaxed);
            if (c > 0)
            {
                sumPower += lufsToPower(kMinHistogramLUFS + i * kBinWidth) * c;
                count += c;
            }
        }
        if (count == 0) return T(0);
 
        const double relGateLUFS = powerToLUFS(sumPower / count) - 20.0;
        int gateBin = static_cast<int>(std::floor((relGateLUFS - kMinHistogramLUFS) / kBinWidth));
        gateBin = std::clamp(gateBin, 0, kNumBins - 1);
 
        // Pass 2: 10th / 95th percentiles above the relative gate.
        uint64_t gatedCount = 0;
        for (int i = gateBin; i < kNumBins; ++i)
            gatedCount += lraHistogram_[i].load(std::memory_order_relaxed);
        if (gatedCount == 0) return T(0);
 
        // Percentiles by the Tech 3342 reference rule: the value of 0-based rank
        // round((n - 1) * p) among the n gated values. Both ranks are always
        // measured values - never the gate threshold - and every n is
        // covered: with one value, or with n equal values, the range is 0.
        const auto rank10 = static_cast<uint64_t>(std::llround(0.10 * static_cast<double>(gatedCount - 1)));
        const auto rank95 = static_cast<uint64_t>(std::llround(0.95 * static_cast<double>(gatedCount - 1)));
 
        double p10 = 0.0, p95 = 0.0;
        uint64_t running = 0;
        bool have10 = false;
        for (int i = gateBin; i < kNumBins; ++i)
        {
            const uint32_t c = lraHistogram_[i].load(std::memory_order_relaxed);
            if (c == 0) continue;
            running += c;
            // `running` values sit at or below this bin: ranks 0 .. running-1.
            if (!have10 && running > rank10)
            {
                p10 = kMinHistogramLUFS + i * kBinWidth;
                have10 = true;
            }
            if (running > rank95)
            {
                p95 = kMinHistogramLUFS + i * kBinWidth;
                break;
            }
        }
        return static_cast<T>(std::max(0.0, p95 - p10));
    }
 
    void reset() noexcept
    {
        for (int ch = 0; ch < kMaxChannels; ++ch)
        {
            preState_[ch] = {};
            rlbState_[ch] = {};
        }
 
        for (auto& power : blockPowers_)
            power.store(0.0, std::memory_order_relaxed);
 
        for (auto& bin : histogram_)
            bin.store(0, std::memory_order_relaxed);
        for (auto& bin : lraHistogram_)
            bin.store(0, std::memory_order_relaxed);
 
        truePeak_.reset();
        truePeakMax_.store(T(0), std::memory_order_relaxed);
 
        blockWritePos_.store(0, std::memory_order_relaxed);
        totalCommittedBlocks_ = 0;
        currentBlockPower_ = 0.0;
        currentBlockSamples_ = 0;
        measurementValid_.store(true, std::memory_order_relaxed);
    }
 
private:
    static constexpr int kMaxChannels = 2; // Expandable to 8 with proper spatial weighting
 
    // Histogram specs: -70 LUFS to +30 LUFS with 0.1 resolution = 1000 bins
    static constexpr double kMinHistogramLUFS = -70.0;
    static constexpr double kMaxHistogramLUFS = 30.0;
    static constexpr double kBinWidth = 0.1;
    static constexpr int kNumBins = 1000;
 
    // Filters state (Forced to double to prevent DF2T precision issues)
    struct BiquadState { double z1 = 0.0, z2 = 0.0; };
    struct BiquadCoeff { double b0 = 1.0, b1 = 0.0, b2 = 0.0, a1 = 0.0, a2 = 0.0; };
 
    void computeKWeighting(double sr)
    {
        // Pre-warped analog parameterization that reproduces the official
        // ITU-R BS.1770-5 table 1/2 coefficients at 48 kHz to machine
        // precision (verified: max error 8.9e-16). This is an exact 48 kHz
        // parameterization, not a claim of an identical response at every
        // rate: the retained 997 Hz cascade gains are +0.691014 dB at 48 kHz,
        // +0.693776 dB at 44.1 kHz and +0.663943 dB at 192 kHz. Residual
        // full-band deviation stays below 0.05 dB through 384 kHz and inside
        // the standard tolerance. The -0.691 constant in powerToLUFS remains
        // tied to this cascade; an RBJ shelf or a gain-normalized RLB high-pass
        // reads ~0.26 LU low on the EBU conformance vectors. The design lives
        // in BiquadCoeffs so every K-weighted measurement shares it.
        const auto shelf = BiquadCoeffs::makeKWeightingShelf(sr);
        pre_ = { shelf.b0, shelf.b1, shelf.b2, shelf.a1, shelf.a2 };
        const auto rlb = BiquadCoeffs::makeKWeightingHighPass(sr);
        rlb_ = { rlb.b0, rlb.b1, rlb.b2, rlb.a1, rlb.a2 };
    }
 
    double applyBiquad(double input, const BiquadCoeff& c, BiquadState& s) noexcept
    {
        // Adding 1e-18 protects against denormal numbers (Flush-To-Zero fallback)
        double output = c.b0 * input + s.z1;
        s.z1 = c.b1 * input - c.a1 * output + s.z2;
        s.z2 = c.b2 * input - c.a2 * output + 1e-18;
        return output;
    }
 
    double applyKWeighting(double input, int channel) noexcept
    {
        double x = applyBiquad(input, pre_, preState_[channel]);
        return applyBiquad(x, rlb_, rlbState_[channel]);
    }
 
    void commitBlock() noexcept
    {
        const double meanPower = currentBlockPower_ / currentBlockSamples_;
        currentBlockPower_ = 0.0;
        currentBlockSamples_ = 0;
 
        // A non-finite block (NaN/Inf fed by the caller) would poison the
        // sliding window and freeze the histograms forever: the K-filter
        // recursion never drains a NaN. A meter must report the signal as it
        // is NOW, so drop the block, clear the filter states and resume
        // measuring clean on the next one.
        if (!std::isfinite(meanPower))
        {
            invalidateMeasurement();
            for (int ch = 0; ch < kMaxChannels; ++ch)
            {
                preState_[ch] = {};
                rlbState_[ch] = {};
            }
            return;
        }
 
        // Update sliding window ring buffer
        int currentPos = blockWritePos_.load(std::memory_order_relaxed);
        blockPowers_[currentPos].store(meanPower, std::memory_order_relaxed);
 
        int nextPos = (currentPos + 1) % 30; // 30 blocks = 3s max window
        blockWritePos_.store(nextPos, std::memory_order_release);
        ++totalCommittedBlocks_;
 
        // BS.1770-4 integrated gating uses 400 ms blocks with 75% overlap.
        // With a 100 ms sub-block hop, that is EXACTLY the mean of the last
        // four sub-blocks committed at every 100 ms step. (Gating raw 100 ms
        // blocks has higher variance and fails the EBU conformance vectors.)
        if (totalCommittedBlocks_ >= 4)
        {
            double gating400 = meanPower;
            for (int back = 1; back < 4; ++back)
            {
                int idx = (currentPos - back + 30) % 30;
                gating400 += blockPowers_[idx].load(std::memory_order_relaxed);
            }
            gating400 *= 0.25;
 
            const double lufs = powerToLUFS(gating400);
            if (!std::isfinite(gating400) || !std::isfinite(lufs))
            {
                invalidateMeasurement();
            }
            else if (lufs >= kMinHistogramLUFS)
            {
                int binIndex = static_cast<int>(std::round((lufs - kMinHistogramLUFS) / kBinWidth));
                if (binIndex >= kNumBins)
                    invalidateMeasurement();
                binIndex = std::clamp(binIndex, 0, kNumBins - 1);
                incrementHistogram(histogram_[binIndex]);
            }
        }
 
        // EBU Tech 3342 loudness range: short-term (3 s) values sampled at
        // 10 Hz - every 100 ms sub-block, the 2.9 s minimum window overlap
        // Tech 3342 has required since V3 (2016) - absolute-gated at -70 LUFS
        // into a histogram.
        if (totalCommittedBlocks_ >= 30)
        {
            const double stLufs = static_cast<double>(calculateLUFSFromBlocks(30));
            if (!std::isfinite(stLufs))
            {
                invalidateMeasurement();
            }
            else if (stLufs >= kMinHistogramLUFS)
            {
                int binIndex = static_cast<int>(std::round((stLufs - kMinHistogramLUFS) / kBinWidth));
                if (binIndex >= kNumBins)
                    invalidateMeasurement();
                binIndex = std::clamp(binIndex, 0, kNumBins - 1);
                incrementHistogram(lraHistogram_[binIndex]);
            }
        }
    }
 
    void invalidateMeasurement() noexcept
    {
        measurementValid_.store(false, std::memory_order_relaxed);
    }
 
    void incrementHistogram(std::atomic<uint32_t>& bin) noexcept
    {
        if (bin.load(std::memory_order_relaxed)
            == std::numeric_limits<uint32_t>::max())
        {
            invalidateMeasurement();
            return;
        }
        bin.fetch_add(1, std::memory_order_relaxed);
    }
 
    T calculateLUFSFromBlocks(int numBlocks) const noexcept
    {
        double sum = 0.0;
        int currentPos = blockWritePos_.load(std::memory_order_acquire);
 
        for (int i = 0; i < numBlocks; ++i)
        {
            int idx = (currentPos - 1 - i + 30) % 30;
            sum += blockPowers_[idx].load(std::memory_order_relaxed);
        }
 
        return static_cast<T>(powerToLUFS(sum / numBlocks));
    }
 
    [[nodiscard]] static double powerToLUFS(double meanPower) noexcept
    {
        if (meanPower <= 1e-10) return -100.0; // Prevent log10(0)
        return std::max(-100.0, -0.691 + 10.0 * std::log10(meanPower));
    }
 
    [[nodiscard]] static double lufsToPower(double lufs) noexcept
    {
        return std::pow(10.0, (lufs + 0.691) / 10.0);
    }
 
    double sampleRate_ = 48000.0;
    std::atomic<bool> prepared_{ false };
 
    BiquadCoeff pre_, rlb_;
    BiquadState preState_[kMaxChannels], rlbState_[kMaxChannels];
 
    int blockSamples_ = 4800;
    double currentBlockPower_ = 0.0;
    int currentBlockSamples_ = 0;
 
    // Concurrency-safe components
    std::array<std::atomic<double>, 30> blockPowers_;
    std::atomic<int> blockWritePos_{0};
    int64_t totalCommittedBlocks_ = 0;
 
    // O(1) Histogram for Integrated Loudness
    std::array<std::atomic<uint32_t>, kNumBins> histogram_;
 
    // EBU Tech 3342 loudness-range histogram (short-term values, 100 ms hop)
    std::array<std::atomic<uint32_t>, kNumBins> lraHistogram_;
 
    // Sticky for one reset-to-reset measurement transaction.
    std::atomic<bool> measurementValid_ { true };
 
    // ITU-R BS.1770-4 true-peak (dBTP) tracking
    TruePeakDetector<T, kMaxChannels> truePeak_;
    std::atomic<T> truePeakMax_ { T(0) };
};
 
} // namespace dspark