// pitch_shift — pure implementation. See pitch_shift.h for the contract and the S16-F2
// route-(b) rationale (WDL drags <windows.h>, so the Preserve DSP is house-native here).
// NO VST3 / REAPER / SWELL / vendor includes; standard library only.
//
// Algorithm: a single delay ring of `window_` frames. The write head advances one frame per
// input sample (source rate → duration preserved). TWO read taps chase the write head, offset
// by half a window; each advances by the shift `ratio_` per frame. A tap that would cross the
// write head wraps by a full window (so it stays a bounded delay behind the writer). The two
// taps are crossfaded by an equal-power window keyed to each tap's distance from the write
// head, so the wrap discontinuity of one tap is masked by the other mid-window — the classic
// two-grain time-domain pitch shifter, no FFT.

#include "pitch_shift.h"

#include <algorithm>
#include <cmath>

namespace reasampler {

namespace {

// A Hann OLA window over a grain phase in [0,1): 0.5(1 - cos(2*pi*phase)). Zero at the grain
// ends (where a tap wraps — the discontinuity), unity mid-grain. Two grains offset by half a
// window PARTITION UNITY (w(p) + w(p+0.5) == 1 for all p), so the two crossfaded taps sum to a
// gain of exactly 1 everywhere — no amplitude ripple across the window, and each tap's wrap
// seam is masked because its window is 0 exactly there.
double hannWeight(double phase) {
    while (phase < 0.0) phase += 1.0;
    while (phase >= 1.0) phase -= 1.0;
    return 0.5 * (1.0 - std::cos(2.0 * 3.14159265358979323846 * phase));
}

}  // namespace

void PitchShifter::configure(std::int64_t windowFrames) {
    window_ = windowFrames;
    if (window_ <= 1) {
        // Pass-through: no ring, process() returns input unchanged.
        ring_.clear();
        writePos_ = 0;
        readPos_ = 0.0;
        ratio_ = 1.0;
        return;
    }
    ring_.assign(static_cast<std::size_t>(window_), 0.0f);
    reset();
}

void PitchShifter::reset() {
    if (window_ > 1) {
        // Zero the ring and seed the read head a half-window behind the writer so the two taps
        // (readPos_ and readPos_ + window/2) straddle the writer from the first frame.
        std::fill(ring_.begin(), ring_.end(), 0.0f);
        writePos_ = 0;
        readPos_ = static_cast<double>(window_) / 2.0;
    } else {
        writePos_ = 0;
        readPos_ = 0.0;
    }
    ratio_ = 1.0;
}

void PitchShifter::warm() {
    if (window_ <= 1) return;  // pass-through needs no warm-up
    // Push one full window of silence so the taps reach steady state before real audio.
    for (std::int64_t i = 0; i < window_; ++i) process(0.0f);
}

void PitchShifter::setShiftRatio(double ratio) {
    if (ratio > 0.0) ratio_ = ratio;  // ignore non-positive (never run taps backward/stall)
}

AudioSample PitchShifter::process(AudioSample in) {
    if (window_ <= 1) return in;  // pass-through (unconfigured / degenerate)

    // 1. Write the incoming sample at the write head (source rate).
    ring_[static_cast<std::size_t>(writePos_)] = in;

    const double w = static_cast<double>(window_);
    const double half = w / 2.0;

    // 2. Read the two taps, each a bounded delay behind the writer. tap0 is `readPos_`; tap1 is
    //    a half-window ahead of it (mod window). Distance-from-writer drives the crossfade so a
    //    tap near the writer (about to wrap) is faded out while its partner (mid-window) is up.
    auto readTap = [&](double pos) -> double {
        // Fractional linear interpolation with ring wrap.
        double p = pos;
        while (p < 0.0) p += w;
        while (p >= w) p -= w;
        const std::int64_t i0 = static_cast<std::int64_t>(p);
        const double frac = p - static_cast<double>(i0);
        std::int64_t i1 = i0 + 1;
        if (i1 >= window_) i1 = 0;
        const double s0 = static_cast<double>(ring_[static_cast<std::size_t>(i0)]);
        const double s1 = static_cast<double>(ring_[static_cast<std::size_t>(i1)]);
        return s0 + (s1 - s0) * frac;
    };

    const double tap0 = readTap(readPos_);
    const double tap1 = readTap(readPos_ + half);

    // Distance of tap0 behind the write head, in [0, window). Its crossfade phase is that
    // distance over the window; tap1 (half a window offset) gets the complementary phase.
    double dist0 = static_cast<double>(writePos_) - readPos_;
    while (dist0 < 0.0) dist0 += w;
    while (dist0 >= w) dist0 -= w;
    const double phase0 = dist0 / w;

    // Hann windows offset by half a grain partition unity, so the two taps sum to gain 1 with
    // each tap's wrap seam masked by its window zero. phase0 drives tap0; tap1 (half-window
    // offset) is at phase0 + 0.5.
    const double g0 = hannWeight(phase0);
    const double g1 = hannWeight(phase0 + 0.5);
    const double out = tap0 * g0 + tap1 * g1;

    // 3. Advance heads: write head one frame (source rate), read head by the shift ratio.
    ++writePos_;
    if (writePos_ >= window_) writePos_ = 0;
    readPos_ += ratio_;
    while (readPos_ >= w) readPos_ -= w;
    while (readPos_ < 0.0) readPos_ += w;

    return static_cast<AudioSample>(out);
}

}  // namespace reasampler
