// pitch_shift — pure implementation. See pitch_shift.h for the contract, the S16-F2
// route-(b) rationale (WDL drags <windows.h>), and the GA-Preserve root cause that replaced
// the naive dual-tap OLA with correlation-aligned splices.
// NO VST3 / REAPER / SWELL / vendor includes; standard library only.
//
// Algorithm: a delay ring of 2*window frames. The write head advances one frame per input
// sample (source rate -> duration preserved). ONE active read tap advances by the shift
// `ratio_` per frame, so its delay behind the writer drifts at (1 - ratio) per frame. When
// that delay leaves the safe band [dLow, dHigh], the tap is RELOCATED by a nominal jump of
// one window (+window toward older content for up-shifts, -window toward the writer for
// down-shifts), refined by a cross-correlation search over +/- maxLag so the relocated read
// point is WAVEFORM-ALIGNED with what the outgoing tap was about to play. Old and new taps
// then crossfade over fadeFrames with a raised-cosine, amplitude-complementary pair (in-phase
// content sums to exactly unity gain). For a pure sine the correlation snaps the jump to an
// integer period count, so the output stays a single tone at the shifted frequency — the
// GA-Preserve acceptance bar. At unity ratio the delay is frozen mid-band and no splice ever
// fires: the shifter is a clean window/2 delay.

#include "pitch_shift.h"

#include <algorithm>
#include <cmath>
#include <limits>

namespace reasampler {

namespace {

constexpr double kPi = 3.14159265358979323846;

}  // namespace

void PitchShifter::configure(std::int64_t windowFrames) {
    window_ = windowFrames;
    if (window_ <= 1) {
        // Pass-through: no ring, process() returns input unchanged.
        ring_.clear();
        ringLen_ = 0;
        writePos_ = 0;
        posA_ = posB_ = 0.0;
        fading_ = false;
        fadePos_ = 0;
        fadeFrames_ = fadeLen_ = maxLag_ = corrFrames_ = dLow_ = dHigh_ = 0;
        ratio_ = 1.0;
        return;
    }
    // 2x-window ring: one window of splice-jump span plus search + fade headroom on each side.
    ringLen_ = 2 * window_;
    ring_.assign(static_cast<std::size_t>(ringLen_), 0.0f);
    // Geometry (all quarters of the window):
    //  - fadeFrames_: the NOMINAL splice crossfade. This window/4 length is only safe when
    //    the outgoing tap cannot reach the writer before the fade ends; splice() scales the
    //    live fade length (fadeLen_) down by the current ratio for up-shifts past ~2x, so
    //    ordinary sampler transpositions (+24 st = ratio 4) never read stale data mid-fade.
    //  - maxLag_: the alignment search half-range — one window/4 covers a full period of any
    //    tone down to 4/window cycles-per-frame (~80 Hz at the product's 50 ms window, 44.1k).
    //  - dLow_/dHigh_: the safe delay band; unity parks the tap mid-band (window/2 delay).
    //  - corrFrames_: the correlation segment length. At an up-splice the reference segment
    //    reads FORWARD from the tap at delay ~dLow_, so dLow_-1 frames is exactly what exists
    //    between the tap and the writer — the cap expresses that safety rather than leaving
    //    it coincidental. 512 bounds the splice burst.
    fadeFrames_ = std::max<std::int64_t>(window_ / 4, 1);
    maxLag_ = window_ / 4;
    dLow_ = window_ / 4;
    dHigh_ = ringLen_ - window_ / 4;
    corrFrames_ = std::max<std::int64_t>(1, std::min<std::int64_t>(dLow_ - 1, 512));
    fadeLen_ = 0;
    reset();
}

void PitchShifter::reset() {
    if (window_ > 1) {
        // Zero the ring and seed the active tap half a window behind the writer — mid safe
        // band, so unity holds it there forever and either shift direction has drift room.
        std::fill(ring_.begin(), ring_.end(), 0.0f);
        writePos_ = 0;
        posA_ = static_cast<double>(ringLen_ - window_ / 2);
        posB_ = posA_;
        fading_ = false;
        fadePos_ = 0;
        fadeLen_ = 0;
    } else {
        writePos_ = 0;
        posA_ = posB_ = 0.0;
        fading_ = false;
        fadePos_ = 0;
        fadeLen_ = 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 tap reaches 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 the tap backward/stall)
}

double PitchShifter::readTap(double pos) const {
    // Fractional linear interpolation with ring wrap.
    double p = pos;
    const double len = static_cast<double>(ringLen_);
    while (p < 0.0) p += len;
    while (p >= len) p -= len;
    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 >= ringLen_) 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;
}

void PitchShifter::splice(std::int64_t nominalJump) {
    // Relocate the active tap by `nominalJump` frames of ADDED delay (+window_ = jump toward
    // older content, -window_ = jump toward the writer), refined by a correlation search so
    // the relocated read point is waveform-aligned with the outgoing tap's upcoming content.
    // The search is coarse (step 4 over +/- maxLag_) then fine (+/- 3 around the coarse best):
    // a bounded burst of ~ (maxLag_/2 + 7) * corrFrames_ multiply-adds, once per splice.
    const std::int64_t iA =
        ((static_cast<std::int64_t>(posA_) % ringLen_) + ringLen_) % ringLen_;

    auto scoreAt = [&](std::int64_t lag) -> double {
        std::int64_t ia = iA;
        std::int64_t ic = ((iA - nominalJump + lag) % ringLen_ + ringLen_) % ringLen_;
        double s = 0.0, ec = 0.0;
        for (std::int64_t k = 0; k < corrFrames_; ++k) {
            const double a = static_cast<double>(ring_[static_cast<std::size_t>(ia)]);
            const double c = static_cast<double>(ring_[static_cast<std::size_t>(ic)]);
            s += a * c;
            ec += c * c;
            if (++ia >= ringLen_) ia = 0;
            if (++ic >= ringLen_) ic = 0;
        }
        // NORMALIZED cross-correlation (standard SOLA): a raw dot product is biased toward
        // the higher-energy lag, so on a decaying tail every up-splice would prefer the
        // loudest candidate over the best-ALIGNED one — a small level step per splice that
        // the amplitude-complementary fade cannot hide. The reference segment's energy is
        // constant across lags, so dividing by sqrt(Ec) alone ranks identically to the full
        // normalized form. A zero-energy candidate scores 0 (splicing into silence is benign).
        return ec > 0.0 ? s / std::sqrt(ec) : 0.0;
    };

    std::int64_t bestLag = 0;
    double bestScore = -std::numeric_limits<double>::infinity();
    for (std::int64_t lag = -maxLag_; lag <= maxLag_; lag += 4) {
        const double s = scoreAt(lag);
        if (s > bestScore) {
            bestScore = s;
            bestLag = lag;
        }
    }
    const std::int64_t coarse = bestLag;
    for (std::int64_t lag = coarse - 3; lag <= coarse + 3; ++lag) {
        if (lag == coarse || lag < -maxLag_ || lag > maxLag_) continue;
        const double s = scoreAt(lag);
        if (s > bestScore) {
            bestScore = s;
            bestLag = lag;
        }
    }

    // Hand the current position to the outgoing tap and relocate the active one. Integer lag
    // on top of the nominal jump preserves posA_'s fractional part — sub-sample continuity
    // between the two taps, so the residual phase error is bounded by half a sample.
    posB_ = posA_;
    double p = posA_ - static_cast<double>(nominalJump) + static_cast<double>(bestLag);
    const double len = static_cast<double>(ringLen_);
    while (p < 0.0) p += len;
    while (p >= len) p -= len;
    posA_ = p;
    // RATIO-SCALED fade length. At an up-splice the OUTGOING tap starts at ~dLow_ delay and
    // keeps draining toward the writer at (ratio - 1) per output frame; the nominal window/4
    // fade only keeps it behind the writer for ratios up to 2. Beyond that (e.g. +24 st =
    // ratio 4, an ordinary sampler transposition) it would cross mid-fade and play stale
    // read-ahead data at substantial gain — a periodic seam. So cap the live fade at the
    // frames of drain headroom actually available, minus 2 (1 for the trigger's sub-dLow_
    // undershoot, 1 for the interpolator's read-ahead). Ratios <= ~2 keep the full nominal
    // fade; ratio 4 gets ~window/12 — shorter but still a smooth burst. Down-shifts grow the
    // outgoing delay at (1 - ratio) < 1 per frame and cannot reach the ring end within
    // window/4 frames, so they always keep the full fade. A pitch-envelope ratio slew
    // mid-fade is covered by the same margin for any realistic per-frame bias.
    fadeLen_ = fadeFrames_;
    if (ratio_ > 1.0) {
        const double headroom = static_cast<double>(dLow_) - (ratio_ - 1.0) - 2.0;
        const std::int64_t safe =
            headroom > 0.0 ? static_cast<std::int64_t>(headroom / (ratio_ - 1.0)) : 1;
        fadeLen_ = std::max<std::int64_t>(1, std::min(fadeFrames_, safe));
    }
    fading_ = true;
    fadePos_ = 0;
}

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;

    // 2. Read the active tap; while a splice fade is live, crossfade against the outgoing tap.
    //    Raised-cosine COMPLEMENTARY gains (gNew + gOld == 1): correlation-aligned content is
    //    in phase, so the sum holds unity amplitude through the fade (equal-power would bulge).
    double out = readTap(posA_);
    if (fading_) {
        const double t = static_cast<double>(fadePos_) / static_cast<double>(fadeLen_);
        const double gNew = 0.5 * (1.0 - std::cos(kPi * t));
        out = gNew * out + (1.0 - gNew) * readTap(posB_);
        if (++fadePos_ >= fadeLen_) fading_ = false;
    } else {
        // 3. Splice scheduling: relocate when the active tap's delay leaves the safe band.
        //    Up-shifts (ratio > 1) drain the delay toward 0 -> jump one window OLDER; down-
        //    shifts grow it toward the ring length -> jump one window TOWARD the writer. At
        //    unity the delay is frozen at window/2 and neither trigger ever fires.
        double d = static_cast<double>(writePos_) - posA_;
        const double len = static_cast<double>(ringLen_);
        while (d < 0.0) d += len;
        while (d >= len) d -= len;
        if (d <= static_cast<double>(dLow_)) {
            splice(+window_);
        } else if (d >= static_cast<double>(dHigh_)) {
            splice(-window_);
        }
    }

    // 4. Advance heads: write head one frame (source rate), tap(s) by the shift ratio.
    ++writePos_;
    if (writePos_ >= ringLen_) writePos_ = 0;
    const double len = static_cast<double>(ringLen_);
    posA_ += ratio_;
    while (posA_ >= len) posA_ -= len;
    if (fading_) {
        posB_ += ratio_;
        while (posB_ >= len) posB_ -= len;
    }

    return static_cast<AudioSample>(out);
}

}  // namespace reasampler