FrequencyShifter.h

// DSPark - Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi - MIT License
 
#pragma once
 
#include "../Core/AudioBuffer.h"
#include "../Core/AudioSpec.h"
#include "../Core/DenormalGuard.h"
#include "../Core/DspMath.h"
#include "../Core/Hilbert.h"
#include "../Core/StateBlob.h"
 
#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <numbers>
#include <vector>
 
namespace dspark {
 
template <FloatType T>
class FrequencyShifter
{
public:
    void prepare(const AudioSpec& spec)
    {
        if (!spec.isValid()) return; // release-safe: keep previous state
 
        sampleRate_ = spec.sampleRate;
        mixMaxStep_ = static_cast<T>(1.0 / (spec.sampleRate * 0.02));
        numChannels_ = spec.numChannels;
 
        // Zero-allocations on audio thread: allocate all Hilberts during prepare.
        hilberts_.resize(numChannels_);
        for (auto& h : hilberts_) {
            h.prepare(spec.sampleRate);
        }
 
        currentMix_ = mix_.load(std::memory_order_relaxed);
        reset();
    }
 
    void processBlock(AudioBufferView<T> buffer) noexcept
    {
        const int numCh = std::min(buffer.getNumChannels(), numChannels_);
        const int numSamples = buffer.getNumSamples();
        if (numSamples == 0 || numCh == 0) return;
 
        // The Hilbert transformer runs per-sample over a 191-tap FIR; denormal
        // input samples would drop every tap into the x86 slow path. The
        // framework convention is that per-sample Hilbert callers install their
        // own guard (see Hilbert.h and Compressor.h).
        DenormalGuard guard;
 
        const T targetMix = mix_.load(std::memory_order_relaxed);
        const T shiftHz = shift_.load(std::memory_order_relaxed);
 
        // The mix is rate limited to full scale per 20 ms: an unsmoothed step
        // would jump the dry/wet crossfade audibly, and a per-block ramp
        // landed in 0.7 ms with 32-sample blocks. The shift needs no
        // smoothing - it only changes the speed of the quadrature carrier,
        // whose phase stays continuous across blocks.
        const T mixStart = currentMix_;
 
        // 1. Compute rotation matrix coefficients once per block
        const double w = (shiftHz * 2.0 * std::numbers::pi) / sampleRate_;
        const T cos_w = static_cast<T>(std::cos(w));
        const T sin_w = static_cast<T>(std::sin(w));
 
        // Cache the starting phase components for this block
        const T startCos = static_cast<T>(std::cos(phase_));
        const T startSin = static_cast<T>(std::sin(phase_));
 
        // 2. Process planar channels to maximize cache locality
        for (int ch = 0; ch < numCh; ++ch)
        {
            T* data = buffer.getChannel(ch);
            auto& hilbert = hilberts_[ch];
 
            // Local quadrature oscillator state (identical carrier and mix
            // ramp on every channel)
            T u = startCos;
            T v = startSin;
 
            for (int i = 0; i < numSamples; ++i)
            {
                const T smoothMix = moveTowards(mixStart, targetMix,
                                                mixMaxStep_ * static_cast<T>(i + 1));
 
                // Hilbert processing (I + jQ)
                auto h = hilbert.process(data[i]);
 
                // Modulate analytic signal: real part of (I+jQ) * (u+jv)
                T shifted = h.real * u - h.imag * v;
 
                // Mix correctly: Use h.real as the phase-aligned dry signal
                data[i] = h.real + (shifted - h.real) * smoothMix;
 
                // Advance quadrature oscillator: rotation matrix
                T next_u = u * cos_w - v * sin_w;
                T next_v = u * sin_w + v * cos_w;
                u = next_u;
                v = next_v;
            }
        }
 
        currentMix_ = moveTowards(mixStart, targetMix, mixMaxStep_ * static_cast<T>(numSamples));
 
        // 3. Advance absolute phase once per block to prevent float drift
        phase_ += w * numSamples;
 
        // Wrap phase precisely
        constexpr double kTwoPi = 2.0 * std::numbers::pi;
        phase_ = std::fmod(phase_, kTwoPi);
        if (phase_ < 0.0) phase_ += kTwoPi;
    }
 
    void reset() noexcept
    {
        phase_ = 0.0;
        for (auto& h : hilberts_) {
            h.reset();
        }
    }
 
    void setShift(T hz) noexcept
    {
        if (!std::isfinite(hz)) return;
        shift_.store(hz, std::memory_order_relaxed);
    }
 
    void setMix(T mix) noexcept
    {
        if (!std::isfinite(mix)) return;
        mix_.store(std::clamp(mix, T(0), T(1)), std::memory_order_relaxed);
    }
 
    [[nodiscard]] T getShift() const noexcept { return shift_.load(std::memory_order_relaxed); }
 
    [[nodiscard]] T getMix() const noexcept { return mix_.load(std::memory_order_relaxed); }
 
    [[nodiscard]] static constexpr int getLatency() noexcept
    {
        return Hilbert<T>::getLatencySamples();
    }
 
    [[nodiscard]] std::vector<uint8_t> getState() const
    {
        // The blob stores float (setState reads float back); the explicit
        // casts also keep this overload resolvable when T is double.
        StateWriter w(stateId("FSHF"), 1);
        w.write("shift", static_cast<float>(shift_.load(std::memory_order_relaxed)));
        w.write("mix", static_cast<float>(mix_.load(std::memory_order_relaxed)));
        return w.blob();
    }
 
    bool setState(const uint8_t* data, size_t size)
    {
        StateReader r(data, size);
        if (!r.isValid() || r.processorId() != stateId("FSHF")) return false;
        setShift(static_cast<T>(r.read("shift", 0.0f)));
        setMix(static_cast<T>(r.read("mix", 1.0f)));
        return true;
    }
 
private:
    double sampleRate_ = 44100.0;
    int numChannels_ = 0;
 
    // Using double for phase accumulation to prevent drift over long sessions
    double phase_ = 0.0;
 
    std::atomic<T> shift_{ T(0) };
    std::atomic<T> mix_{ T(1) };
 
    // Smoothed state for the audio thread
    T currentMix_{ T(1) };
    T mixMaxStep_{ T(1.0 / 960.0) };   
 
    std::vector<Hilbert<T>> hilberts_;
};
 
} // namespace dspark