#include <def21489.h>
#include <Cdef21489.h>
#include <math.h>
#include <filter.h>
#include <drv/fft_drv.h>
#include "fft.h"


#define pi 3.1415926
/* Declaring the external buffers needed for FFT Accelerator*/
//extern float FFT_IP_buff[]; //ÊäÈ뻺³å
//extern float FFT_OP_buff[]; //Êä³ö»º³å

static float FFT_VCF_buff[2*V];
static float FFT_HCF_buff[2*H];
static float FFT_IM_OP_buff[N*2];
static float FFT_SP_CF_buff[N*4];

/*CPIFFT,IBFFT,ILFFT,ICFFT,IMFFT,IIFFt*/
/*
CPIFFT: ͨµÀÖ¸Õë¼Ä´æÆ÷,±£´æÏÂÒ»¸ö´«Êä¿ØÖÆ¿éµÄ¿ªÊ¼Î»Öá£
IBFFT: »ùÖ·¼Ä´æÆ÷£¬Ñ­»·»º³å¿ªÊ¼µØÖ·
ILFFT: ³¤¶È¼Ä´æÆ÷£¬Ñ­»·»º³å³¤¶È
ICFFT: ¼ÆÊý¼Ä´æÆ÷£¬±êʾ»¹Ê£Óà¶àÉÙ×ÖÐèÒª´«Êä
IMFFT: ¿ÉÐ޸ļĴæÆ÷£¬DMA¶ÁдºóIIFFT±ä»¯µÄÓзûºÅÔöÁ¿
IIFFT: Ë÷Òý¼Ä´æÆ÷£¬Ö¸ÏòÏÂÒ»¸öÄÚ²¿ÄÚ´æ¶ÁдλÖá£
*/


/*Coefficient TCB for loading V cofficients */


static int FFT_VCF_TCB[6]={0,(int)FFT_VCF_buff,2*V,2*V,1,(int)FFT_VCF_buff};// Vertical coeff TCB
static int FFT_VIP_TCB[6]={0,(int)0,2*N-1,2*N-1,2*H,(int)0};// input TCB modify is 2H
	
static int FFT_VLASTIP_TCB[6]={0,(int)0,2*N,1,1,(int)0+2*N-1};// input TCB chain
	
static int FFT_IMOP_TCB1[6]={0,(int)FFT_IM_OP_buff,2*N,2*N,1,(int)FFT_IM_OP_buff};// Vertical FFT receive buffer.
			
static int FFT_PWCF_TCB[N/128][6]={{0,(int)FFT_SP_CF_buff,4*N,512,1,(int)FFT_SP_CF_buff+0}};
static int FFT_IMIP_TCB[N/128][6]={{0,(int)FFT_IM_OP_buff,2*N,256,1,(int)FFT_IM_OP_buff+0}};
static int FFT_IMOP_TCB[N/128][6]={{0,(int)FFT_IM_OP_buff,2*N,256,1,(int)FFT_IM_OP_buff+0}};

	
static int FFT_HCF_TCB[6]={0,(int)FFT_HCF_buff,2*H,2*H,1,(int)FFT_HCF_buff}; //  Horizontal coeff TCB
static int FFT_HIP_TCB[6]={0,(int)FFT_IM_OP_buff,2*N-1,2*N-1,2*V,(int)FFT_IM_OP_buff};// Special product transmit buffer. modify is 2V
static int FFT_HLASTIP_TCB[6]={0,(int)FFT_IM_OP_buff,2*N,1,1,(int)FFT_IM_OP_buff+2*N-1};// Special product transmit buffer.
static int FFT_OP_TCB[6]={0,(int)0,2*N-1,2*N-1,2*V,(int)0};//modify is 2V
static int FFT_LASTOP_TCB[6]={0,(int)0,2*N,1,1,(int)0+2*N-1};

	
//sp_cf³¤¶ÈΪV*H*4
void fft_coeff_calculation(void)
{
	int i,j,k;
	float temp[128];
	float w;
	//e^-2pikm 
	for(i=0,k=0;i<H;i++){
		for(j=0;j<V;j++){
			w=-2*pi/N*i*j;
			FFT_SP_CF_buff[k++]=cosf(w);
			FFT_SP_CF_buff[k++]=sinf(w);
			FFT_SP_CF_buff[k++]=-sinf(w);
			FFT_SP_CF_buff[k++]=cosf(w);
		}
	}
	
	twidfftf(temp,temp+V/2,V);		
	//½»´íÅÅÁÐϵÊý,´ò°üģʽ
	for(i=0,j=V/2,k=0;i<V/2;i++){
		FFT_VCF_buff[k++]=temp[i];
		FFT_VCF_buff[k++]=temp[j+i];
		FFT_VCF_buff[k++]=-temp[j+i];
		FFT_VCF_buff[k++]=temp[i];
	}
	
	twidfftf(temp,temp+H/2,H);
	for(i=0,j=H/2,k=0;i<H/2;i++){
		FFT_HCF_buff[k++]=temp[i];
		FFT_HCF_buff[k++]=temp[j+i];
		FFT_HCF_buff[k++]=-temp[j+i];
		FFT_HCF_buff[k++]=temp[i];
	}	
}

	
void fft_config(bool repeat,int _sig)
{
	int temp,i;
	int index = _sig*5/30;
	int offset = (_sig-index*6)*5 ;
		
	if(_sig>=0){
	//Mapping the FFT DMA interrupt		
		temp=* (pPICR0+index);
		temp&=~(0x1f<<offset);
		temp|= (0x1b<<offset);
		*(pPICR0+index)=temp;
	}

	
	*pPMCTL1&=~(BIT_17|BIT_18);
	*pPMCTL1|=FFTACCSEL;
	//PMCTL1 effect latency
	asm("nop;nop;nop;nop;");

	//Clearing the FFTCTL registers
	*pFFTCTL2=0;
	*pFFTCTL1=FFT_RST;
	asm("nop;nop;nop;nop;");
	

	for(i=1;i<N/128;i++){
		FFT_PWCF_TCB[i][1]=FFT_PWCF_TCB[0][1];
		FFT_PWCF_TCB[i][2]=FFT_PWCF_TCB[0][2];
		FFT_PWCF_TCB[i][3]=FFT_PWCF_TCB[0][3];
		FFT_PWCF_TCB[i][4]=FFT_PWCF_TCB[0][4];
		FFT_PWCF_TCB[i][5] = (int)FFT_SP_CF_buff+i*512;
		
		FFT_IMIP_TCB[i][1]=FFT_IMIP_TCB[0][1];
		FFT_IMIP_TCB[i][2]=FFT_IMIP_TCB[0][2];
		FFT_IMIP_TCB[i][3]=FFT_IMIP_TCB[0][3];
		FFT_IMIP_TCB[i][4]=FFT_IMIP_TCB[0][4];
		FFT_IMIP_TCB[i][5] = (int)FFT_IM_OP_buff+i*256;
		
		FFT_IMOP_TCB[i][1]=FFT_IMOP_TCB[0][1];
		FFT_IMOP_TCB[i][2]=FFT_IMOP_TCB[0][2];
		FFT_IMOP_TCB[i][3]=FFT_IMOP_TCB[0][3];
		FFT_IMOP_TCB[i][4]=FFT_IMOP_TCB[0][4];
		FFT_IMOP_TCB[i][5] = (int)FFT_IM_OP_buff+i*256;
	}
	
		
	
	FFT_VCF_TCB[0]=(int)FFT_VIP_TCB+5-0x80000;
	FFT_VIP_TCB[0]=(int)FFT_VLASTIP_TCB+5-0x80000;
	temp=(int)FFT_PWCF_TCB[0];
	FFT_VLASTIP_TCB[0]=0x100000|temp+5-0x80000;
	FFT_IMOP_TCB1[0]=(int)FFT_IMOP_TCB[0]+5-0x80000;
	
	
	for(i=0;i<N/128-1;i++){
		FFT_PWCF_TCB[i][0]=(int)FFT_IMIP_TCB[i]+5-0x80000;
		temp=(int)FFT_PWCF_TCB[i+1];
		FFT_IMIP_TCB[i][0]=0x100000|temp+5-0x80000;
		FFT_IMOP_TCB[i][0]=(int)FFT_IMOP_TCB[i+1]+5-0x80000;
	}
	
	FFT_PWCF_TCB[i][0]=(int)FFT_IMIP_TCB[i]+5-0x80000;
	temp=(int)FFT_HCF_TCB;
	FFT_IMIP_TCB[i][0]=0x100000|temp+5-0x80000;
	FFT_IMOP_TCB[i][0]=(int)FFT_OP_TCB+5-0x80000;
	
	

	FFT_HCF_TCB[0]=(int)FFT_HIP_TCB+5-0x80000;
	FFT_HIP_TCB[0]=(int)FFT_HLASTIP_TCB+5-0x80000;
	FFT_OP_TCB[0]=(int)FFT_LASTOP_TCB+5-0x80000;
	
	
	//FFT_LASTOP_TCB[0] = (int)FFT_VCF_TCB+5-0x80000;
	
//	temp=(int)FFT_VCF_TCB;
	//*pCPIFFT=0x100000|temp+5-0x80000;
//	*pCPOFFT=(int)FFT_IMOP_TCB1+5-0x80000;
	
	//FFT_LASTOP_TCB[0]=0x100000|temp+5-0x80000;

	*pFFTCTL2 = FFT_CPACKIN|FFT_CPACKOUT;
	
	temp = log(V/8)/log(2);
	*pFFTCTL2|= (1<<(temp+6));
	temp = log(V)/log(2);
	*pFFTCTL2|= (temp<<3);
	temp = log(H/8)/log(2);
	*pFFTCTL2|= (1<<(temp+15));
	temp = log(H)/log(2);
	*pFFTCTL2|= (temp<<12);
	temp = log(N/256)/log(2);
	*pFFTCTL2|= 1<<(temp+21);
	
	if(repeat){
		*pFFTCTL2 |= FFT_RPT;
	}

	*pFFTCTL1=FFT_EN|FFT_DMAEN|FFT_START;
}


void fft_startup(int input_address,int out_address)
{

	int temp=(int)FFT_VCF_TCB;
		
	FFT_VIP_TCB[1]=FFT_VIP_TCB[5]=input_address;
	FFT_VLASTIP_TCB[1]=input_address;
	FFT_VLASTIP_TCB[5]=input_address+2*N-1;

	FFT_OP_TCB[1]=FFT_OP_TCB[5]=out_address;
	FFT_LASTOP_TCB[1]=out_address;
	FFT_LASTOP_TCB[5]=out_address+2*N-1;
		

	*pCPIFFT=0x100000|temp+5-0x80000;
	*pCPOFFT=(int)FFT_IMOP_TCB1+5-0x80000;
	
}