#include "Flanger.h"

namespace serum
{

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

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

void FlangerUnit::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.05 + p[0] * 2.0;
    const double depth    = (0.3 + p[1] * 4.7) * sr / 1000.0;   // 0.3..5 ms
    const double base     = 1.5 * sr / 1000.0;
    const float  feedback = p[2] * 0.9f;

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

        lfoPhase += 6.28318530717958647692 * rate / sr;
        if (lfoPhase > 6.28318530717958647692) lfoPhase -= 6.28318530717958647692;
        const double mod = std::sin (lfoPhase) * depth;

        double readPos = writePos - (base + mod);
        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));
        const float outL = delayL[(size_t) i0] * (1.0f - frac) + delayL[(size_t) i1] * frac;
        const float outR = delayR[(size_t) i0] * (1.0f - frac) + delayR[(size_t) i1] * frac;

        delayL[(size_t) writePos] = inL + outL * feedback;
        delayR[(size_t) writePos] = inR + outR * feedback;

        l[i] = outL;
        r[i] = outR;
        writePos = (writePos + 1) % len;
    }
}

} // namespace serum