#include <stddef.h>
#include <stdint.h>
#include <math.h>
 
#include "system/task_fft.h"
#include "system/data_channel.h"
#include "system/Complex.h"
#include "system/float_word.h"
 
static void bit_reverse(Complex *x, size_t n);
static void fft_inplace(Complex *x, size_t n);
 
#define FFT_SIZE 1024
 
int task_fft_run(void *task)
{
    fft_config *config = (fft_config *)task;
 
    Complex buffer[FFT_SIZE];
 
    // 1) 1024 Werte aus dem Eingangskanal lesen
    for (size_t i = 0; i < FFT_SIZE; ++i)
    {
        float sample = 0.0f;
        data_channel_read(config->base.sources[0], (uint32_t *)&sample);
 
        buffer[i].re = sample;
        buffer[i].im = 0.0f;
    }
 
    // 2) FFT berechnen
    fft_inplace(buffer, FFT_SIZE);
 
    // 3) Magnitude berechnen und skalieren
    for (size_t i = 0; i < FFT_SIZE; ++i)
    {
        float_word w;
        float mag = sqrtf(buffer[i].re * buffer[i].re + buffer[i].im * buffer[i].im);
 
        if (i == 0)
            mag *= 1.0f / FFT_SIZE;           // DC-Komponente
        else
            mag *= 2.0f / FFT_SIZE;           // alle anderen Frequenzen
 
        w.value = mag;
        data_channel_write(config->base.sink, w.word);
    }
 
    return 0;
}
 
// --------------------------------------------------
// Hilfsfunktionen: Bit-Reversal + FFT
// --------------------------------------------------
static void bit_reverse(Complex *x, size_t n)
{
    size_t j = 0;
    for (size_t i = 0; i < n; ++i)
    {
        if (i < j)
        {
            Complex tmp = x[i];
            x[i] = x[j];
            x[j] = tmp;
        }
        size_t bit = n >> 1;
        while (bit & j)
        {
            j ^= bit;
            bit >>= 1;
        }
        j |= bit;
    }
}
 
static void fft_inplace(Complex *x, size_t n)
{
    bit_reverse(x, n);
 
    for (size_t len = 2; len <= n; len <<= 1)
    {
        float ang = -2.0f * (float)M_PI / (float)len;
        float wlen_re = cosf(ang);
        float wlen_im = sinf(ang);
 
        for (size_t i = 0; i < n; i += len)
        {
            float w_re = 1.0f;
            float w_im = 0.0f;
 
            for (size_t j = 0; j < (len >> 1); ++j)
            {
                Complex u = x[i + j];
 
                Complex v;
                v.re = w_re * x[i + j + (len >> 1)].re - w_im * x[i + j + (len >> 1)].im;
                v.im = w_re * x[i + j + (len >> 1)].im + w_im * x[i + j + (len >> 1)].re;
 
                x[i + j].re              = u.re + v.re;
                x[i + j].im              = u.im + v.im;
                x[i + j + (len >> 1)].re = u.re - v.re;
                x[i + j + (len >> 1)].im = u.im - v.im;
 
                float tmp_re = w_re * wlen_re - w_im * wlen_im;
                float tmp_im = w_re * wlen_im + w_im * wlen_re;
                w_re = tmp_re;
                w_im = tmp_im;
            }
        }
    }
}