#include "fx_equalizer.h"
#include <math.h>
#include <stdint.h>

#define FX_EQ_SAMPLE_RATE 48000.0f

static const float fx_eq_default_freqs[FX_EQ_NUM_BANDS] =
{
    31.0f, 62.0f, 125.0f, 250.0f, 500.0f, 1000.0f, 2000.0f, 4000.0f, 8000.0f, 16000.0f
};

void FxEqualizer_Init(FxEqualizerState *state, FxEqualizerParams *params)
{
    params->enabled = true;
    for (uint32_t i = 0; i < FX_EQ_NUM_BANDS; i++)
    {
        params->bands[i].freq_hz = fx_eq_default_freqs[i];
        params->bands[i].gain_db = 0.0f;
        params->bands[i].q       = 1.0f;

        state->band_state[i].last_freq_hz = -1.0f; /* force le calcul des coefficients au 1er sample */
        state->band_state[i].last_gain_db = 0.0f;
        state->band_state[i].last_q       = 0.0f;
        state->x1[i] = state->x2[i] = state->y1[i] = state->y2[i] = 0.0f;
    }

    // Pour couper les basses par defaukt et éviter les larsens : 
    params->bands[0].gain_db = -15.0f;
    params->bands[1].gain_db = -10.0f;
    params->bands[2].gain_db = -2.5f;
}

/* Filtre "peaking EQ" (cloche), formule de l'Audio EQ Cookbook (Robert Bristow-Johnson). */
static void FxEq_ComputeBandCoeffs(FxEqBandState *bs, const FxEqBand *band)
{
    float A     = powf(10.0f, band->gain_db / 40.0f);
    float w0    = 2.0f * (float)M_PI * band->freq_hz / FX_EQ_SAMPLE_RATE;
    float alpha = sinf(w0) / (2.0f * band->q);
    float cosw0 = cosf(w0);

    float a0 = 1.0f + alpha / A;
    bs->b0 = (1.0f + alpha * A) / a0;
    bs->b1 = (-2.0f * cosw0) / a0;
    bs->b2 = (1.0f - alpha * A) / a0;
    bs->a1 = (-2.0f * cosw0) / a0;
    bs->a2 = (1.0f - alpha / A) / a0;

    bs->last_freq_hz = band->freq_hz;
    bs->last_gain_db = band->gain_db;
    bs->last_q       = band->q;
}

float FxEqualizer_Process(FxEqualizerState *state, const FxEqualizerParams *params, float input)
{
    if (!params->enabled)
    {
        return input;
    }

    float y = input;
    for (uint32_t i = 0; i < FX_EQ_NUM_BANDS; i++)
    {
        const FxEqBand *band = &params->bands[i];
        FxEqBandState  *bs   = &state->band_state[i];

        if (band->freq_hz != bs->last_freq_hz || band->gain_db != bs->last_gain_db || band->q != bs->last_q)
        {
            FxEq_ComputeBandCoeffs(bs, band);
        }

        float x = y;
        y = bs->b0 * x + bs->b1 * state->x1[i] + bs->b2 * state->x2[i]
                        - bs->a1 * state->y1[i] - bs->a2 * state->y2[i];

        state->x2[i] = state->x1[i];
        state->x1[i] = x;
        state->y2[i] = state->y1[i];
        state->y1[i] = y;
    }

    return y;
}