/***************************************************************************/
/* FFT.C
/* Fast-Fourier-Transformation
/* Author:          Jens Ahrensfeld
/* Datum :          24.06.1999
/* letzte Änderung: 09.06.2000
/***************************************************************************/
#define AVNEEDFLOAT

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "avtypes.h"
#include "averror.h"
#include "avrtl.h"
#include "avfft.h"

#define	PI			3.1415926535897932384626433832795

/***************************************************************************/
/* Konstruktor FFT-Objekt									               */
/*                                                                         */
/***************************************************************************/
void FFTinit(FFT *pFFT, UINT32 N)
{
	pFFT->m_numPoints = 0;
	pFFT->m_numStages = 0;

    pFFT->pTwfRe = NULL;
	pFFT->pTwfIm = NULL;

	if(IsPowerOfTwo(N) == AV_E_FALSE)
	{
		printf("\n%u is not a power of 2!\n",N);
		exit(1);
	}

	pFFT->m_numStages = (UINT32) (1e-06 + log((avfloat_t)N)/log(2.0));
	pFFT->m_numPoints = (UINT32 )N;

	pFFT->pTwfRe = (avfloat_t*)malloc(pFFT->m_numPoints * sizeof(avfloat_t) /2);
	pFFT->pTwfIm = (avfloat_t*)malloc(pFFT->m_numPoints * sizeof(avfloat_t) /2);

	FFTCalcTwiddleTable(pFFT->pTwfRe, pFFT->pTwfIm, pFFT->m_numPoints);

#ifdef FFTMsg
	printf("\n%d-Point FFT object created.\n",pFFT->m_numPoints);
#endif
}
/***************************************************************************/
/* Destruktor FFT-Objekt   									               */
/*                                                                         */
/***************************************************************************/
void FFTfree(FFT *pFFT)
{
	if (pFFT->pTwfRe != NULL)
	{
		free(pFFT->pTwfRe);
        pFFT->pTwfRe = NULL;
	}

	if (pFFT->pTwfIm != NULL)
	{
		free(pFFT->pTwfIm);
	    pFFT->pTwfIm = NULL;
	}

	pFFT->m_numPoints = 0;
	pFFT->m_numStages = 0;

#ifdef FFTMsg
	printf("\n%d-Point FFT object deleted.\n",pFFT->m_numPoints);
#endif
}

/***************************************************************************/
/* FFT                                                                     */
/* FAST-FOURIER-TRANSFORMATION 	s(t) -> S(f)                               */
/***************************************************************************/
void fft(FFT *pFFT, avfloat_t *in_re, avfloat_t *in_im)
{
register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j;
avfloat_t tempr, tempi, s, c;

#ifdef FFTMsg
UINT32 numMul, numAdd;	
printf ("\nStart %d-Point FFT.\n\n",pFFT->m_numPoints);
#endif

	// Do the bit reversal
	i2 = pFFT->m_numPoints >> 1;
	j = 0;
	for (i=0; i <pFFT->m_numPoints-1;i++) {
		if (i < j) {
			tempr = in_re[i];
			tempi = in_im[i];
			in_re[i]		=  in_re[j];
			in_im[i]		=  in_im[j];
			in_re[j]		=  tempr;
			in_im[j]		=  tempi;
		}
		k = i2;
		while (k <= j) {
			j = j-k;
			k >>= 1;
		}
		j = j+k;
	}

	// Calculate the FFT

#ifdef FFTMsg
numMul =0;
numAdd =0;
#endif
	numNodesPerStage = pFFT->m_numPoints;
	for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) {
		k=0;
		numNodesPerStage = numNodesPerStage/2;
		opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage);

#ifdef FFTMsg
printf ("Processing STAGE # %2d",stageCnt);
#endif
		for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++)	{
			twf = 0;
			opCnt = opsPerNode;

			while (opCnt)	{
#ifdef FFTMsg
				numMul++;
				numAdd +=2;
#endif
				c =   pFFT->pTwfRe[twf];
				s = - pFFT->pTwfIm[twf];
				kk = k + opsPerNode;
				tempr			= (c * in_re[kk]) - (s * in_im[kk]);
				tempi			= (s * in_re[kk]) + (c * in_im[kk]);
				in_re[kk]		= in_re[k] - tempr;
				in_im[kk]		= in_im[k] - tempi;
				in_re[k]		= in_re[k] + tempr;
				in_im[k]		= in_im[k] + tempi;
				k++;
				opCnt--;
				twf += numNodesPerStage;
			}

			k += opsPerNode;
		}
#ifdef FFTMsg
printf (" ... finished.\n");
#endif
	}

#ifdef FFTMsg
printf ("\n%d-Point FFT is completed.\n",pFFT->m_numPoints);
printf ("Total number of complex additions = %u\n",numAdd);
printf ("Total number of complex multiplications = %u\n",numMul);
#endif
}

/***************************************************************************/
/* IFFT                                                                    */
/* INVERSE-FAST-FOURIER-TRANSFORMATION 	S(f) -> s(t)                       */
/***************************************************************************/
void ifft(FFT *pFFT, avfloat_t *in_re, avfloat_t *in_im)
{
register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j;
avfloat_t tempr, tempi, s, c;

	#ifdef FFTMsg
	printf ("\nStart %d-Point IFFT.\n\n",pFFT->m_numPoints);
	#endif

	// Do the bit reversal
	i2 = pFFT->m_numPoints >> 1;
	j = 0;
	for (i=0; i <pFFT->m_numPoints-1;i++) {
		if (i < j) {
			tempr = in_re[i];
			tempi = in_im[i];
			in_re[i]		=  in_re[j];
			in_im[i]		=  in_im[j];
			in_re[j]		=  tempr;
			in_im[j]		=  tempi;
		}
		k = i2;
		while (k <= j) {
			j = j-k;
			k >>= 1;
		}
		j = j+k;
	}

	// Calculate the IFFT
	numNodesPerStage = pFFT->m_numPoints;
	for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) {
		k=0;
		numNodesPerStage = numNodesPerStage/2;
		opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage);

		#ifdef FFTMsg
		printf ("Processing STAGE # %2d",stageCnt);
		#endif

		for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++)	{
			twf = 0;
			opCnt = opsPerNode;

			while (opCnt)	{
				c =  pFFT->pTwfRe[twf];
				s =  pFFT->pTwfIm[twf];
				kk = k + opsPerNode;
				tempr			= (c * in_re[kk]) - (s * in_im[kk]);
				tempi			= (s * in_re[kk]) + (c * in_im[kk]);
				in_re[kk]		= in_re[k] - tempr;
				in_im[kk]		= in_im[k] - tempi;
				in_re[k]		= in_re[k] + tempr;
				in_im[k]		= in_im[k] + tempi;
				k++;
				opCnt--;
				twf += numNodesPerStage;
			}
			k += opsPerNode;
		}
	#ifdef FFTMsg
	printf (" ... finished.\n");
	#endif
	}
#ifdef FFTMsg
printf ("\n%d-Point IFFT is completed.\n",pFFT->m_numPoints);
#endif
}

/***************************************************************************/
/* FFTS Autoscale Version  X = 1/N * FFT{x}                                                                    
/* FAST-FOURIER-TRANSFORMATION 	s(t) -> S(f)                               
/***************************************************************************/
void ffts(FFT *pFFT, avfloat_t *in_re, avfloat_t *in_im)
{
register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j;
avfloat_t tempr, tempi, s, c;

#ifdef FFTMsg
UINT32 numMul, numAdd;	
printf ("\nStart %d-Point FFT.\n\n",pFFT->m_numPoints);
#endif

	// Do the bit reversal
	i2 = pFFT->m_numPoints >> 1;
	j = 0;
	for (i=0; i <pFFT->m_numPoints-1;i++) {
		if (i < j) {
			tempr = in_re[i];
			tempi = in_im[i];
			in_re[i]		=  in_re[j];
			in_im[i]		=  in_im[j];
			in_re[j]		=  tempr;
			in_im[j]		=  tempi;
		}
		k = i2;
		while (k <= j) {
			j = j-k;
			k >>= 1;
		}
		j = j+k;
	}

	// Calculate the FFT

#ifdef FFTMsg
numMul =0;
numAdd =0;
#endif
	numNodesPerStage = pFFT->m_numPoints;
	for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) {
		k=0;
		numNodesPerStage = numNodesPerStage/2;
		opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage);

#ifdef FFTMsg
printf ("Processing STAGE # %2d",stageCnt);
#endif
		for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++)	{
			twf = 0;
			opCnt = opsPerNode;

			while (opCnt)	{
#ifdef FFTMsg
				numMul++;
				numAdd +=2;
#endif
				c =   pFFT->pTwfRe[twf];
				s = - pFFT->pTwfIm[twf];
				kk = k + opsPerNode;
				tempr			= (c * in_re[kk]) - (s * in_im[kk]);
				tempi			= (s * in_re[kk]) + (c * in_im[kk]);
				in_re[kk]		= (in_re[k] - tempr)/2;
				in_im[kk]		= (in_im[k] - tempi)/2;
				in_re[k]        = (in_re[k] + tempr)/2;
				in_im[k]        = (in_im[k] + tempi)/2;
				k++;
				opCnt--;
				twf += numNodesPerStage;
			}

			k += opsPerNode;
		}
#ifdef FFTMsg
printf (" ... finished.\n");
#endif
	}

#ifdef FFTMsg
printf ("\n%d-Point FFT is completed.\n",pFFT->m_numPoints);
printf ("Total number of complex additions = %u\n",numAdd);
printf ("Total number of complex multiplications = %u\n",numMul);
#endif
}

/***************************************************************************/
/* TWIDDLE-FAKTOR-TABLE                 							       */
/* Erstellt Twiddle-Faktor-Tabelle  von WN^0 bis WN^N/2                    */
/***************************************************************************/
void FFTCalcTwiddleTable (avfloat_t *pRealData, avfloat_t *pImagData, UINT32 numPoints)
{
UINT32 i, size;
avfloat_t arg1;

	size = numPoints/2;
	arg1 = (avfloat_t)(2.0 * PI / (avfloat_t)numPoints );

	#ifdef FFTMsg
	printf ("Calculating and initializing Twiddle-Table (%d kB)...",size*sizeof(COMPLEX));
	#endif

	for (i = 0; i < size; i++) {
		pRealData[i] = (avfloat_t)cos(arg1* (avfloat_t)i);
		pImagData[i] = (avfloat_t)sin(arg1* (avfloat_t)i);
	}
	#ifdef FFTMsg
	printf(" completed.\n");
	#endif
}

/***************************************************************************/
/* MODULUS                 										           */
/* Berechnet den Betrag einer komplexen Zahl                               */
/***************************************************************************/
void Modulus(avfloat_t *pRealData, avfloat_t *pImagData, UINT32 N)
{
UINT32 i;
avfloat_t temp;

	for (i = 0; i < N; i++) {
		temp = (avfloat_t)sqrt( pRealData[i] * pRealData[i] + pImagData[i] * pImagData[i]);
		pImagData[i] = (avfloat_t)atan2(pImagData[i],pRealData[i]);
		pRealData[i] = temp;
	}
}
/***************************************************************************/
/* Hanning                              								   */
/* Legt das Hanningfenster auf die Abtastwerte im Zeitbereich der Groesse N	           */
/***************************************************************************/
void Hanning(avfloat_t *pRealData, avfloat_t *pImagData, UINT32 N, UINT32 maximum)
{
	UINT32 n;
	avfloat_t arg;

	for (n=0; n < N; n++) 
	{
	arg = (avfloat_t)(2*PI*n /(N-1) - 2*PI*maximum/N);
	pRealData[n] = pRealData[n] * (avfloat_t)(1 + cos(arg)) /2;
	pImagData[n] = pImagData[n] * (avfloat_t)(1 + cos(arg)) /2;
	}
}

/***************************************************************************/
/* HANNING_K                              								   */
/* Gibt einen Faktor k in Abhängigkeit von n bezogen auf N zurück.         */
/***************************************************************************/
avfloat_t hanning_k(UINT32 n, UINT32 N)
{
avfloat_t arg;
arg = (avfloat_t)(2 * PI / (avfloat_t)N);

return ((1-(avfloat_t)cos(arg*(avfloat_t)n))/2);
}

/***************************************************************************/
/* GAUSS_K                              								   */
/* Gibt einen Faktor k in Abhängigkeit von n bezogen auf N zurück.         */
/***************************************************************************/
avfloat_t gauss_k(UINT32 n, UINT32 m, UINT32 s)
{
avfloat_t arg;
arg = (avfloat_t)((n-m)*(n-m)/(2*s*s));

return (avfloat_t)(exp(-arg));
}

/***************************************************************************/
/* Normalize)                              								   */
/*                                                                         */
/***************************************************************************/
void Scale(avfloat_t *pRealData, avfloat_t *pImagData, avfloat_t scaleFactor, UINT32 N)
{
	UINT32 k;

	// Scaling Data
	for (k=0; k < N; k++) 
	{
		pRealData[k] *= (avfloat_t)scaleFactor;
		pImagData[k] *= (avfloat_t)scaleFactor;
	}
}

/***************************************************************************/
/* BiPower()
/* checks if N is power of 2                                               
/***************************************************************************/
AVERR IsPowerOfTwo(UINT32 N)
{
	UINT32 i, iterations;
	iterations = sizeof(UINT32)*8;

	if (N == 0)
		return AV_E_FALSE;

	for (i=0; i <iterations; i++)
	{
		if (N == (UINT32)(2 << i))
			return AV_E_OK;
	}

	return AV_E_FALSE;
}

/***************************************************************************/
/* FFT2Dinit()
/* 
/***************************************************************************/
void FFT2Dinit(struct _sFFT2D *pFFT, UINT32 Nx, UINT32 Ny)
{
	pFFT->pXfft = NULL;
	pFFT->pYfft = NULL;

	pFFT->m_Nx = Nx;
	pFFT->m_Ny = Ny;

	pFFT->pXfft = (struct _sFFT*)malloc(sizeof(pFFT->pXfft));
	FFTinit(pFFT->pXfft, Nx);

	if (Nx == Ny)
		pFFT->pYfft = pFFT->pXfft;

	else
	{
		pFFT->pYfft = (struct _sFFT*)malloc(sizeof(pFFT->pYfft));
		FFTinit(pFFT->pYfft, Ny);
	}
}

/***************************************************************************/
/* FFT2Dfree()
/* 
/***************************************************************************/
void FFT2Dfree(struct _sFFT2D *pFFT)
{
		if (pFFT->pXfft != NULL)
		{
			FFTfree(pFFT->pXfft);
			pFFT->pXfft = NULL;
		}
		if (pFFT->pYfft != NULL)
		{
			FFTfree(pFFT->pYfft);
			pFFT->pYfft = NULL;
		}

		pFFT->m_Nx = 0;
		pFFT->m_Ny = 0;
}

/***************************************************************************/
/* fft2d()
/* 
/***************************************************************************/
void fft2d(struct _sFFT2D *pFFT, avfloat_t **ppReal, avfloat_t **ppImag)
{
	avfloat_t *pTempr, *pTempi;
	UINT32 i, row;


	pTempr = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t));
	pTempi = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t));

	/* Transform ROWs */
	for (row=0; row <pFFT->m_Ny; row++)
		fft(pFFT->pXfft, ppReal[row], ppImag[row]);

	/* Transform COLs */
	for (row=0; row <pFFT->m_Nx; row++)
	{
		for(i=0; i <pFFT->m_Ny; i++)
		{
			pTempr[i] = ppReal[i][row];
			pTempi[i] = ppImag[i][row];
		}

		fft(pFFT->pYfft, pTempr, pTempi);
		for(i=0; i <pFFT->m_Ny; i++)
		{
			ppReal[i][row] = pTempr[i];
			ppImag[i][row] = pTempi[i];
		}
	}
	free(pTempr);
	free(pTempi);

}

/***************************************************************************/
/* ifft2d()
/* 
/***************************************************************************/
void ifft2d(struct _sFFT2D *pFFT, avfloat_t **ppReal, avfloat_t **ppImag)
{
	avfloat_t *pTempr, *pTempi;
	UINT32 i, row;


	pTempr = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t));
	pTempi = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t));

	/* Transform COLs */
	for (row=0; row <pFFT->m_Nx; row++)
	{
		for(i=0; i <pFFT->m_Ny; i++)
		{
			pTempr[i] = ppReal[i][row];
			pTempi[i] = ppImag[i][row];
		}

		ifft(pFFT->pYfft, pTempr, pTempi);
		for(i=0; i <pFFT->m_Ny; i++)
		{
			ppReal[i][row] = pTempr[i];
			ppImag[i][row] = pTempi[i];
		}
	}
	free(pTempr);
	free(pTempi);

	/* Transform ROWs */
	for (row=0; row <pFFT->m_Ny; row++)
		ifft(pFFT->pXfft, ppReal[row], ppImag[row]);

}

/***************************************************************************/
/* DFT()
/* DISKRETE FOURIER-TRANSFORMATION 	S(f) -> s(t)                           */
/***************************************************************************/
void DFT(avfloat_t *pDataR, avfloat_t *pDataI, UINT32 N)
{
	avfloat_t *a, *b;
	avfloat_t phi, c, s;
	UINT32 i, j;

	a = (avfloat_t*)malloc(N * sizeof(avfloat_t));
	b = (avfloat_t*)malloc(N * sizeof(avfloat_t));

	memcpy((avfloat_t*)a,(avfloat_t*)pDataR,N * sizeof(avfloat_t));
	memcpy((avfloat_t*)b,(avfloat_t*)pDataI,N * sizeof(avfloat_t));

	for (i = 0; i < N; i++)
	{
		pDataR[i] = 0;
		pDataI[i] = 0;
		
		for (j = 0; j < N; j++)
		{
			phi = (avfloat_t)(2*i*j*PI/N);
			c =   (avfloat_t)(0.5*cos(phi));
			s =   (avfloat_t)(0.5*sin(phi));
			
			pDataR[i] += c*a[j] - s*b[j];
			pDataI[i] += s*a[j] + c*b[j];
		}
	}
	free(a);
	free(b);

}

/***************************************************************************/
/* IDFT()
/* INVERSE DISKRETE FOURIER-TRANSFORMATION 	S(f) -> s(t)                   */
/***************************************************************************/
void IDFT(avfloat_t *pDataR, avfloat_t *pDataI, UINT32 N)
{
	avfloat_t *a, *b;
	avfloat_t phi, c, s;
	UINT32 i, j;

	a = (avfloat_t*)malloc(N * sizeof(avfloat_t));
	b = (avfloat_t*)malloc(N * sizeof(avfloat_t));

	memcpy((avfloat_t*)a,(avfloat_t*)pDataR,N * sizeof(avfloat_t));
	memcpy((avfloat_t*)b,(avfloat_t*)pDataI,N * sizeof(avfloat_t));

	for (i = 0; i < N; i++)
	{
		pDataR[i] = 0;
		pDataI[i] = 0;
		
		for (j = 0; j < N; j++)
		{
			phi = (avfloat_t)(2*i*j*PI/N);
			c =  (avfloat_t) (0.5*(avfloat_t)cos(phi));
			s =  (avfloat_t)(-0.5*(avfloat_t)sin(phi));
			
			pDataR[i] += c*a[j] - s*b[j];
			pDataI[i] += s*a[j] + c*b[j];
		}
	}
	free(a);
	free(b);

}