/***********************************************************
*
* FFT library. SWUST, rhodesn@163.com
* MAX point is 4096 dot
* 2006-06-24
************************************************************/

#define PI 3.14159265358979

#include <math.h>
#include <string.h>
#include <dsp_radix2.h>
#include <dsp_bitrev_cplx.h>
#include "fft.h"
/*global functions*/

void digitrev_index(short *index, int n,int radix);
void gen_twiddle(short *w, int n, double scale);

/*global vars*/
#pragma DATA_SECTION(g_DSPFFTData,".dsp_fft_sect")
#pragma DATA_ALIGN(g_DSPFFTData,8)
static unsigned char g_DSPFFTData[40960+128];

#pragma DATA_ALIGN(g_x1,8)
short *g_x1;
//#pragma DATA_ALIGN(g_x2,8)
//int *g_x2;
#pragma DATA_ALIGN(g_window,8)
short *g_window;
#pragma DATA_ALIGN(g_index,8)
short *g_index;

/*
static short g_index[64];
static short g_window[numlength];
static short g_x1[numlength*2];
static int   g_x2[numlength];
*/

static int radix;
int nx1;

/*
	initializing the DSP_FFT functions, generate the table and index for FFT useing
	DotNumber: 1024/2048/4096... 2' power
	iRadix   : 2
*/
void DSP_FFT_init(int DotNumber,int iRadix)
{
	double scale;
	scale = 32767.5;
	radix = iRadix;
	nx1	  = DotNumber;
	
	memset(g_DSPFFTData,0,40960+128);
	g_x1 = (short *)g_DSPFFTData;
//	g_x2 = (int *)(g_x1+2*DotNumber);
	g_window = (short *)(g_x1+2*DotNumber);
	g_index = (short *)(g_window+DotNumber);

	/*init the index for fft bitrev*/
	digitrev_index(g_index,nx1,radix);
	/*init the fft coefficients*/
	gen_twiddle(g_window,nx1,scale);

}

/*
	test FFT algorithm. three kinds of frequency.
	and result of FFT is R0 I0, R1 I1,.....
*/
void DSP_FFT_test(short *OutData)
{
	int i;
	
	/*構(gòu)造要做FFT的序列，按實(shí)部0、虛部0，實(shí)部1、虛部1...序列構(gòu)造*/
	for (i=0; i<nx1; i++){
		g_x1[2*i]=(short)((cos(PI*i/5.0)+cos(PI*i/10.0)+cos(PI*i/20.0))*128);//
		g_x1[2*i+1]=0;
	}
	/*FFT algorithm*/
	DSP_radix2(nx1,g_x1,g_window);
	DSP_bitrev_cplx((int *)g_x1, g_index, nx1);
	
	memcpy((void *)OutData,(void *)g_x1,nx1*4);
	
	//calculate the magnitude of FFT
/*
	for (i=0;i<nx1; i++)
		//g_x2[i]=sqrt(g_x1[2*i]*g_x1[2*i]+g_x1[2*i+1]*g_x1[2*i+1]);
		g_x2[i] = sqrt(OutData[2*i]*OutData[2*i]+OutData[2*i+1]*OutData[2*i+1]);
*/
	
}

/*
	FFT core. 
	input : AudioData: real data, counter is DotNumber
	output: FftData  : magnitude of FFT, sqrt(R*R+I*I)
*/
void DSP_FFT_core(short *AudioData,short *FftData)
{
	int i;
	/*construct the sequence of input data*/
#pragma MUST_ITERATE(1024,,2)	
	for (i=0; i<nx1; i++)
	{
		g_x1[2*i] = AudioData[i];
		g_x1[2*i+1]=0;
	}
	/*radix 2 of fft*/
	DSP_radix2(nx1,g_x1,g_window);
	/*bit reversal of complex*/
	DSP_bitrev_cplx((int *)g_x1, g_index, nx1);
	
	memcpy((void *)FftData,(void *)g_x1,nx1*4);
	
/*	
	for (i=0;i<nx1; i++){
		g_x2[i]=sqrt(g_x1[2*i]*g_x1[2*i]+g_x1[2*i+1]*g_x1[2*i+1]);
	}
*/	
}

void digitrev_index(short *index, int n,int radix)              
{                                                                   
    int   i, j, k;//, radix = 2;                                                  
    short nbits, nbot, ntop, ndiff, n2, raddiv2;                    
                                                                    
    nbits = 0;                                                      
    i = n;                                                          
    while (i > 1)                                                   
    {                                                               
        i = i >> 1;                                                 
        nbits++;                                                    
    }                                                               
                                                                    
    raddiv2 = radix >> 1;                                           
    nbot    = nbits >> raddiv2;                                     
    nbot    = nbot << raddiv2 - 1;                                  
    ndiff   = nbits & raddiv2;                                      
    ntop    = nbot + ndiff;                                         
    n2      = 1 << ntop;                                            
                                                                    
    index[0] = 0;                                                   
    for ( i = 1, j = n2/radix + 1; i < n2 - 1; i++)                 
    {                                                               
        index[i] = j - 1;                                           
                                                                    
        for (k = n2/radix; k*(radix-1) < j; k /= radix)             
            j -= k*(radix-1);                                       
                                                                    
        j += k;                                                     
    }                                                               
    index[n2 - 1] = n2 - 1;                                         
} 

/* ======================================================================== */
/*  D2S -- Truncate a 'double' to a 'short', with clamping.                 */
/* ======================================================================== */
static short d2s(double d)
{
    if (d >=  32767.0) return  32767;
    if (d <= -32768.0) return -32768;
    return (short)d;
}
    
void gen_twiddle(short *w, int n, double scale)
{
    double angle, step;
    int i;

    step = (2.0 * PI) / (double)n;
    for (i = 0, angle = 0.0; i < n; i += 2, angle += step)
    {
        w[i + 0] = d2s(scale * -cos(angle));
        w[i + 1] = d2s(scale * -sin(angle));
    }

    return ;
}