#include <plib.h>
#include <DrvLib.h>
#include <math.h>
#include <FnFFTLib.h>

extern float W25k;
extern float gFactor;

//prepare each 256 arrays for sin & cos table
//FnFFTInit(&con,&sin);
//FnFFT512(&src,&con,&sin);

void FnFFTInit(float *CNT, float *SNT)
{
	int	i;
	float CN0,SN0;

	CN0 = cos(PIE*2/FFT_N);
	SN0 = -sin(PIE*2/FFT_N);
	*CNT = 1;
	*SNT = 0;
	
	for(i=1;i<FFT_NN;i++)
	{
		*(CNT+i) = CN0*(*(CNT+i-1)) - SN0*(*(SNT+i-1));
		*(SNT+i) = SN0*(*(CNT+i-1)) + CN0*(*(SNT+i-1));
	}	
	_DrvUART2MsgStr("\r\nFFT-Table Initialize Ready \r\n");	
}

FFT_ReturnData FnFFT512(float *src, float *cnt, float *snt)
{
	int i;
	unsigned int retMag;
	long radix2;
	float *rsrc, *isrc;
	float retFreq, realBuf[512], imgBuf[512], temp1, temp2;
	FFT_ReturnData fftRet;

	for(i=0;i<512;i++)	
	{
		realBuf[i] = *(src+i);
		imgBuf[i] = 0;
	}
	rsrc = realBuf;
	isrc = imgBuf;
	//
	radix2 = 9;
	FnBrev(radix2, rsrc);	
	FnFFTInplace(rsrc,isrc,cnt,snt);
	FnFFTSum(rsrc, isrc, (512/2));	//save frequency data into "rsrc"
	
/*
	retFreq = FnFFTMax(rsrc, (125/2-1));	//calculate maximum frequency position
	retMag = rsrc[(int)retFreq];
	retFreq *= 48.828125;
	
	fftRet.freq = retFreq;
	fftRet.mag = retMag;
*/
	return fftRet;
}

FFT_ReturnData FnDFT25K(float *src, int cenFreq, int tsize, int rng)
{
	unsigned int retMag;
	int maxAdr,minRang,maxRang,i,j;
	unsigned int _frVal, _fiVal;
	float _rVal, _iVal;
	float fCos, fSin, Wn;
	float frBuf[rng*2+1];
	FFT_ReturnData fftRet;
	//float temp1, temp2;
	
	maxAdr = cenFreq;
	minRang = maxAdr-rng;	
	maxRang = maxAdr+rng;

	for(i=minRang;i<(maxRang+1);i++)
	{
		_rVal = 0;
		_iVal = 0;
		for(j=0;j<tsize;j++)
		{	
			Wn = W25k * i *j;

			fCos = cosf(Wn);
			fSin = sinf(Wn);

			_rVal += ((*(src+j))*fCos);
			_iVal += ((*(src+j))*fSin);
		}

		//accumulate real and image value
		_rVal *= gFactor;
		_iVal *= gFactor;
		_frVal = (unsigned int)(powf(_rVal,2.0));
		_fiVal = (unsigned int)(powf(_iVal,2.0));
		frBuf[i-minRang] = (float)(_frVal + _fiVal);
	}

	#if 0 //0323,modify
	temp1 = frBuf[0];
	for(i=1;i<(rng*2+1);i++)
	{
		temp2 = frBuf[i]-temp1;
		temp2 += (frBuf[i]-frBuf[i+1]);
		temp1 = frBuf[i];
		frBuf[i] = temp2;
		//powf(*(rsrc+i),2);
	}
	frBuf[0] = frBuf[1]+(frBuf[1]-frBuf[2]);
	#endif

	//search max value on frequency domain	
	maxAdr = FnFFTMax(frBuf, (rng*2)+0);
	retMag = frBuf[maxAdr];
	cenFreq+=(maxAdr-rng);

	fftRet.freq = cenFreq;
	fftRet.mag = retMag;

	return fftRet;
}

void FnBrev(long m, float *src)
{
	long n,j,k;
	long i,i1,i2;
	long l,l1,l2;
	long tx;

	/* Calculate the number of points */
	n = 1;
	for (i=0;i<m;i++) 
		n *= 2;

	/* Do the bit reversal */
	i2 = n >> 1;
	j = 0;
	for (i=0;i<n-1;i++)
	{
		if (i < j)
		{
			tx = *(src+i);
			*(src+i) = *(src+j);
			*(src+j) = tx;
		}
		k = i2;
		while (k <= j)
		{
			j -= k;
			k >>= 1;
		}
		j += k;
	}
}

void FnFFTInplace(float *fxr, float *fxi, float *CNT, float *SNT)
{
	unsigned int i,j,k,m,N1,N2,index;
	float tempr, tempi;

	for(i=0;i<log2N;i++)
	{
		N1 = 1 << (i+1);
		N2 = N1 >> 1;
		for(j=0;j<N2;j++)
		{
			index = j*(1<<(log2N-i-1));
			//
			for(k=j;k<FFT_N;k+=N1)
			{
				m = k+N2;
				tempr = (*(fxr+m)) * (*(CNT+index)) - (*(fxi+m)) * (*(SNT+index));				
				tempi = (*(fxi+m)) * (*(CNT+index)) + (*(fxr+m)) * (*(SNT+index));				
				
				*(fxr+m) = *(fxr+k) - tempr;
				*(fxi+m) = *(fxi+k) - tempi;
				*(fxr+k) = *(fxr+k) + tempr;
				*(fxi+k) = *(fxi+k) + tempi;				
			}
		}
	}
}

int FnFFTMax(float *fSrc, int fftSize)
{
	float maxVal;
	int maxAdr, i;

	//search max value on frequency domain	
	maxVal = *(fSrc+1);
	maxAdr = (int)(fSrc+1);

	for(i=2;i<fftSize;i++){
		if(*(fSrc+i) > maxVal){
			maxVal = *(fSrc+i);
			maxAdr = (int)(fSrc+i);
		}
	}	
	maxAdr -=((int)(fSrc));
	return (maxAdr/4);
}

void FnFFTSum(float *rSrc, float *iSrc, int fftSize)
{
	int i;
	float rval, ival;
	
	*rSrc = 0;
	for(i=1;i<fftSize;i++)
	{
		rval = fabs(*(rSrc+i));
		ival = fabs(*(iSrc+i));
		*(rSrc+i) = rval+ival;
	}
}