#include <stdio.h>
#include "platform.h"
#include "xparameters.h"
#include "xparameters.h"
#include "xstatus.h"
#include "../../hello_world_bsp_0/microblaze_0/include/fsl.h"
#include "xiic.h"
#include "xiic_l.h"
#include "xio.h"

#define AD_SLAVE_ADDR 0x28

void print(char *str);

int main()
{
    init_platform();

    print("In main, rev 1\n\r");

    int imag, real, send;
    int recv;
    int indata[256];
    int outdata[16];
	int i;

    print("init indata");


    for (i=0; i<256; ++i) {
    	indata[i] = 0;
    }

    print(", done\r\n");

    print("init sample");

    u8 buf[512];
    u8 writeBuf = 0x10;

    // Configure ADC
    XIic_Send(XPAR_AXI_IIC_0_BASEADDR,AD_SLAVE_ADDR,&writeBuf,1,0);

    print(", done\r\n");

    while (1) {
       int i;
       // Send computed values to VGA
       for (i=0; i<16; ++i) {
    	   int temp = outdata[(15-i)] >> 24;
    	   send = outdata[(15-i)] << 8;
    	   send += temp;
    	   putfslx(send, 0, FSL_DEFAULT);
	   }

	   // Read new input data
	   XIic_Recv(XPAR_AXI_IIC_0_BASEADDR,AD_SLAVE_ADDR,buf,512,0);

	   for(i = 0; i < 512; i = i + 2){
		   indata[i/2] = (buf[i] << 8) + buf[i+1] -2048;
	   }

	   // Window Function
	   int wind_len = 40;
	   for(i = 0; i < wind_len; ++i){
		   indata[i] /= (wind_len - i);
	   }

	   for(i = 255; i > (255 - wind_len); --i){
		   indata[i] /= (i - (255 - wind_len));
	   }

	   // Send data to FFT
	   for (i=0; i<256; ++i) {
		   send = indata[i] << 22;
		   putfslx(send, 1, FSL_DEFAULT);
	   }

	   //Read data from FFT
	   for(i = 0; i < 16; ++i){
		   outdata[i] = 0;
	   }

	   for(i=0; i<128; ++i) {
		   getfslx(recv, 1, FSL_DEFAULT);
		   real = recv >> 16;
		   imag = (recv << 16) >> 16;

		   int temp = (real*real+imag*imag);
		   int t2 = temp >> 15;
		   t2 = t2>15? 15: t2;
		   outdata[i/8] += (t2 << ((7-(i%8))*4));
		}
	}
	return 0;
}
