#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <complex.h>

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

/* Permutation */
uint16_t fft512_p[512];

void fft512_init() {
    /* Initialize permutation table */
    for (uint16_t i = 0; i < 512; i++) {
        uint16_t p = 0;
        for (int j = 0; j < 9; j++) {
            p |= ((i >> j) & 1) << (8 - j);
        }
        fft512_p[i] = p;
    }
}

void fft512_do(float* in, float complex* out) {
    /* Reorder inputs */
    for (int i = 0; i < 512; i++)
        out[i] = in[fft512_p[i]];

    /* log2(n) iterations. */
    for (int i = 1; i < 10; i++) {
        int n  = 1 << i; /* elements per segement */
        int k  = 1 << (8-i+1); /* number of segments */
        int nh = n >> 1; /* n halfed */
        
        /* Debug */
        printf("n=%d nh=%d k=%d (%d)\n", n, nh, k, n*k);
        
        /* Iterate the segments */
        for (int j = 0; j < k; j++) {
            int l = j * n;
            int r = l + nh;

            /* Combine the two halfes of the segement. */
            for (int m = 0; m < nh; m++) {
                float x = M_PI * m / (float)nh;
                float complex e = CMPLXF(cos(x), -sin(x));
                float complex g = out[l+m];
                float complex u = out[r+m];

                out[l+m] = g + u * e;
                out[r+m] = g - u * e;
            }
        }
    }
}

void dft512_do(float* in, float complex* out) {
    for (int i = 0; i < 512; i++) {
        out[i] = 0;
        for (int j = 0; j < 512; j++) {
            float x = (2 * M_PI * i * j) / (float)512;
            float complex e = CMPLXF(cos(x), -sin(x));

            out[i] += in[j] * e;
        }
    }
}

int main(void) {
    float in[512];
    float complex out[2][512];
    
    fft512_init();

    /* Initialize input vector */
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 128; j++) {
            if ((i % 2) == 0) {
                in[i*128+j] = 1.0;
            } else {
                in[i*128+j] = 0.0;
            }
        }
    }

    fft512_do(in, out[0]);
    dft512_do(in, out[1]);
    
    for (int i = 0; i < 512; i++) {
        printf("%f + i%f\t - \t%f + i%f\n",
            creal(out[0][i]), cimag(out[0][i]),
            creal(out[1][i]), cimag(out[1][i])
        );
    }
    puts("");
    
    return 0;
}