Interpolation.h

// DSPark - Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi - MIT License
 
#pragma once
 
#include "DspMath.h"
#include "SimdOps.h"
 
#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <vector>
 
namespace dspark {
 
template <FloatType T>
[[nodiscard]] inline T interpolateLinear(T y0, T y1, T frac) noexcept {
    return y0 + frac * (y1 - y0);
}
 
template <FloatType T>
[[nodiscard]] inline T interpolateLinear(const T* buffer, int length, T position) noexcept {
    assert(length > 0 && position >= T(0) && position < static_cast<T>(length));
    if (length <= 0) return T(0);
 
    // An out-of-range position (including NaN) would make the int cast below
    // undefined and the buffer reads out of bounds: clamp before casting.
    int idx0;
    T frac;
    if (!(position >= T(0))) { idx0 = 0; frac = T(0); }
    else if (position >= static_cast<T>(length)) { idx0 = length - 1; frac = T(0); }
    else {
        idx0 = static_cast<int>(position);
        frac = position - static_cast<T>(idx0);
    }
 
    int idx1 = idx0 + 1;
    if (idx1 >= length) idx1 = 0;
 
    return interpolateLinear(buffer[idx0], buffer[idx1], frac);
}
 
template <FloatType T>
[[nodiscard]] inline T interpolateHermite(T y0, T y1, T y2, T y3, T frac) noexcept {
    T c = (y2 - y0) * T(0.5);
    T v = y1 - y2;
    T w = c + v;
    T a = w + v + (y3 - y1) * T(0.5);
    T b = w + a;
    return (((a * frac) - b) * frac + c) * frac + y1;
}
 
template <FloatType T>
[[nodiscard]] inline T interpolateHermite(const T* buffer, int length, T position) noexcept {
    assert(length > 0 && position >= T(0) && position < static_cast<T>(length));
    if (length <= 0) return T(0);
 
    // Same release-safe clamp as the linear overload (undefined int cast /
    // out-of-bounds reads otherwise).
    int idx1;
    T frac;
    if (!(position >= T(0))) { idx1 = 0; frac = T(0); }
    else if (position >= static_cast<T>(length)) { idx1 = length - 1; frac = T(0); }
    else {
        idx1 = static_cast<int>(position);
        frac = position - static_cast<T>(idx1);
    }
 
    int idx0 = (idx1 > 0) ? idx1 - 1 : length - 1;
    int idx2 = idx1 + 1; if (idx2 >= length) idx2 -= length;
    int idx3 = idx2 + 1; if (idx3 >= length) idx3 -= length;
 
    return interpolateHermite(buffer[idx0], buffer[idx1], buffer[idx2], buffer[idx3], frac);
}
 
template <FloatType T>
[[nodiscard]] inline T interpolateCubic(const T* buffer, int length, T position) noexcept {
    return interpolateHermite(buffer, length, position);
}
 
template <FloatType T>
[[nodiscard]] inline T interpolateLagrange(T y0, T y1, T y2, T y3, T frac) noexcept {
    T d = frac;
    T dm1 = d - T(1);
    T dm2 = d - T(2);
    T dp1 = d + T(1);
 
    // Precomputed division multipliers: 1/6 ~ 0.16666667, 1/2 = 0.5
    static constexpr T inv6 = T(1.0 / 6.0);
    static constexpr T inv2 = T(0.5);
 
    T l0 = -(d * dm1 * dm2) * inv6;
    T l1 =  (dp1 * dm1 * dm2) * inv2;
    T l2 = -(dp1 * d * dm2) * inv2;
    T l3 =  (dp1 * d * dm1) * inv6;
 
    return l0 * y0 + l1 * y1 + l2 * y2 + l3 * y3;
}
 
template <FloatType T>
[[nodiscard]] inline T interpolateLagrange(const T* buffer, int length, T position) noexcept {
    assert(length > 0 && position >= T(0) && position < static_cast<T>(length));
    if (length <= 0) return T(0);
 
    // Same release-safe clamp as the linear overload (undefined int cast /
    // out-of-bounds reads otherwise).
    int idx1;
    T frac;
    if (!(position >= T(0))) { idx1 = 0; frac = T(0); }
    else if (position >= static_cast<T>(length)) { idx1 = length - 1; frac = T(0); }
    else {
        idx1 = static_cast<int>(position);
        frac = position - static_cast<T>(idx1);
    }
 
    int idx0 = (idx1 > 0) ? idx1 - 1 : length - 1;
    int idx2 = idx1 + 1; if (idx2 >= length) idx2 -= length;
    int idx3 = idx2 + 1; if (idx3 >= length) idx3 -= length;
 
    return interpolateLagrange(buffer[idx0], buffer[idx1], buffer[idx2], buffer[idx3], frac);
}
 
template <FloatType T>
[[nodiscard]] inline T interpolateAllpass(T currentSample, T previousSample,
                                          T frac, T& state) noexcept
{
    // frac == 0 would place the pole exactly at z = -1 (a marginally stable
    // resonator at Nyquist) and frac == -1 would divide by zero: clamp away
    // from both. The clamp changes the delivered delay only for out-of-range
    // requests.
    T safeFrac = (frac < T(0.001)) ? T(0.001) : frac;
    T coeff = (T(1) - safeFrac) / (T(1) + safeFrac);
 
    T output = coeff * (currentSample - state) + previousSample;
 
    // Cut the recursive tail below ~-300 dBFS so a decaying state can never
    // reach the denormal range (matters on targets without FTZ/DAZ).
    if (std::abs(output) < T(1e-15)) output = T(0);
 
    state = output;
    return output;
}
 
template <FloatType T>
class SincInterpolator
{
public:
    static constexpr int kTaps   = 32;          
    static constexpr int kBefore = kTaps / 2 - 1; 
    static constexpr int kAfter  = kTaps / 2;   
    static constexpr int kPhases = 256;         
 
    SincInterpolator()
        : table_(static_cast<size_t>((kPhases + 1) * kTaps))
    {
        constexpr double kBeta = 10.0;
        constexpr double kPi = 3.14159265358979323846;
        const double invI0Beta = 1.0 / besselI0(kBeta);
        const double halfSpan = static_cast<double>(kTaps) / 2.0;
 
        for (int p = 0; p <= kPhases; ++p)
        {
            T* row = table_.data() + static_cast<size_t>(p) * kTaps;
            const double phase = static_cast<double>(p) / kPhases;
            if (p == 0 || p == kPhases)
            {
                // Integer positions: an exact identity (sin(pi * k) rounds
                // to ~1e-16, not 0, so the sinc is not evaluated here).
                std::fill(row, row + kTaps, T(0));
                row[p == 0 ? kBefore : kBefore + 1] = T(1);
                continue;
            }
            double h[kTaps];
            double sum = 0.0;
            for (int t = 0; t < kTaps; ++t)
            {
                const double x = static_cast<double>(t - kBefore) - phase;
                const double r = x / halfSpan;
                const double window = (r * r < 1.0)
                    ? besselI0(kBeta * std::sqrt(1.0 - r * r)) * invI0Beta
                    : 0.0;
                h[t] = std::sin(kPi * x) / (kPi * x) * window;
                sum += h[t];
            }
            for (int t = 0; t < kTaps; ++t)
                row[t] = static_cast<T>(h[t] / sum);   // unit DC gain
        }
    }
 
    [[nodiscard]] T read(const T* x, double frac) const noexcept
    {
        const double p = frac * static_cast<double>(kPhases);
        const int ip = std::clamp(static_cast<int>(p), 0, kPhases - 1);
        const T t = static_cast<T>(p - static_cast<double>(ip));
        const T* h0 = table_.data() + static_cast<size_t>(ip) * kTaps;
        const T* h1 = h0 + kTaps;   // the table holds kPhases + 1 rows
        const T a = simd::dotProductT(h0, x, kTaps);
        const T b = simd::dotProductT(h1, x, kTaps);
        return a + t * (b - a);
    }
 
    [[nodiscard]] T readRing(const T* ring, int64_t mask, int64_t intPos, double frac) const noexcept
    {
        const int64_t first = intPos - kBefore;
        const auto start = static_cast<size_t>(first & mask);
        if (start + static_cast<size_t>(kTaps) <= static_cast<size_t>(mask) + 1)
            return read(ring + start, frac);   // contiguous: no gather
        T window[kTaps];
        for (int t = 0; t < kTaps; ++t)
            window[t] = ring[static_cast<size_t>((first + t) & mask)];
        return read(window, frac);
    }
 
private:
    [[nodiscard]] static double besselI0(double x) noexcept
    {
        double sum = 1.0, term = 1.0;
        const double halfX = x / 2.0;
        for (int k = 1; k < 60; ++k)
        {
            const double f = halfX / static_cast<double>(k);
            term *= f * f;
            sum += term;
            if (term < sum * 1e-17) break;
        }
        return sum;
    }
 
    std::vector<T> table_;   
};
 
template <FloatType T>
class StretchedSincReader
{
public:
    static constexpr int kSteps = 16;          
    static constexpr int kStepsPerOctave = 8;  
    static constexpr int kPhases = 64;         
    static constexpr int kMaxTaps = 128;       
 
    StretchedSincReader()
    {
        constexpr double kBeta = 10.0;
        constexpr double kPi = 3.14159265358979323846;
        const double invI0Beta = 1.0 / besselI0(kBeta);
 
        size_t total = 0;
        for (int k = 0; k < kSteps; ++k)
        {
            const double rate = std::exp2(static_cast<double>(k + 1) / kStepsPerOctave);
            half_[k] = static_cast<int>(std::ceil(16.0 * rate));
            offset_[k] = total;
            total += static_cast<size_t>(kPhases + 1) * static_cast<size_t>(2 * half_[k]);
        }
        table_.assign(total, T(0));
 
        std::vector<double> h;
        for (int k = 0; k < kSteps; ++k)
        {
            const double rate = std::exp2(static_cast<double>(k + 1) / kStepsPerOctave);
            const int taps = 2 * half_[k];
            h.assign(static_cast<size_t>(taps), 0.0);
            for (int p = 0; p <= kPhases; ++p)
            {
                const double phase = static_cast<double>(p) / kPhases;
                double sum = 0.0;
                for (int t = 0; t < taps; ++t)
                {
                    const double u = (static_cast<double>(t - (half_[k] - 1)) - phase) / rate;
                    const double r = u / 16.0;
                    const double window = (r * r < 1.0)
                        ? besselI0(kBeta * std::sqrt(1.0 - r * r)) * invI0Beta
                        : 0.0;
                    const double sinc = (u == 0.0) ? 1.0 : std::sin(kPi * u) / (kPi * u);
                    h[static_cast<size_t>(t)] = sinc * window;
                    sum += h[static_cast<size_t>(t)];
                }
                T* row = table_.data() + offset_[k] + static_cast<size_t>(p) * static_cast<size_t>(taps);
                for (int t = 0; t < taps; ++t)
                    row[t] = static_cast<T>(h[static_cast<size_t>(t)] / sum);   // unit DC gain
            }
        }
    }
 
    [[nodiscard]] static int stepFor(double rate) noexcept
    {
        const double steps = std::ceil(std::log2(std::max(rate, 1.0)) * kStepsPerOctave - 1e-9);
        return std::clamp(static_cast<int>(steps) - 1, 0, kSteps - 1);
    }
 
    [[nodiscard]] T readRing(const T* ring, int64_t mask, int64_t intPos, double frac, int step) const noexcept
    {
        const int half = half_[step];
        const int taps = 2 * half;
        const double p = frac * static_cast<double>(kPhases);
        const int ip = std::clamp(static_cast<int>(p), 0, kPhases - 1);
        const T t = static_cast<T>(p - static_cast<double>(ip));
        const T* h0 = table_.data() + offset_[step] + static_cast<size_t>(ip) * static_cast<size_t>(taps);
        const T* h1 = h0 + taps;
 
        const int64_t first = intPos - (half - 1);
        const auto start = static_cast<size_t>(first & mask);
        if (start + static_cast<size_t>(taps) > static_cast<size_t>(mask) + 1) [[unlikely]]
        {
            // The window straddles the ring's end: gather it (value-initialised,
            // so no compiler can see an unwritten element reach the dot product).
            T window[kMaxTaps] {};
            for (int k = 0; k < taps; ++k)
                window[k] = ring[static_cast<size_t>((first + k) & mask)];
            const T a = simd::dotProductT(h0, window, taps);
            const T b = simd::dotProductT(h1, window, taps);
            return a + t * (b - a);
        }
        const T* x = ring + start;
        const T a = simd::dotProductT(h0, x, taps);
        const T b = simd::dotProductT(h1, x, taps);
        return a + t * (b - a);
    }
 
    [[nodiscard]] int reach(int step) const noexcept { return half_[step]; }
 
private:
    [[nodiscard]] static double besselI0(double x) noexcept
    {
        double sum = 1.0, term = 1.0;
        const double halfX = x / 2.0;
        for (int k = 1; k < 60; ++k)
        {
            const double f = halfX / static_cast<double>(k);
            term *= f * f;
            sum += term;
            if (term < sum * 1e-17) break;
        }
        return sum;
    }
 
    std::vector<T> table_;     
    size_t offset_[kSteps] {}; 
    int half_[kSteps] {};      
};
 
} // namespace dspark