#include "PitchShifter.h"

#include <JuceHeader.h>

void PitchShifter::prepare (double sampleRate)
{
    window = (float) std::max (256.0, sampleRate * 0.035);
    baseDelay = window;
    bufferSize = (int) (baseDelay + window + 16.0f);
    buffer.assign ((size_t) bufferSize, 0.0f);
    writePos = 0;
    o = 0.0f;
}

void PitchShifter::reset()
{
    std::fill (buffer.begin(), buffer.end(), 0.0f);
    writePos = 0;
    o = 0.0f;
}

float PitchShifter::at (int index) const
{
    index %= bufferSize;
    if (index < 0)
        index += bufferSize;

    return buffer[(size_t) index];
}

float PitchShifter::readCubic (float pos) const
{
    const int i = (int) std::floor (pos);
    const float frac = pos - (float) i;

    const float x0 = at (i - 1);
    const float x1 = at (i);
    const float x2 = at (i + 1);
    const float x3 = at (i + 2);

    const float a0 = -0.5f * x0 + 1.5f * x1 - 1.5f * x2 + 0.5f * x3;
    const float a1 = x0 - 2.5f * x1 + 2.0f * x2 - 0.5f * x3;
    const float a2 = -0.5f * x0 + 0.5f * x2;
    const float a3 = x1;

    return ((a0 * frac + a1) * frac + a2) * frac + a3;
}

float PitchShifter::process (float input, float ratio)
{
    buffer[(size_t) writePos] = input;

    if (std::fabs (ratio - 1.0f) < 1e-4f)
        return input;

    const float theta = juce::MathConstants<float>::pi * o / window;
    const float g1 = std::sin (theta) * std::sin (theta);
    const float g2 = std::cos (theta) * std::cos (theta);

    float o2 = o + window * 0.5f;
    if (o2 >= window)
        o2 -= window;

    const float pos1 = (float) writePos - (baseDelay - o);
    const float pos2 = (float) writePos - (baseDelay - o2);

    const float result = g1 * readCubic (pos1) + g2 * readCubic (pos2);

    o += (ratio - 1.0f);
    if (o >= window)
        o -= window;
    else if (o < 0.0f)
        o += window;

    ++writePos;
    if (writePos >= bufferSize)
        writePos = 0;

    return result;
}