#include "audio_analysis.h"
#include <math.h>
#include <string.h>

#define FFT_SIZE AUDIO_ANALYSIS_FFT_SIZE
#define NUM_BINS AUDIO_ANALYSIS_NUM_BINS

/* --- Capture en double buffer (ping-pong), ecrite depuis le chemin audio temps
 * reel, lue depuis la tache IHM. Un seul ecrivain, un seul lecteur : pas de verrou
 * necessaire, meme principe que la FIFO audio principale. --- */
static float    g_capture_buf[2][FFT_SIZE];
static uint32_t g_capture_fill_idx = 0;
static uint8_t  g_capture_write_buf = 0;
static volatile uint8_t g_capture_ready_buf = 0xFFU; /* 0xFF = aucun buffer complet en attente */

static bool g_analysis_enabled = false;

/* --- Tables precalculees (a l'init uniquement, jamais recalculees en boucle) --- */
static float g_hann_window[FFT_SIZE];
static float g_twiddle_cos[FFT_SIZE / 2U];
static float g_twiddle_sin[FFT_SIZE / 2U];

/* --- Buffers de travail pour la FFT (reels/imaginaires en place) et resultat --- */
static float g_fft_real[FFT_SIZE];
static float g_fft_imag[FFT_SIZE];
static float g_magnitudes_db[NUM_BINS]; /* seule la moitie 0..Fe/2 est conservee */
static float g_peak_freq_hz = 0.0f;

static void AudioAnalysis_BuildTables(void)
{
    for (uint32_t i = 0; i < FFT_SIZE; i++)
    {
        /* Fenetre de Hann : reduit les fuites spectrales dues a la troncature du bloc. */
        g_hann_window[i] = 0.5f - 0.5f * cosf(2.0f * (float)M_PI * (float)i / (float)(FFT_SIZE - 1U));
    }
    for (uint32_t i = 0; i < FFT_SIZE / 2U; i++)
    {
        float angle = -2.0f * (float)M_PI * (float)i / (float)FFT_SIZE;
        g_twiddle_cos[i] = cosf(angle);
        g_twiddle_sin[i] = sinf(angle);
    }
}

/* Permutation en bit-reverse, etape preliminaire classique d'une FFT radix-2
 * "decimation in time" calculee en place. */
static void AudioAnalysis_BitReverse(float *real, float *imag)
{
    uint32_t j = 0;
    for (uint32_t i = 0; i < FFT_SIZE - 1U; i++)
    {
        if (i < j)
        {
            float tmp = real[i]; real[i] = real[j]; real[j] = tmp;
            tmp = imag[i]; imag[i] = imag[j]; imag[j] = tmp;
        }
        uint32_t k = FFT_SIZE >> 1U;
        while (k <= j)
        {
            j -= k;
            k >>= 1U;
        }
        j += k;
    }
}

/* FFT radix-2 iterative (Cooley-Tukey), en place, a partir des tables de twiddle
 * precalculees. Complexite O(N log N), largement dans les capacites d'un Cortex-M7
 * a FPU materielle pour N=1024, execute hors du chemin audio temps reel. */
static void AudioAnalysis_ComputeFFT(float *real, float *imag)
{
    AudioAnalysis_BitReverse(real, imag);

    for (uint32_t size = 2U; size <= FFT_SIZE; size <<= 1U)
    {
        uint32_t half = size >> 1U;
        uint32_t table_step = FFT_SIZE / size;
        for (uint32_t i = 0; i < FFT_SIZE; i += size)
        {
            for (uint32_t k = 0; k < half; k++)
            {
                uint32_t twiddle_idx = k * table_step;
                float c = g_twiddle_cos[twiddle_idx];
                float s = g_twiddle_sin[twiddle_idx];
                uint32_t idx1 = i + k;
                uint32_t idx2 = i + k + half;

                float tr = real[idx2] * c - imag[idx2] * s;
                float ti = real[idx2] * s + imag[idx2] * c;

                real[idx2] = real[idx1] - tr;
                imag[idx2] = imag[idx1] - ti;
                real[idx1] += tr;
                imag[idx1] += ti;
            }
        }
    }
}

void AudioAnalysis_Init(void)
{
    memset(g_capture_buf, 0, sizeof(g_capture_buf));
    g_capture_fill_idx = 0;
    g_capture_write_buf = 0;
    g_capture_ready_buf = 0xFFU;
    g_analysis_enabled = false;

    memset(g_magnitudes_db, 0, sizeof(g_magnitudes_db));

    AudioAnalysis_BuildTables();
}

void AudioAnalysis_PushSample(float sample)
{
    if (!g_analysis_enabled)
    {
        return;
    }

    g_capture_buf[g_capture_write_buf][g_capture_fill_idx] = sample;
    g_capture_fill_idx++;

    if (g_capture_fill_idx >= FFT_SIZE)
    {
        g_capture_fill_idx = 0;
        g_capture_ready_buf = g_capture_write_buf;
        g_capture_write_buf = 1U - g_capture_write_buf;
    }
}

bool AudioAnalysis_Update(void)
{
    if (!g_analysis_enabled)
    {
        return false;
    }

    uint8_t ready = g_capture_ready_buf;
    if (ready == 0xFFU)
    {
        return false;
    }
    g_capture_ready_buf = 0xFFU;

    for (uint32_t i = 0; i < FFT_SIZE; i++)
    {
        g_fft_real[i] = g_capture_buf[ready][i] * g_hann_window[i];
        g_fft_imag[i] = 0.0f;
    }

    AudioAnalysis_ComputeFFT(g_fft_real, g_fft_imag);

    /* Magnitude normalisee (une sinusoide plein-echelle donne ~0dB) puis convertie
     * en dB, avec un plancher pour eviter log(0). Destine a un affichage visuel,
     * pas a une mesure calibree. Seule la moitie 0..Fe/2 du spectre est conservee :
     * le spectre d'un signal reel est symetrique, la seconde moitie n'apporte rien. */
    /* Recherche du pic en meme temps que le calcul des magnitudes. Le bin 0 (continu,
     * 0Hz) est ignore : une composante DC ne doit pas etre rapportee comme "frequence
     * dominante". */
    uint32_t peak_bin = 1U;
    float peak_mag = 0.0f;

    for (uint32_t i = 0; i < NUM_BINS; i++)
    {
        float re = g_fft_real[i];
        float im = g_fft_imag[i];
        float mag = sqrtf(re * re + im * im) / ((float)FFT_SIZE / 4.0f);

        if (i >= 1U && mag > peak_mag)
        {
            peak_mag = mag;
            peak_bin = i;
        }

        if (mag < 1.0e-6f)
        {
            mag = 1.0e-6f;
        }
        g_magnitudes_db[i] = 20.0f * log10f(mag);
    }

    g_peak_freq_hz = (float)peak_bin * (float)AUDIO_ANALYSIS_SAMPLE_RATE_HZ / (float)FFT_SIZE;

    return true;
}

const float *AudioAnalysis_GetMagnitudesDb(void)
{
    return g_magnitudes_db;
}

float AudioAnalysis_GetPeakFrequencyHz(void)
{
    return g_peak_freq_hz;
}

void AudioAnalysis_SetEnabled(bool enabled)
{
    if (enabled && !g_analysis_enabled)
    {
        /* Redemarre une capture propre a chaque activation. */
        g_capture_fill_idx = 0;
        g_capture_write_buf = 0;
        g_capture_ready_buf = 0xFFU;
    }
    g_analysis_enabled = enabled;
}

bool AudioAnalysis_IsEnabled(void)
{
    return g_analysis_enabled;
}