/***************************************************************************/
/* FFT.CPP
/* Fast-Fourier-Transformation
/* Author:          Jens Ahrensfeld
/* Datum :          24.06.1999
/* letzte Änderung: 09.06.2000
/***************************************************************************/
#include <math.h>
#include <malloc.h>
#include <memory.h>
#include <stdio.h>
#include <stdlib.h>
#include "../../include/fft.h"

#define	PI			3.1415926535897932384626433832795

//#define FFTMsg

/***************************************************************************/
/* Konstruktor FFT-Objekt									               */
/*                                                                         */
/***************************************************************************/
cfft::cfft(unsigned N)
{
	if(BiPower(N) != 0)
	{
		printf("\n%u is not a power of 2!\n",N);
		exit(1);
	}

	m_numStages = (unsigned) (1e-06 + log((double)N)/log(2.0));
	m_numPoints = (unsigned int)N;

	pTwiddleTbl =  new COMPLEX[m_numPoints/2];
	if (!pTwiddleTbl) 
	{
		printf("\nZu wenig Speicher !\n");
		exit(1);
	}
	
	FFTCalcTwiddleTable(pTwiddleTbl, m_numPoints);

#ifdef FFTMsg
	printf("\n%d-Point FFT object created.\n",m_numPoints);
#endif
}
/***************************************************************************/
/* Destruktor FFT-Objekt   									               */
/*                                                                         */
/***************************************************************************/
cfft::~cfft()
{
	delete [] pTwiddleTbl;

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

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

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

	// Do the bit reversal
	i2 = m_numPoints >> 1;
	j = 0;
	for (i=0; i<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 = m_numPoints;
	for (stageCnt=1; stageCnt <= m_numStages; stageCnt++) {
		k=0;
		numNodesPerStage = numNodesPerStage/2;
		opsPerNode = 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 =   pTwiddleTbl[twf].real;
				s = - pTwiddleTbl[twf].imag;
				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",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 cfft::ifft(double *in_re, double *in_im)
{
register unsigned stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j;
double tempr, tempi, s, c;

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

	// Do the bit reversal
	i2 = m_numPoints >> 1;
	j = 0;
	for (i=0; i<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 = m_numPoints;
	for (stageCnt=1; stageCnt <= m_numStages; stageCnt++) {
		k=0;
		numNodesPerStage = numNodesPerStage/2;
		opsPerNode = 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 =  pTwiddleTbl[twf].real;
				s =  pTwiddleTbl[twf].imag;
				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",m_numPoints);
#endif
}

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

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

	pFFT->m_numStages = (unsigned) (1e-06 + log((double)N)/log(2.0));
	pFFT->m_numPoints = (unsigned int)N;

	pFFT->pTwiddleTbl = (COMPLEX*)malloc(pFFT->m_numPoints * sizeof(COMPLEX) /2);
	if (!pFFT->pTwiddleTbl)
	{
		printf("\nZu wenig Speicher !"); 
		exit(1);
	}

	FFTCalcTwiddleTable(pFFT->pTwiddleTbl, 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->pTwiddleTbl != NULL)
	{
		free(pFFT->pTwiddleTbl);
		pFFT->pTwiddleTbl = 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, double *in_re, double *in_im)
{
register unsigned stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j;
double tempr, tempi, s, c;

#ifdef FFTMsg
unsigned 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->pTwiddleTbl[twf].real;
				s = - pFFT->pTwiddleTbl[twf].imag;
				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, double *in_re, double *in_im)
{
register unsigned stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j;
double 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->pTwiddleTbl[twf].real;
				s =  pFFT->pTwiddleTbl[twf].imag;
				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
}

/***************************************************************************/
/* TWIDDLE-FAKTOR-TABLE                 							       */
/* Erstellt Twiddle-Faktor-Tabelle  von WN^0 bis WN^N/2                    */
/***************************************************************************/
void FFTCalcTwiddleTable (COMPLEX *pTwfTbl, unsigned numPoints)
{
unsigned i, size;
double arg1;

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

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

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

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

	for (i = 0; i < N; i++) {
		temp = sqrt( pRealData[i] * pRealData[i] + pImagData[i] * pImagData[i]);
		pImagData[i] = atan2(pImagData[i],pRealData[i]);
		pRealData[i] = temp;
	}
}

/***************************************************************************/
/* MODULUS2                 										           */
/* Berechnet den Betrag einer komplexen Zahl                               */
/***************************************************************************/
void Modulus2(double *pRealData, double *pImagData, double *pModulus, unsigned N)
{
unsigned i;

	for (i = 0; i < N; i++) {
		pModulus[i] = sqrt( pRealData[i] * pRealData[i] + pImagData[i] * pImagData[i]);
	}
}

/***************************************************************************/
/* Hanning                              								   */
/* Legt das Hanningfenster auf die Abtastwerte im Zeitbereich der Groesse N	           */
/***************************************************************************/
void Hanning(double *pRealData, double *pImagData, unsigned N, unsigned maximum)
{
	unsigned n;
	double arg;

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

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

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

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

return (exp(-arg));
}

/***************************************************************************/
/* Normalize)                              								   */
/*                                                                         */
/***************************************************************************/
void Scale(double *pRealData, double *pImagData, double scaleFactor, unsigned N)
{
	unsigned int k;

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

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

	if (N == 0)
		return -1;

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

	return -1;
}

/***************************************************************************/
/* FFT2Dinit()
/* 
/***************************************************************************/
void FFT2Dinit(struct _sFFT2D *pFFT, unsigned Nx, unsigned 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, double **ppReal, double **ppImag)
{
	double *pTempr, *pTempi;
	unsigned i, row;


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

	/* 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, double **ppReal, double **ppImag)
{
	double *pTempr, *pTempi;
	unsigned i, row;


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

	/* 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(double *pDataR, double *pDataI, unsigned N)
{
	double *a, *b;
	double phi, c, s;
	unsigned i, j;

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

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

	for (i = 0; i < N; i++)
	{
		pDataR[i] = 0;
		pDataI[i] = 0;
		
		for (j = 0; j < N; j++)
		{
			phi = 2*i*j*PI/N;
			c =   0.5*cos(phi);
			s =   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(double *pDataR, double *pDataI, unsigned N)
{
	double *a, *b;
	double phi, c, s;
	unsigned i, j;

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

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

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

}