#include "Reverb.h"

namespace serum
{

namespace
{
    constexpr int combTuning[8]   = { 1116, 1188, 1277, 1356, 1422, 1491, 1557, 1617 };
    constexpr int allpassTuning[4] = { 556, 441, 341, 225 };
}

void ReverbUnit::prepare (double sampleRate, int)
{
    sr = sampleRate;
    const double scale = sr / 44100.0;

    for (int i = 0; i < 8; ++i)
    {
        const int len = (int) (combTuning[i] * scale) + 1;
        combL[(size_t) i].assign ((size_t) len, 0.0f);
        combR[(size_t) i].assign ((size_t) len, 0.0f);
    }
    for (int i = 0; i < 4; ++i)
    {
        const int len = (int) (allpassTuning[i] * scale) + 1;
        apL[(size_t) i].assign ((size_t) len, 0.0f);
        apR[(size_t) i].assign ((size_t) len, 0.0f);
    }

    const int preLen = (int) (sr * 0.2);
    preL.assign ((size_t) preLen, 0.0f);
    preR.assign ((size_t) preLen, 0.0f);
    reset();
}

void ReverbUnit::reset()
{
    for (int i = 0; i < 8; ++i)
    {
        std::fill (combL[(size_t) i].begin(), combL[(size_t) i].end(), 0.0f);
        std::fill (combR[(size_t) i].begin(), combR[(size_t) i].end(), 0.0f);
        combPosL[(size_t) i] = combPosR[(size_t) i] = 0;
        combFiltL[(size_t) i] = combFiltR[(size_t) i] = 0.0f;
    }
    for (int i = 0; i < 4; ++i)
    {
        std::fill (apL[(size_t) i].begin(), apL[(size_t) i].end(), 0.0f);
        std::fill (apR[(size_t) i].begin(), apR[(size_t) i].end(), 0.0f);
        apPosL[(size_t) i] = apPosR[(size_t) i] = 0;
    }
    std::fill (preL.begin(), preL.end(), 0.0f);
    std::fill (preR.begin(), preR.end(), 0.0f);
    prePos = 0;
}

void ReverbUnit::process (float* l, float* r, int numSamples, const float p[4])
{
    const float feedback = maps::sizeToValue (p[0]);
    const float damping  = p[1];
    const float width    = p[2];
    const int   preLen   = (int) preL.size();
    const float preDelay = (preLen > 0) ? p[3] * 0.15f * sr : 0.0f;

    for (int i = 0; i < numSamples; ++i)
    {
        // Predelay.
        float inL = l[i], inR = r[i];
        if (preLen > 0)
        {
            const int read = (prePos - (int) preDelay + preLen) % preLen;
            preL[(size_t) prePos] = inL;
            preR[(size_t) prePos] = inR;
            inL = preL[(size_t) read];
            inR = preR[(size_t) read];
            prePos = (prePos + 1) % preLen;
        }

        // Combs.
        float sumL = 0.0f, sumR = 0.0f;
        for (int c = 0; c < 8; ++c)
        {
            auto& buf = combL[(size_t) c];
            const int pos = combPosL[(size_t) c];
            float out = buf[(size_t) pos];
            combFiltL[(size_t) c] = out * (1.0f - damping) + combFiltL[(size_t) c] * damping;
            buf[(size_t) pos] = inL + combFiltL[(size_t) c] * feedback;
            combPosL[(size_t) c] = (pos + 1) % (int) buf.size();
            sumL += out;
        }
        for (int c = 0; c < 8; ++c)
        {
            auto& buf = combR[(size_t) c];
            const int pos = combPosR[(size_t) c];
            float out = buf[(size_t) pos];
            combFiltR[(size_t) c] = out * (1.0f - damping) + combFiltR[(size_t) c] * damping;
            buf[(size_t) pos] = inR + combFiltR[(size_t) c] * feedback;
            combPosR[(size_t) c] = (pos + 1) % (int) buf.size();
            sumR += out;
        }

        // Allpasses.
        float aL = sumL, aR = sumR;
        for (int a = 0; a < 4; ++a)
        {
            auto& buf = apL[(size_t) a];
            const int pos = apPosL[(size_t) a];
            const float stored = buf[(size_t) pos];
            buf[(size_t) pos] = aL + stored * 0.5f;
            aL = stored - aL;
            apPosL[(size_t) a] = (pos + 1) % (int) buf.size();
        }
        for (int a = 0; a < 4; ++a)
        {
            auto& buf = apR[(size_t) a];
            const int pos = apPosR[(size_t) a];
            const float stored = buf[(size_t) pos];
            buf[(size_t) pos] = aR + stored * 0.5f;
            aR = stored - aR;
            apPosR[(size_t) a] = (pos + 1) % (int) buf.size();
        }

        // Width (stereo cross-mix).
        const float w = 1.0f - width * 0.5f;
        const float outL = aL * w + aR * (1.0f - w);
        const float outR = aR * w + aL * (1.0f - w);

        l[i] = outL;
        r[i] = outR;
    }
}

} // namespace serum