#include <stdlib.h>
#include <math.h>
#include <iomanip>
#include <fstream>

#include <tchar.h>
#include "FFT.h"





FFT::FFT()
{

	N = 0;

}


FFT::FFT(int _N)
{

	//factor out powers of two
	N = 0;

	while(_N >>= 1)
		N++;

	L = (int)(1>>N);

	W = new __int64[L/2];
	iW = new __int64[L/2];
	BR = new int[L];
	BRX = new CPX[L];

	initW();
	initBR();
}


FFT::~FFT()
{

	delete [] W;
	delete [] iW;
	delete [] BR;
	delete [] BRX;

}

void FFT::initW()
{

	int lcv;
	short *p;
	double s;
	double c;
	double phase;

    const double pi=3.14159265358979323846264338327;

	for(lcv = 0; lcv < L/2; lcv++)
	{
		//Forward twiddles
		p =     (short *)&W[lcv];
		phase = (-2*pi*lcv)/L;
		c =		cos(phase);
		s =		sin(phase);
		p[0] = (short)(c*32767);
		p[1] = (short)(-s*32767);
		p[2] = (short)(s*32767);
		p[3] = (short)(c*32767);

		//Reverse twiddles
		p =     (short *)&iW[lcv];
		phase = (2*pi*lcv)/L;
		c =		cos(phase);
		s =		sin(phase);
		p[0] = (short)(c*32767);
		p[1] = (short)(-s*32767);
		p[2] = (short)(s*32767);
		p[3] = (short)(c*32767);
	}

}




void FFT::initBR()
{
	int lcv, lcv2, index;

	for(lcv = 0; lcv < L; lcv++)
	{
		index = 0;
		for(lcv2 = 0; lcv2 < N; lcv2++)
		{
			index += ((lcv >> lcv2) & 0x1);
			index <<= 1;
		}
		index >>= 1;

		BR[lcv] = index;
	}

}

void FFT::doFFT(CPX *_x, bool _shuf)
{

	int lcv, lcv2, lcv3;
	int bsize, nblocks;
	CPX *a, *b;
	__int64 *w;

	if(_shuf)
		doShuffle(_x);	//bit reverse the array

	bsize = 1;
	nblocks = L >> 1;

	for(lcv = 0; lcv < N; lcv++)					//Loop over N stages
	{

		a = _x;
		b = _x+bsize;
		w = W;

		for(lcv2 = 0; lcv2 < nblocks; lcv2++)		//Loop over blocks
		{

	        for(lcv3 = 0; lcv3 < bsize; lcv3++)		//Butterflies within block
			{
				bfly(a,b,w);
				w += nblocks;
				a++; b++;
			}
			a += bsize;
			b += bsize;
			w = W;
		}

		bsize <<= 1;
		nblocks >>= 1;

	}

};


void FFT::doiFFT(CPX *_x, bool _shuf)
{

	int lcv, lcv2, lcv3;
	CPX *a, *b;
	__int64 *w;

	int bsize, nblocks;

	if(_shuf)
		doShuffle(_x);	//bit reverse the array




	bsize = 1;
	nblocks = L >> 1;

	for(lcv = 0; lcv < N; lcv++)					//Loop over N stages
	{

		a = _x;
		b = _x+bsize;
		w = iW;

		for(lcv2 = 0; lcv2 < nblocks; lcv2++)		//Loop over blocks
		{

	        for(lcv3 = 0; lcv3 < bsize; lcv3++)		//Butterflies within block
			{
				bfly(a,b,w);
				w += nblocks;
				a++; b++;
			}
			a += bsize;
			b += bsize;
			w = iW;
		}

		bsize <<= 1;
		nblocks >>= 1;

	}

}


void FFT::doFFTdf(CPX *_x, bool _shuf)
{


	int lcv, lcv2, lcv3;
	int bsize, nblocks;
	CPX *a, *b;
	__int64 *w;

	bsize = L >> 1;
	nblocks = 1;

	for(lcv = 0; lcv < N; lcv++)					//Loop over N stages
	{

		a = _x;
		b = _x+bsize;
		w = W;

		for(lcv2 = 0; lcv2 < nblocks; lcv2++)		//Loop over blocks
		{
	        for(lcv3 = 0; lcv3 < bsize; lcv3++)		//Butterflies within block
			{
				bflydf(a,b,w);
				w += nblocks;
				a++; b++;
			}
			a += bsize;
			b += bsize;
			w = W;
		}

		bsize >>= 1;
		nblocks <<= 1;

	}

	if(_shuf)
		doShuffle(_x);	//bit reverse the array


}


void FFT::doiFFTdf(CPX *_x, bool _shuf)
{


	int lcv, lcv2, lcv3;
	int bsize, nblocks;
	CPX *a, *b;
	__int64 *w;

	bsize = L >> 1;
	nblocks = 1;

	for(lcv = 0; lcv < N; lcv++)					//Loop over N stages
	{

		a = _x;
		b = _x+bsize;
		w = iW;

		for(lcv2 = 0; lcv2 < nblocks; lcv2++)		//Loop over blocks
		{
	        for(lcv3 = 0; lcv3 < bsize; lcv3++)		//Butterflies within block
			{
				bflydf(a,b,w);
				w += nblocks;
				a++; b++;
			}
			a += bsize;
			b += bsize;
			w = W;
		}

		bsize >>= 1;
		nblocks <<= 1;

	}

	if(_shuf)
		doShuffle(_x);	//bit reverse the array


}

void FFT::doShuffle(CPX *_x)
{

	int lcv;

	memcpy(BRX, _x, L*sizeof(CPX));

	for(lcv = 0; lcv < L; lcv++)
		_x[lcv] = BRX[BR[lcv]];

}