/*
   sonnenlicht poly ensemble

   Copyright 2025 Gordon JC Pearce <gordonjcp@gjcp.net>

   Permission to use, copy, modify, and/or distribute this software for any
   purpose with or without fee is hereby granted, provided that the above
   copyright notice and this permission notice appear in all copies.

   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
   MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
   SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
   WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
   OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/

#include "chorus.hpp"

#include <math.h>
#include <stdio.h>
#include <string.h>
Chorus::Chorus() {
    lpfOut1 = new float[bufferSize];
    lpfOut2 = new float[bufferSize];
    ram = new float[DELAYSIZE];

    lfoPhase = 1;
    lfoSpeed = 6.283 * 10.7 / sampleRate;  // plainly silly value to show if it hasn't been set

    gainTC = 1 - exp(-6.283 * 10 / sampleRate);  // 1/10th of a second declick
    bbdTC = 1 - exp(-6.283 * 30 / sampleRate);   // hpf into BBD at 159Hz

    // not quite Butterworth but you'd never hear the difference
    // these are calculated from the real-world component values
    postFilter1l = new SVF(9688, .549);
    postFilter2l = new SVF(10377, 1.291);
    postFilter1r = new SVF(9688, .549);
    postFilter2r = new SVF(10377, 1.291);

    // zero out the delay buffer
    memset(ram, 0, sizeof(float) * DELAYSIZE);
    memset(lpfOut1, 0, sizeof(float) * bufferSize);
    memset(lpfOut2, 0, sizeof(float) * bufferSize);
}

Chorus::~Chorus() {
    delete lpfOut1;
    delete lpfOut2;
    delete ram;
    delete postFilter1l;
    delete postFilter2l;
    delete postFilter1r;
    delete postFilter2r;
}

void Chorus::run(float* input, float** outputs, uint32_t frames) {
    // run highpass / bass boost and stereo chorus effect for one full block

    float s0 = 0, s1 = 0;
    float dly1, frac, flt;
    uint16_t tap, delay;

    for (uint32_t i = 0; i < frames; i++) {
        // run a step of LFO
        lfoPhase += (lfoState & 0x01) ? lfoSpeed : -lfoSpeed;

        if (abs(lfoPhase) > 1) {
            // lfoPhase -= 2;
            lfoState++;
        }

        // highpass/bass boost
        flt = ((input[i] - hpDelay) * hpCut) + hpDelay;
        hpDelay = flt;
        input[i] += (flt * hpGain);

        flt = ((input[i] - bbdRC) * bbdTC) + bbdRC;
        bbdRC = flt;

        ram[delayptr] = input[i] - flt;

// delays in milliseconds
#define BASE 0.0035
#define AMT 0.002

        dly1 = (BASE + (AMT * lfoPhase)) * sampleRate;
        delay = (int)dly1;
        frac = dly1 - delay;

        tap = delayptr - delay;
        s1 = ram[(tap - 1) & 0x3ff];
        s0 = ram[tap & 0x3ff];
        lpfOut1[i] = ((s1 - s0) * frac) + s0;

        dly1 = (BASE - (AMT * lfoPhase)) * sampleRate;
        delay = (int)dly1;
        frac = dly1 - delay;

        tap = delayptr - delay;
        s1 = ram[(tap - 1) & 0x3ff];
        s0 = ram[tap & 0x3ff];
        lpfOut2[i] = ((s1 - s0) * frac) + s0;

        delayptr++;
        delayptr &= 0x3ff;
    }
    postFilter1l->runSVF(lpfOut1, lpfOut1, frames);
    postFilter2l->runSVF(lpfOut1, lpfOut1, frames);
    postFilter1r->runSVF(lpfOut2, lpfOut2, frames);
    postFilter2r->runSVF(lpfOut2, lpfOut2, frames);

    for (uint32_t i = 0; i < frames; i++) {
        float y = input[i];
        gainRC = (gain - gainRC) * gainTC + gainRC;
        outputs[0][i] = y + (gainRC * lpfOut1[i]);
        outputs[1][i] = y + (gainRC * lpfOut2[i]);
    }
}

void Chorus::setHpf(uint8_t mode) {
    // the simple 1-pole lowpass has its output
    // subtracted from input for the highpass
    // or added for bass boost
    // cutoff is calculated as
    // k = 1-exp(-2pi * Fc * sampleRate)
    switch (mode) {
        case 0x00:
            hpCut = 1 - exp(-6.283 * 720 / sampleRate);
            hpGain = -1;
            break;
        case 0x08:
            hpCut = 1 - exp(-6.283 * 225 / sampleRate);
            hpGain = -1;
            break;
        case 0x10:
            hpCut = 1;
            hpGain = 0;
            break;
        case 0x18:
            hpCut = 1 - exp(-6.283 * 85 / sampleRate);
            hpGain = 1.707;
            break;
    }
}

void Chorus::setChorus(uint8_t mode) {
    // switch chorus mode
    switch (mode) {
        case 0x60:
        case 0x20:
            gain = 0;
            break;
        case 0x40:
            gain = 1.2;
            lfoSpeed = 6.283 * 0.525 / sampleRate / 2;
            break;
        case 0x00:
            gain = 1.2;
            lfoSpeed = 6.283 * 0.85 / sampleRate / 2;
            break;
    }
}