/****************************************************
* FFT4.c
* implements the functions defined in FFT.h
*
* by Cliff Zhou
* on Apr 12, 2007
* in Innofidei, Inc. Beijing
*
* Revision:
*
*
*(C) 2006-2010, Innofidei Corporation, Beijing. All Rights Reserved.
*
****************************************************/


#include <math.h>			//using absolute value in scale_sense
#include <stdio.h>			
#include <stdlib.h>
#include "FFT4.h"
#include "../../common/src/type.h"


/**** DITFFT ***************************************************************
* radix-4 FFT,  processing 4^k data
*
* INPUTS:
*	FFTnum, signed complex data to be transformed, each IQ components at 12bits
*	n,	 FFT length and data length of FFTdata
*	num_ppln, number of pipeline for Cordic, 15 pipeline in transmitter
*
* OUTPUT:
*	FFTnum, the processed data stored in the same buffer as input data in Bit-Reverse order
*
* RETURN: no
*
******************************************************************************/


int DITFFT(scint16* FFTnum, int* scaled, int n, int shift, ubyte num_ppln)
{
	int 		step, i, j;
	scint32 	product1, product2, product3, product4;
	scint32 	sum1,sum2, sum3, sum4;
	scint16		a1, a2, a3, a4;
	
	int stage=0;
	//int shift = 0;		//the first stage, not need shifting
			
	
	sint32 coef_addr;		//phase of twiddle factor from 0 ~ 4095, representing 0 ->pi-> -pi -> 0

	int	fft_stage = 0;			//total fft stages
	int 	tmp = n;			//to get total fft stage
	
	while( tmp > 1)
	{
		tmp = tmp / RADIX;
		fft_stage ++;
	}


	step=n/RADIX;		//step size in each butterfly

	while(step>=1)		//at each stage, outer loop
	{
				/* total scaled bits */
				*scaled +=  shift;
		
		for(j=0;j<n/(step*RADIX);j++)		//For j part of butterfly, middle loop
		{
			
			for(i=0;i<step;i++)		//Inner loop
			{
				/*scaling input data from 16bit to 12bit (effective)*/

				a1 = scaling( FFTnum[i+j*4*step], shift);
				a2 = scaling( FFTnum[i+step+j*4*step], shift);
				a3 = scaling( FFTnum[i+2*step+j*4*step], shift);
				a4 = scaling( FFTnum[i+3*step+j*4*step], shift);
				
				

				/* four point DFT, from 12bits to 14bits. then left shift 4bits as in 20bits to Cordic */
				sum1.real= (a1.real+a2.real+a3.real+a4.real) << SHIFT4 ;
	            sum1.imag= (a1.imag+a2.imag+a3.imag+a4.imag) << SHIFT4 ;
				sum2.real= (a1.real+a2.imag-a3.real-a4.imag) << SHIFT4 ;
				sum2.imag= (a1.imag-a2.real-a3.imag+a4.real) << SHIFT4 ;
	            sum3.real= (a1.real-a2.real+a3.real-a4.real) << SHIFT4 ;
				sum3.imag= (a1.imag-a2.imag+a3.imag-a4.imag) << SHIFT4 ;
	            sum4.real= (a1.real-a2.imag-a3.real+a4.imag) << SHIFT4 ;
				sum4.imag= (a1.imag+a2.real-a3.imag-a4.real) << SHIFT4 ;


								
				//multiplied by twiddle factor except the last stage, with Cordic
				//CORDIC phase is 20bits from -pi to pi
				//data from 15bits to 16bits, inside pipeline using 15+5 = 20bit
				if ( step != 1) {
					coef_addr = coef_addr_gen(i+j*4*step, stage, n);
					product1 = p2r_cordic32(sum1, coef_addr << (P_WIDTH - fft_stage*2), num_ppln) ;	//20bit phase and 20bits data
					
				    	coef_addr = coef_addr_gen(i+step+j*4*step, stage, n);
				    	product2 = p2r_cordic32(sum2,  coef_addr << (P_WIDTH - fft_stage*2), num_ppln);
				    	
				    	coef_addr = coef_addr_gen(i+2*step+j*4*step, stage, n);
				    	product3 = p2r_cordic32(sum3,  coef_addr << (P_WIDTH - fft_stage*2), num_ppln);
				    	
				    	coef_addr = coef_addr_gen(i+3*step+j*4*step, stage, n);
				    	product4 = p2r_cordic32(sum4,  coef_addr <<(P_WIDTH - fft_stage*2), num_ppln);
			    	}
			    	else		
			    	{
			    		product1 = sum1;
			    		product2 = sum2;
			    		product3 = sum3;
			    		product4 = sum4;
			    	}
			    	
			    	/*mem is 16bits*/				
				FFTnum[i+j*4*step].real		= (sint16)(product1.real>>SHIFT4 );
			    	FFTnum[i+step+j*4*step].real	= (sint16)(product2.real>>SHIFT4 );
			    	FFTnum[i+2*step+j*4*step].real	= (sint16)(product3.real>>SHIFT4 );
			    	FFTnum[i+3*step+j*4*step].real	= (sint16)(product4.real>>SHIFT4 );
			    	

				FFTnum[i+j*4*step].imag			= (sint16)(product1.imag>>SHIFT4 );
			    	FFTnum[i+step+j*4*step].imag	= (sint16)(product2.imag>>SHIFT4 );
			    	FFTnum[i+2*step+j*4*step].imag	= (sint16)(product3.imag>>SHIFT4 );
			    	FFTnum[i+3*step+j*4*step].imag	= (sint16)(product4.imag>>SHIFT4 );
			    	
			}
		}
		
		//test the max left shift bits to get the full range of memory
		shift = scale_sense(FFTnum, n);

		/*** to set scaling for each stage manually ****/
		//if (stage < 2) shift = 2;
		//else		shift = 3;

		if ( FFT_DEBUG > 0 )
			printf("\tSHIFT=%d\n", shift);
			
		step=step/RADIX ;
		stage++;		
				
	}
	
	/*bit reverse*/
	reorder(FFTnum, n);
	
	return shift;
}


/**** reorder ***************************************************************
* bit-reverse in radix-4 for processed data before output. 
*
* INPUTS:
*	FFTnum, processed signed complex data , each IQ components at 16bit
*	n,	 FFT length and data length of FFTdata
*
* OUTPUT:
*	FFTnum, the processed data stored in Natural order
*
* RETURN: no
*
******************************************************************************/

void reorder(scint16* FFTnum, int N)
{
	int sn,si,ci,i,j,stage;
	scint16* temp; 			//to store the bit reversed data
    
	sn=N;
	stage=0;

	temp = (scint16 *)malloc(N * sizeof(scint16));

	//get total stage of IFFT
	while(sn>1)
	{
		sn=sn >> 2;		//two bits in radix-4
		stage++;
	}

	for(i=0;i<N;i++)
	{   
		ci=0;
		si=i;
		
		for(j=0;j<stage;j++)
		{
            		ci=(ci << 2) + (si & 0x03);
			si= si >> 2;
		 }

		temp[ci]=FFTnum[i];
	}

	//store data back into memory
	for(i=0;i<N;i++)
	{
		FFTnum[i]=temp[i];
	}
}




/**** scale_sense ***************************************************************
* test the max absolute value for each I and Q in each FFT stage, AND
* determine the shifting bits to reach the signed 12bits
*
* INPUTS:
*	FFTnum, processed signed complex data , each IQ components at 16bit
*	n,	 FFT length and data length of FFTdata
*
* OUTPUT:
*	return the left shift bits to 16bits in each IFFT stage
*
******************************************************************************/
int scale_sense(scint16 * FFTnum, int n)
{
	int i;
	int shift = 0;			
	int max_abs = 0;
	int pos = 0;		//for testing purpose
	
	//test the max component of data at one stage
	for(i=0; i<n; i++)
	{
		if( abs(FFTnum[i].imag) > max_abs )	{max_abs = abs(FFTnum[i].imag); pos = i;}
		if( abs(FFTnum[i].real) > max_abs )	{max_abs = abs(FFTnum[i].real); pos = i;}
	}
	
	//printf("Max = %d @ position = %d\n", max_abs, pos);
	//determine the left shifting bits to reach full range of 16bit memory
	if (max_abs <= D_MAX)
		while( (max_abs = max_abs * 2) < D_MAX ) 	shift--;
	else 
	{
		shift++;
		while( (max_abs = max_abs / 2) > D_MAX ) 	shift++;

	}
	
	//printf("Shift = %d\n", shift);
	
	return shift;
}


/**** coef_addr_gen ***************************************************************
* calculate the twiddle factor phase in 0 ~ 4095 for each address at each stage 
*
* INPUTS:
*	d_addr, data address in memory, unsigned 12bit
*	stage,	 stage number of FFT, from 0 ~ 6
*
* OUTPUT:
*	return the twiddle factor phase from 0 to 4095, unsigned 12bits
*
******************************************************************************/
/****************************
int coef_addr_gen(int d_addr, int stage)
{
	int  cj, quad;
	int address;
	
	int step;			//stage 1's butterfly step size
	

    	
	step = 1;
	cj = (d_addr >> (10-stage*2) ) & 0x03;
	quad = d_addr & (0x0fff >> (stage*2+2));
		
	step = step << (stage*2);
	
	
	address = cj * step * quad;
	
	address = 4096 - address;	//FFT: using negative phase
		
	return address;
	
}
**************************************************************************************/
int coef_addr_gen(int d_addr, int stage, int N)
{
	int  cj, quad;
	int address;
	
	int step;			//stage 1's butterfly step size
	int mask = 0x03;
	int i=1;
	
	int	fft_stage = 0;			//total fft stages
	int 	tmp = N;			//to get total fft stage
	
	while( tmp > 1)
	{
		tmp = tmp / RADIX;
		fft_stage ++;
	}

	for(i=1; i<fft_stage; i++)
		mask = (mask << 2) + 3;

	
    	
	step = 1;
	cj = (d_addr >> (fft_stage*2-2-stage*2) ) & 0x03;
	quad = d_addr & (mask >> (stage*2+2));
		
	step = step << (stage*2);
	
	
	address = cj * step * quad;
	
	address = N - address;	//FFT: using negative phase
		
	return address;
	
}


/**** scaling *****************************************************************
* scale a complex 16bit input data by 'shift' bits, either left(neg) or right(pos) bits. 
*	if shift < 0, left shifting abs(shift) bits, 2^abs(shift)
*	if shift > 0, right shifting shift bits AND rounding to nearest
*	NO overflow control
*
* INPUTS:
*	data, a complex 16bits data, 
*	shift, a signed int for left or right shift bits
*
* OUTPUT:
*	shifted complex number	
*
*******************************************************************************/
scint16 scaling(scint16 data, int shift)
{
	if(shift < 0)
	{
		data.real = data.real << (-shift);
		data.imag = data.imag << (-shift);
	}
	else if(shift > 0)
	{
		data.real = (data.real >> shift) + ((data.real >>(shift-1)) & 0x01);
		data.imag = (data.imag >> shift) + ((data.imag >>(shift-1)) & 0x01 );
	}

	return data;
}