#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 in data channel schreiben
    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {

	float_word c;
	c.value = complex_abs(&input[i]);
	c.value = sqrt( pow( input[i].re, 2 ) + pow( input[i].im, 2 ) );

	if (i > (uint32_t)0)
	{	
		c.value = c.value * (2.0/DATA_CHANNEL_DEPTH);
	}
	else
	{
		c.value = c.value * (1.0/DATA_CHANNEL_DEPTH);
	}


	data_channel_write(config->base.sink, c.word);
    }

    return 0;

}

