#include "fx_octaver.h"
#include <math.h>

#define FX_OCTAVER_BUFFER_MASK    (FX_OCTAVER_BUFFER_SIZE - 1U)
#define FX_OCTAVER_ENV_TABLE_SIZE 256U

/* Rapport de vitesse de lecture pour chaque voix : lire le buffer plus vite que
 * l'ecriture multiplie la frequence (octave au-dessus), plus lentement la divise
 * (octave en-dessous). */
static const float fx_octaver_ratios[FX_OCTAVER_NUM_VOICES] =
{
    0.25f, /* -2 octaves */
    0.5f,  /* -1 octave  */
    2.0f,  /* +1 octave  */
    4.0f   /* +2 octaves */
};

/* Fenetre de Hann tabulee (evite un cosf() par grain et par echantillon). */
static float fx_octaver_env_table[FX_OCTAVER_ENV_TABLE_SIZE + 1U];
static bool  fx_octaver_env_table_ready = false;

static void FxOctaver_BuildEnvTable(void)
{
    if (fx_octaver_env_table_ready)
    {
        return;
    }
    for (uint32_t i = 0; i <= FX_OCTAVER_ENV_TABLE_SIZE; i++)
    {
        float phase = (float)i / (float)FX_OCTAVER_ENV_TABLE_SIZE;
        fx_octaver_env_table[i] = 0.5f - 0.5f * cosf(2.0f * (float)M_PI * phase);
    }
    fx_octaver_env_table_ready = true;
}

static inline float FxOctaver_EnvLookup(float phase)
{
    float pos = phase * (float)FX_OCTAVER_ENV_TABLE_SIZE;
    uint32_t i0 = (uint32_t)pos;
    if (i0 >= FX_OCTAVER_ENV_TABLE_SIZE)
    {
        i0 = FX_OCTAVER_ENV_TABLE_SIZE - 1U;
    }
    float frac = pos - (float)i0;
    return fx_octaver_env_table[i0] * (1.0f - frac) + fx_octaver_env_table[i0 + 1U] * frac;
}

static inline float FxOctaver_ReadInterpolated(const float *buffer, uint32_t write_idx, float delay)
{
    /* write_idx grandit sans jamais etre replie (compteur perpetuel) ; le convertir
     * directement en float finirait par perdre en precision (un float ne represente
     * exactement les entiers que jusqu'a ~16.7 millions, soit quelques minutes a
     * 48kHz). On calcule donc la position de lecture uniquement a partir de l'index
     * DEJA REPLIE dans le buffer, qui reste toujours petit. */
    float write_pos = (float)(write_idx & FX_OCTAVER_BUFFER_MASK);
    float read_pos = write_pos - delay;
    while (read_pos < 0.0f)
    {
        read_pos += (float)FX_OCTAVER_BUFFER_SIZE;
    }
    uint32_t i0 = (uint32_t)read_pos & FX_OCTAVER_BUFFER_MASK;
    uint32_t i1 = (i0 + 1U) & FX_OCTAVER_BUFFER_MASK;
    float frac = read_pos - (float)(uint32_t)read_pos;
    return buffer[i0] * (1.0f - frac) + buffer[i1] * frac;
}

/* Lecteur de grains a deux pointeurs en fondu croise : chaque pointeur avance
 * (ou recule) dans le buffer a une vitesse "ratio" differente de celle de
 * l'ecriture, ce qui change la hauteur percue. Les deux grains sont toujours
 * en opposition de phase exacte (decales d'un demi-grain a l'origine, et le
 * meme increment est applique aux deux avant chaque repli) : la fenetre de
 * Hann du second grain vaut donc toujours 1 moins celle du premier, ce qui
 * garantit un niveau de sortie constant pendant le fondu et evite un second
 * calcul de fenetre. */
static float FxOctaver_ProcessVoice(FxOctaverVoiceState *vs, const float *buffer, uint32_t write_idx, float ratio)
{
    float step = 1.0f - ratio;

    for (uint32_t g = 0; g < 2U; g++)
    {
        vs->delay[g] += step;
        if (vs->delay[g] >= (float)FX_OCTAVER_GRAIN_SIZE)
        {
            vs->delay[g] -= (float)FX_OCTAVER_GRAIN_SIZE;
        }
        else if (vs->delay[g] < 0.0f)
        {
            vs->delay[g] += (float)FX_OCTAVER_GRAIN_SIZE;
        }
    }

    float phase0 = vs->delay[0] / (float)FX_OCTAVER_GRAIN_SIZE;
    float env0 = FxOctaver_EnvLookup(phase0);
    float env1 = 1.0f - env0;

    float sample0 = FxOctaver_ReadInterpolated(buffer, write_idx, vs->delay[0]);
    float sample1 = FxOctaver_ReadInterpolated(buffer, write_idx, vs->delay[1]);

    return env0 * sample0 + env1 * sample1;
}

void FxOctaver_Init(FxOctaverState *state, FxOctaverParams *params)
{
    params->enabled     = false;
    params->level_down2 = 0.0f;
    params->level_down1 = 0.0f;
    params->level_dry   = 1.0f;
    params->level_up1   = 0.0f;
    params->level_up2   = 0.0f;

    for (uint32_t i = 0; i < FX_OCTAVER_BUFFER_SIZE; i++)
    {
        state->buffer[i] = 0.0f;
    }
    state->write_idx = 0U;

    for (uint32_t v = 0; v < FX_OCTAVER_NUM_VOICES; v++)
    {
        state->voices[v].delay[0] = 0.0f;
        state->voices[v].delay[1] = (float)FX_OCTAVER_GRAIN_SIZE * 0.5f;
    }

    FxOctaver_BuildEnvTable();
}

float FxOctaver_Process(FxOctaverState *state, const FxOctaverParams *params, float input)
{
    if (!params->enabled)
    {
        return input;
    }

    state->buffer[state->write_idx & FX_OCTAVER_BUFFER_MASK] = input;

    float down2 = FxOctaver_ProcessVoice(&state->voices[0], state->buffer, state->write_idx, fx_octaver_ratios[0]);
    float down1 = FxOctaver_ProcessVoice(&state->voices[1], state->buffer, state->write_idx, fx_octaver_ratios[1]);
    float up1   = FxOctaver_ProcessVoice(&state->voices[2], state->buffer, state->write_idx, fx_octaver_ratios[2]);
    float up2   = FxOctaver_ProcessVoice(&state->voices[3], state->buffer, state->write_idx, fx_octaver_ratios[3]);

    state->write_idx++;

    return input * params->level_dry
         + down2 * params->level_down2
         + down1 * params->level_down1
         + up1   * params->level_up1
         + up2   * params->level_up2;
}