#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>
#include <stdio.h>

void fft_radix4(complex float *x) {
    int n = DATA_CHANNEL_DEPTH;
    int stages = log(n) / log(4);  // Anzahl der FFT-Stufen

    // Bit-Reversal-Rearrangement (Umordnung der Daten für FFT)
    for (int i = 0; i < n; i++) {
        int rev = 0, num = i;
        for (int bit = 0; bit < stages; bit++) {
            rev = rev * 4 + (num % 4);
	    //printf("i: %d, rev: %d\n", i, rev);
            num /= 4;
        }
        if (i < rev) {
            complex float temp = x[i];
            x[i] = x[rev];
            x[rev] = temp;
        }
    }

    // Radix-4 Butterfly-Berechnung
    for (int s = 1; s <= stages; s++) {
        int m = pow(4, s);      // Gruppengröße (4^s)
        int quarter_m = m / 4;  // Viertel der Gruppengröße
        float theta = -2.0f * M_PI / m;  // Grundwinkel der Wurzeln der Einheit
	//printf("Stage: %d, m: %d, theta: %f\n", s, m, theta);

        for (int k = 0; k < n; k += m) {  // Iteration über Gruppen
            for (int j = 0; j < quarter_m; j++) {  // Innerhalb der Gruppe
                // Wurzeln der Einheit
                complex float w0 = 1.0f;  // Wurzel für j = 0
                complex float w1 = cexpf(I * theta * j);      // Wurzel für j = 1
                complex float w2 = cexpf(I * theta * 2 * j);  // Wurzel für j = 2
                complex float w3 = cexpf(I * theta * 3 * j);  // Wurzel für j = 3

                // Lade die Werte aus der Gruppe
                complex float t0 = x[k + j];
                complex float t1 = x[k + j + quarter_m] * w1;
                complex float t2 = x[k + j + 2 * quarter_m] * w2;
                complex float t3 = x[k + j + 3 * quarter_m] * w3;
		//printf("w1: %f + %fi, w2: %f + %fi, w3: %f + %fi\n", crealf(w1), cimagf(w1), crealf(w2), cimagf(w2), crealf(w3), cimagf(w3));

		//printf("Before: t0: %f + %fi, t1: %f + %fi, t2: %f + %fi, t3: %f + %fi\n", crealf(t0), cimagf(t0), crealf(t1), cimagf(t1), crealf(t2), cimagf(t2), crealf(t3), cimagf(t3));
                // Butterfly-Operationen
                x[k + j] = t0 + t1 + t2 + t3;
                x[k + j + quarter_m] = t0 - t1 + I * (t3 - t2);
                x[k + j + 2 * quarter_m] = t0 - t2 + t1 - t3;
                x[k + j + 3 * quarter_m] = t0 - t1 - I * (t3 - t2);
		//printf("After: x[%d]: %f + %fi, x[%d]: %f + %fi\n", k + j, crealf(x[k + j]), cimagf(x[k + j]), k + j + quarter_m, crealf(x[k + j + quarter_m]), cimagf(x[k + j + quarter_m]));
            }
        }
    }
}



int task_fft_run(void *task) {

    fft_config *config = (fft_config *)task;
    complex float x[DATA_CHANNEL_DEPTH];
    float c[DATA_CHANNEL_DEPTH];
    
    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
        float a;
        data_channel_read(config->base.sources[0], (uint32_t *) &a);
        x[i] = a;
	//printf("Input x[%d] = %f + %fi\n", i, crealf(x[i]), cimagf(x[i]));
    }

    fft_radix4(x);

    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
	//printf("Output complex x[%d] = %f + %fi\n", i, crealf(x[i]), cimagf(x[i]));
	c[i] = sqrt(pow(crealf(x[i]), 2) + pow(cimagf(x[i]), 2));	// Betrag
	if (i == 0)
	    c[i] = c[i] * 1/DATA_CHANNEL_DEPTH;	// Sklaierung
	else
	   c[i] = c[i] * 2/DATA_CHANNEL_DEPTH;	// Sklaierung
	printf("Output Magnitude skaliert c[%d] = %f\n", i, c [i]);

	float_word output;
	output.value = c[i];	

        data_channel_write(config->base.sink, output.word);
    }
   
    return 0;
}