#include "Chorus.h"

namespace serum
{

void ChorusUnit::prepare (double sampleRate, int)
{
    sr = sampleRate;
    const int maxDelay = (int) (sr * 0.05);   // 50ms
    delayL.assign ((size_t) maxDelay, 0.0f);
    delayR.assign ((size_t) maxDelay, 0.0f);
    reset();
}

void ChorusUnit::reset()
{
    std::fill (delayL.begin(), delayL.end(), 0.0f);
    std::fill (delayR.begin(), delayR.end(), 0.0f);
    writePos = 0;
    lfoPhase = 0.0;
}

void ChorusUnit::process (float* l, float* r, int numSamples, const float p[4])
{
    const int len = (int) delayL.size();
    if (len < 4)
        return;

    const double rate  = 0.1 + p[0] * 4.9;              // 0.1..5 Hz
    const double depth = (0.5 + p[1] * 5.5) * sr / 1000.0;  // 0.5..6 ms
    const double base  = 8.0 * sr / 1000.0;             // 8 ms centre
    const float  width = p[2];

    for (int i = 0; i < numSamples; ++i)
    {
        const float inL = l[i], inR = r[i];
        delayL[(size_t) writePos] = inL;
        delayR[(size_t) writePos] = inR;

        lfoPhase += 6.28318530717958647692 * rate / sr;
        if (lfoPhase > 6.28318530717958647692) lfoPhase -= 6.28318530717958647692;

        float sumL = 0.0f, sumR = 0.0f;
        for (int v = 0; v < 2; ++v)
        {
            const double mod = std::sin (lfoPhase + v * 3.14159265358979323846) * depth;
            const double d = base + mod + (v == 1 ? width * depth * 0.5 : 0.0);
            double readPos = writePos - d;
            if (readPos < 0.0) readPos += len;
            const int i0 = (int) readPos;
            const int i1 = (i0 + 1) % len;
            const float frac = (float) (readPos - std::floor (readPos));
            sumL += delayL[(size_t) i0] * (1.0f - frac) + delayL[(size_t) i1] * frac;
            sumR += delayR[(size_t) i0] * (1.0f - frac) + delayR[(size_t) i1] * frac;
        }

        l[i] = sumL * 0.5f;
        r[i] = sumR * 0.5f;
        writePos = (writePos + 1) % len;
    }
}

} // namespace serum