/* fft.c */

#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <assert.h>
#include "mem.h"
#include "fft.h"

#define FFT_SIGN (-1)

int fft_size = 0;
fft_type *fft_buffer = NULL;
long fft_logN;

void fft_init(int size)
{
	assert(fft_size == 0);
	assert(fft_buffer == NULL);

	/* We use complex pairs, so we need twice the number of elements */
	fft_buffer = safe_malloc(sizeof(fft_type) * size * 2);
	fft_size = size;
	fft_logN = (long)(log(fft_size)/log(2.0)+0.5);
}

void fft_data_float(float *buf)
{
	int i;

	for (i = 0; i < fft_size; i++) {
		fft_buffer[i*2] = buf[i];
		fft_buffer[i*2+1] = 0.0;
	}
}

void fft_data_signed16(int16_t *buf)
{
	int i;

	for (i = 0; i < fft_size; i++) {
		fft_buffer[i*2] = (fft_type)buf[i] / (INT16_MAX+1);
		fft_buffer[i*2+1] = 0.0;
//		printf("input: %f, %f\n", fft_buffer[i*2], fft_buffer[i*2+1]);
	}
}

void fft_data_unsigned16(uint16_t *buf)
{
	int i;

	for (i = 0; i < fft_size; i++) {
		fft_buffer[i*2] = (fft_type)buf[i] / (INT16_MAX+1) -
		    (INT16_MAX+1);
		fft_buffer[i*2+1] = 0.0;
	}
}

fft_type *fft_getresult(void)
{
	return fft_buffer;
}

void fft_window(void)
{
	int i;

	for (i = 0; i < fft_size; i++) {
		fft_buffer[2*i] = fft_buffer[2*i] * (0.5 + 0.5 *
			 cos(M_PI * (i - fft_size/2)/(fft_size/2 + 1)));
	}
}

/*
 * FFT routine
 * Based on routine (C) 1996 S.M.Bernsee.
 */
void fft_compute(void)
{
    fft_type wr, wi, arg, *p1, *p2, temp;
    fft_type tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
    long i, bitm, j, le, le2, k;

    for (i = 2; i < 2*fft_size-2; i += 2) {

        for (bitm = 2, j = 0; bitm < 2*fft_size; bitm <<= 1) {

            if (i & bitm) j++;
            j <<= 1;

        }

        if (i < j) {

            p1 = fft_buffer+i; p2 = fft_buffer+j;
            temp = *p1; *(p1++) = *p2;
            *(p2++) = temp; temp = *p1;
            *p1 = *p2; *p2 = temp;

        }

    }

    for (k = 0, le = 2; k < fft_logN; k++) {

        le <<= 1;
        le2 = le>>1;
        ur = 1.0;
        ui = 0.0;
        arg = M_PI / (le2>>1);
        wr = cos(arg);
        wi = FFT_SIGN*sin(arg);

        for (j = 0; j < le2; j += 2) {

            p1r = fft_buffer+j; p1i = p1r+1;
            p2r = p1r+le2; p2i = p2r+1;

            for (i = j; i < 2*fft_size; i += le) {

                tr = *p2r * ur - *p2i * ui;
                ti = *p2r * ui + *p2i * ur;
                *p2r = *p1r - tr; *p2i = *p1i - ti;
                *p1r += tr; *p1i += ti;
                p1r += le; p1i += le;
                p2r += le; p2i += le;

            }

            tr = ur*wr - ui*wi;
            ui = ur*wi + ui*wr;
            ur = tr;

        }

    }

}