#include "TSLinkDrv.h"
#include <stdio.h>
#include <sysreg.h>
#include "common.h"
#include <filter.h> 
#include <math.h>

int buffer[1024];
int count = 0;

complex_float data[NDATA];   /* data for a 256-point FFT */ 
complex_float output[NDATA]; 


static complex_float twidtab[(3*NDATA)/4]; 


/* note that the temporary buffer should be as large as 
   the largest FFT generated */ 
complex_float temp[NDATA]; 
float real[NDATA];


void main(void)
{
	int i;
	int* pAddr;
	float MaxData;
	unsigned int MaxIndex=0;
	
	__builtin_sysreg_write(__SYSCON, 0x1A79E7);
	__builtin_sysreg_write(__SDRCON, 0x5213);
	LinkInit(1, LINK_SPEED);
	LinkInit(3, LINK_SPEED);

	
	for(i = 0; i < NDATA; i++)
	{
		data[i].re=sin(2*PI*i*FC/FS);
		data[i].im=0;
	}
	twidfft (twidtab,NDATA); 
	
	
/*	pAddr = (int*)0x4000000;
	for(i = 0; i < 0x1000000; i++)
		*(pAddr + i) = 0x4000000 + i;
*/			
//	for(i = 0; i < 1024; i++)
//		buffer[i] = *(pAddr + i);
//	LinkSendDMAW(1, (void*)buffer, 1024);
//	LinkSend2DDMAW(1, (void*)0x4000000, 0x1000, 0x1000);
	
//	printf("send done.\n");

	MakeBuffer(buffer);
	printf("Test start\n");
	while(1)
	{
		LinkSendDMAW(1, (void*)buffer, 1024);
		LinkRecvDMAW(3, (void*)buffer, 1024);
		count++;
/*		if(count % 100000 == 99999)
		{
			if(CheckBuffer(buffer))
				printf("%d, OK\n", count + 1);
			else
				printf("%d, error\n", count + 1);
		}*/
		if(!CheckBuffer(buffer))
		{
			printf("%d\n", count);
			break;
		}
		
		cfft (data, temp, output, twidtab, 1, NDATA); 
		
		for(i = 0; i < NDATA; i++)
		{
			real[i] = output[i].re * output[i].re + output[i].im * output[i].im;
		}
		
		MaxData= 0;
		MaxIndex=0;
		for(i = 0; i < NDATA / 2; i++)
		{
			if(real[i] > MaxData)
			{
				MaxIndex = i;
				MaxData = real[i];
			}
		}
		
		if(MaxIndex != 0x0a)
		{
			printf("dsp1 fft error\n");
			break;
		}
		
		if(count % 1000000 == 999999)
			printf("%d OK, about %ds\n", count + 1, (count + 1)/556);
	}

	printf("DSP1 failed\n");

}