/*************************************************************************
 * This code is written by N. Kehtarnavaz and N. Kim as part of the 
 * textbook "Digital Signal Processing System-Level Design Using LabVIEW".
 *************************************************************************/

#include <math.h>    
#include "twiddleR.h"
#include "twiddleI.h" 

#include <rtdx.h>                      /* RTDX                      */
#include "target.h"                    /* TARGET_INITIALIZE()       */

#define 	kBUFFER_SIZE 128
#define		NUMDATA		128 /* number of real data samples */
#define		NUMPOINTS	64  /* number of point in the DFT, NUMDATA/2  */

#define		TRUE		1
#define		FALSE		0
#define		BE		TRUE
#define		LE		FALSE
#define		ENDIAN		LE	/* selects proper endianaess.  If 
					   buliding code in Big Endian, use BE, else use LE */

#define		PI		3.141592653589793 /* defineition of pi */

typedef struct { /* define the data type for the radix-4 twiddle factors */
short imag;
short real;
} COEFF;

/* BIG Endian */
#if ENDIAN == TRUE

typedef struct {
		short imag;
		short real;
		} COMPLEX;

#else

/* LITTLE Endian */
typedef struct {
	short real;
	short imag;
	} COMPLEX;

#endif

#pragma DATA_ALIGN(x,NUMPOINTS);

COMPLEX x[NUMPOINTS+1]; 	/* array of complex DFT data */
COEFF W4[NUMPOINTS];
short g[NUMDATA];
COMPLEX A[NUMPOINTS]; 		/* array of complex A coefficients */
COMPLEX B[NUMPOINTS]; 		/* array of complex B coefficients */
COMPLEX G[2*NUMPOINTS]; 	/* array of complex DFT result */
unsigned short IIndex[NUMPOINTS], JIndex[NUMPOINTS]; 
int count;

int magR[NUMDATA]; 
int magI[NUMDATA];

int output[kBUFFER_SIZE];

void make_q15(short out[], float in[], int N);
void R4DigitRevIndexTableGen(int, int *, unsigned short *, unsigned short *);
void split1(int, COMPLEX *, COMPLEX *, COMPLEX *, COMPLEX *);
void digit_reverse(int *, unsigned short *, unsigned short *, int);
void radix4(int, short[], short[]);
void fft();

RTDX_CreateInputChannel(cinput);
RTDX_CreateOutputChannel(coutput);

void main()
{	
	int i,k;
    short tr[NUMPOINTS], ti[NUMPOINTS]; 
	
	// Target initialization for RTDX
	TARGET_INITIALIZE();

	/*enable RTDX channels*/
	RTDX_enableInput(&cinput);
	RTDX_enableOutput(&coutput);

	  
    //Read Twiddle factors to COMPLEX array and make Q-15;
    make_q15(tr, TR, NUMPOINTS);  //Data in Header files from Matlab
    make_q15(ti, TI, NUMPOINTS);           
               
    for(i=0;i<NUMPOINTS;i++)
    {
    	W4[i].real = tr[i];
    	W4[i].imag = ti[i];
    }    
    
	/* Initialize A,B, IA, and IB arrays */
	for(k=0; k<NUMPOINTS; k++)
	{
		A[k].imag = (short)(16383.0*(-cos(2*PI/(double)(2*NUMPOINTS)*(double)k)));
		A[k].real = (short)(16383.0*(1.0 - sin(2*PI/(double)(2*NUMPOINTS)*(double)k)));
		B[k].imag = (short)(16383.0*(cos(2*PI/(double)(2*NUMPOINTS)*(double)k)));
		B[k].real = (short)(16383.0*(1.0 + sin(2*PI/(double)(2*NUMPOINTS)*(double)k)));
	}
	
	/* Initialize tables for FFT digit reversal function */
	R4DigitRevIndexTableGen(NUMPOINTS, &count, IIndex, JIndex);
	
	for (;;)	/* Infinite message loop. */
	{
		while(!RTDX_read(&cinput, g, sizeof(g)));
	
		/* Call FFT algorithm */        
		fft();
		
		for (k=0; k<NUMDATA; k++)
		{
			magR[k] = (G[k].real*G[k].real) << 1;
			magI[k] = (G[k].imag*G[k].imag) << 1;
			
			output[k] = magR[k] + magI[k];
		}      
		
		/* Write scaled data back to host. */
		RTDX_write(&coutput, &output, sizeof(output));
	}
}
	
void fft()
{       
	int n;
	/* Forward DFT */
	/* From the 2N point real sequence, g(n), for the N-point complex sequence, x(n) */

	for (n=0; n<NUMPOINTS; n++)
	{
		x[n].imag = g[2*n + 1]; /* x2(n) = g(2n + 1) */
		x[n].real = g[2*n]; /* x1(n) = g(2n) */
	}

	/* Compute the DFT of x(n) to get X(k) -> X(k) = DFT{x(n)} */
	radix4(NUMPOINTS, (short *)x, (short *)W4);

	digit_reverse((int *)x, IIndex, JIndex, count);
	/* Because of the periodicity property of the DFT, we know that X(N+k)=X(k). */
	x[NUMPOINTS].real = x[0].real;
	x[NUMPOINTS].imag = x[0].imag;
	/* The split function performs the additional computations required to get
	G(k) from X(k). */

	split1(NUMPOINTS, x, A, B, G);
	/* Use complex conjugate symmetry properties to get the rest of G(k) */
	G[NUMPOINTS].real = x[0].real - x[0].imag;
	G[NUMPOINTS].imag = 0;
	
	for (n=1; n<NUMPOINTS; n++)
	{
		G[2*NUMPOINTS-n].real = G[n].real;
		G[2*NUMPOINTS-n].imag = -G[n].imag;
	}

}

void radix4(int n, short x[], short w[])
{
	int n1, n2, ie, ia1, ia2, ia3, i0, i1, i2, i3, j, k;
	short t, r1, r2, s1, s2, co1, co2, co3, si1, si2, si3;
	n2 = n;
	ie = 1;
	for (k = n; k > 1; k >>= 2) 
	{
		n1 = n2;
		n2 >>= 2;
		ia1 = 0;
		for (j = 0; j < n2; j++) 
		{
			ia2 = ia1 + ia1;
			ia3 = ia2 + ia1;
			co1 = w[ia1 * 2 + 1];
			si1 = w[ia1 * 2];
			co2 = w[ia2 * 2 + 1];
			si2 = w[ia2 * 2];
			co3 = w[ia3 * 2 + 1];
			si3 = w[ia3 * 2];
			ia1 = ia1 + ie;
			for (i0 = j; i0 < n; i0 += n1) 
			{
				i1 = i0 + n2;
				i2 = i1 + n2;
				i3 = i2 + n2;
				r1 = x[2 * i0] + x[2 * i2];
				r2 = x[2 * i0] - x[2 * i2];
				t = x[2 * i1] + x[2 * i3];
				x[2 * i0] = r1 + t;
				r1 = r1 - t;
				s1 = x[2 * i0 + 1] + x[2 * i2 + 1];
				s2 = x[2 * i0 + 1] - x[2 * i2 + 1];
				t = x[2 * i1 + 1] + x[2 * i3 + 1];
				x[2 * i0 + 1] = s1 + t;
				s1 = s1 - t;
				x[2 * i2] = (r1 * co2 + s1 * si2) >> 15;
				x[2 * i2 + 1] = (s1 * co2-r1 * si2)>>15;
				t = x[2 * i1 + 1] - x[2 * i3 + 1];
				r1 = r2 + t;
				r2 = r2 - t;
				t = x[2 * i1] - x[2 * i3];
				s1 = s2 - t;
				s2 = s2 + t;
				x[2 * i1] = (r1 * co1 + s1 * si1) >>15;
				x[2 * i1 + 1] = (s1 * co1-r1 * si1)>>15;
				x[2 * i3] = (r2 * co3 + s2 * si3) >>15;
				x[2 * i3 + 1] = (s2 * co3-r2 * si3)>>15;
			}
		}
		ie <<= 2;
	}
}

void digit_reverse(int *yx, unsigned short *JIndex, unsigned short *IIndex, int count)
{
	int i;
	unsigned short I, J;
	int YXI, YXJ;
	for (i = 0; i<count; i++)
	{
		I = IIndex[i];
		J = JIndex[i];
		YXI = yx[I];
		YXJ = yx[J];
		yx[J] = YXI;
		yx[I] = YXJ;
	}
}

void R4DigitRevIndexTableGen(int n, int *count, unsigned short *IIndex, unsigned short *JIndex)
{
	int j, n1, k, i;
	j = 1;
	n1 = n - 1;
	*count = 0;
	for(i=1; i<=n1; i++)
	{
		if(i < j)
		{
			IIndex[*count] = (unsigned short)(i-1);
			JIndex[*count] = (unsigned short)(j-1);
			*count = *count + 1;
		}
		k = n >> 2;
		while(k*3 < j)
		{
			j = j - k*3;
			k = k >> 2;
		}
		j = j + k;
	}
}

void split1(int N, COMPLEX *X, COMPLEX *A, COMPLEX *B, COMPLEX *G)
{														  
	int k;
 	int Tr, Ti;

	for (k=0; k<N; k++)
	{
		Tr = (int)X[k].real * (int)A[k].real - (int)X[k].imag * (int)A[k].imag +
			(int)X[N-k].real * (int)B[k].real + (int)X[N-k].imag * (int)B[k].imag;
		G[k].real = (short)(Tr>>15);
					
		Ti = (int)X[k].imag * (int)A[k].real + (int)X[k].real * (int)A[k].imag +
			(int)X[N-k].real * (int)B[k].imag - (int)X[N-k].imag * (int)B[k].real;
		G[k].imag = (short)(Ti>>15);  					   
	}
}

void make_q15(short out[], float in[], int N)
{
	int i;
	
	for(i=0;i<N;i++)
	{
		out[i]=0x7fff*in[i];  //Convert to Q-15, good approximate
	}
}
