#include "math.h"
#include "ICETEK-DM642-PCI.h"

//工作變量
unsigned char m_dbFrameBuffer[SIMGWIDTH*SIMGHEIGHT];
unsigned char m_dbTargetImage[TIMGWIDTH*TIMGHEIGHT];
unsigned char m_dbFFTImage[FFTIMGWIDTH*FFTIMGHEIGHT];
unsigned int m_uVideoStatus,m_bFreeze;
complex TD[TIMGWIDTH*TIMGHEIGHT];
complex FD[TIMGWIDTH*TIMGHEIGHT];
complex w[TIMGWIDTH];
complex x1[TIMGWIDTH];
complex x2[TIMGWIDTH];

void ICETEKDM642PCIBoardInit()
{
	m_uVideoStatus=m_bFreeze=0;
}

void ICETEKDM642PCIFourier()
{
	// 中間變量
	float fTemp,fWork1,fWork2;
	unsigned char *pWork,*pWork3;
	int nWork,ii,jj;
	complex *pWork1,*pWork2;
	
	// 循環變量
	int	i;
	int	j;
	
	// 進行付立葉變換的寬度和高度（2的整數次方）
	int	w;
	int	h;
	
	int	wp;
	int	hp;
	
	// 賦初值
	w = 1;
	h = 1;
	wp = 0;
	hp = 0;
	
	// 計算進行付立葉變換的寬度和高度（2的整數次方）
	while ( w*2<=TIMGWIDTH )
	{
		w*=2; wp++;
	}
	while ( h*2<=TIMGHEIGHT )
	{
		h*=2; hp++;
	}
	pWork1=TD;
	for ( i=0;i<h;i++ )	// 行
	{
		pWork=m_dbFrameBuffer+i*SIMGWIDTH;
		for ( j=0;j<w;j++,pWork++,pWork1++ )	// 列
		{
			pWork1->real=(*pWork);
			pWork1->img=0;
		}
	}
	pWork1=TD; pWork2=FD;
	// 對y方向進行快速付立葉變換
	for ( i=0;i<h;i++ )	FFT(pWork1+w*i,pWork2+w*i,wp);
	// 保存變換結果
	for ( i=0;i<h;i++ )
		for ( j=0;j<w;j++ )
			TD[j*h+i]=FD[i*w+j];

	// 對x方向進行快速付立葉變換
	for ( i=0;i<w;i++ )	FFT(pWork1+i*h,pWork2+i*h,hp);
	for ( i=0;i<w;i++ )
		for ( j=0;j<h;j++ )
			TD[j*w+i]=FD[i*h+j];

	pWork1=TD; pWork2=FD; pWork=m_dbTargetImage;
	for ( i=0;i<h;i++ )	// 行
	{
		for ( j=0;j<w;j++ )	// 列
		{
			// 計算頻譜
			fWork1=(pWork2+j*h+i)->real;
			fWork2=(pWork2+j*h+i)->img;
			fTemp=sqrt(fWork1*fWork1+fWork2*fWork2)/100;	//將幅度適當縮小后轉換成灰度顯示
			if ( fTemp>255 )	// 判斷是否超過255
				fTemp = 255;	// 對于超過的，直接設置為255
			pWork3=pWork+TIMGWIDTH*(TIMGHEIGHT-1-(i<h/2?i+h/2:i-h/2))+(j<w/2?j+w/2:j-w/2);
			(*pWork3)=(unsigned char)fTemp;
		}
	}
	pWork3=m_dbFFTImage;
	for ( i=0;i<FFTIMGHEIGHT;i++ )
	{
		for ( j=0;j<FFTIMGWIDTH;j++,pWork3++ )
		{
			pWork=m_dbTargetImage+TIMGHEIGHT/FFTIMGHEIGHT*i*TIMGHEIGHT+TIMGWIDTH/FFTIMGWIDTH*j;
			nWork=0;
			for ( ii=0;ii<TIMGHEIGHT/FFTIMGHEIGHT;ii++,pWork+=TIMGWIDTH )
				for ( jj=0;jj<TIMGWIDTH/FFTIMGWIDTH;jj++ )
					nWork+=pWork[jj];
			nWork/=((TIMGHEIGHT/FFTIMGHEIGHT)*(TIMGWIDTH/FFTIMGWIDTH));
			(*pWork3)=nWork;
		}
	}
}

void FFT(complex *TD,complex *FD,int r)
{
	// 付立葉變換點數
	int	count;
	// 循環變量
	int i,j,k;
	// 中間變量
	int bfsize,p;
	// 角度
	float angle;
	complex *W,*X1,*X2,*X;
	
	W=w; X1=x1; X2=x2;
	// 計算付立葉變換點數
	count=1<<r;
	
	// 計算加權系數
	for ( i=0;i<count/2;i++ )
	{
		angle=-i*PI*2/count;
		(W+i)->real=cos(angle);
		(W+i)->img=sin(angle);	
	}
	
	// 將時域點寫入X1
	for ( i=0;i<count;i++ )
	{
		(X1+i)->real=(TD+i)->real;
		(X1+i)->img=(TD+i)->img;
	}
	
	// 采用蝶形算法進行快速付立葉變換
	for ( k=0;k<r;k++ )
	{
		for ( j=0;j<1<<k;j++ )
		{
			bfsize=1<<(r-k);
			for ( i=0;i<bfsize/2;i++ )
			{
				p=j*bfsize;
				(X2+i+p)->real=(X1+i+p)->real+(X1+i+p+bfsize/2)->real;
				(X2+i+p)->img=(X1+i+p)->img+(X1+i+p+bfsize/2)->img;
				(X2+i+p+bfsize/2)->real=((X1+i+p)->real-(X1+i+p+bfsize/2)->real)*(W+i*(1<<k))->real 
				                          -((X1+i+p)->img-(X1+i+p+bfsize/2)->img)*(W+i*(1<<k))->img;
				(X2+i+p+bfsize/2)->img=((X1+i+p)->real-(X1+i+p+bfsize/2)->real)*(W+i*(1<<k))->img 
				                          +((X1+i+p)->img-(X1+i+p+bfsize/2)->img)*(W+i*(1<<k))->real;             		
			}
		}
		X=X1; X1=X2; X2=X;
	}
	
	// 重新排序
	for ( j=0;j<count;j++ )
	{
		p = 0;
		for ( i=0;i<r;i++ )
			if ( j&(1<<i) )
				p+=1<<(r-i-1);
		(FD+j)->real=(X1+p)->real;
		(FD+j)->img=(X1+p)->img;
	}
}