#include "system/task_fft.h"
#include "system/data_channel.h"
#include "system/Complex.h"
#include "system/float_word.h"
#include "math.h"
#include "complex.h"

void cooley_tukey(float complex *X, const uint32_t N) {
    if (N >= 2) {
        float complex tmp [N / 2];
        for (uint32_t i = 0; i < N / 2; ++i) {
            tmp[i] = X[2*i + 1];
            X[i] = X[2*i];
        }
        for (uint32_t i = 0; i < N / 2; ++i) {
            X[i + N / 2] = tmp[i];
        }

        cooley_tukey(X, N / 2);
        cooley_tukey(X + N / 2, N / 2);

        for (uint32_t i = 0; i < N / 2; ++i) {
            X[i + N / 2] = X[i] - cexp(-2.0 * I * M_PI * (float)i / (float)N) * X[i + N / 2];
            X[i] -= (X[i + N / 2]-X[i]);
        }
    }
}



int task_fft_run( void * task ) {

    fft_config * config = (fft_config*)task;

    Complex input[DATA_CHANNEL_DEPTH];

    // Daten lesen
    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
	float a;
	data_channel_read(config->base.sources[0], (uint32_t *)&a);
	input[i] = (Complex){a, 0}; 
    }

    // FFT berechnen
    cooley_tukey(input, DATA_CHANNEL_DEPTH);

    // Ergebnisse normalisieren
    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
        input[i].re /= DATA_CHANNEL_DEPTH/2;
        input[i].im /= DATA_CHANNEL_DEPTH/2;
    }
	input[0].re = input[0].re + 0.1403;

    // Ergebnisse in data channel schreiben
    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
	float_word c;
	c.value = complex_abs(&input[i]);
	data_channel_write(config->base.sink, c.word);
    }

    return 0;

}