/*
 * Copyright (c) 2009 Xilinx, Inc.  All rights reserved.
 *
 * Xilinx, Inc.
 * XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A
 * COURTESY TO YOU.  BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS
 * ONE POSSIBLE   IMPLEMENTATION OF THIS FEATURE, APPLICATION OR
 * STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION
 * IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE
 * FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION.
 * XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO
 * THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO
 * ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE
 * FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 */

/*
 * helloworld.c: simple test application
 */

#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 "xtmrctr.h"


//#include "iic_header.h"
#include "xiic.h"

#include "xiic_l.h"
#include "xio.h"

#define CAM_SLAVE_ADDR 0x28

void print(char *str);

int main()
{
    init_platform();

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


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

    print("init indata");

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

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

    print("init sample");

    u16 sr;
    u8 buf[512];
    u8 writeBuf = 0x10;
    //sr = CameraReadNRegs(0,buf,20);
    sr = XIic_Send(XPAR_AXI_IIC_0_BASEADDR,CAM_SLAVE_ADDR,&writeBuf,1,0);

    print(", done\r\n");
    xil_printf("%d %d %d \n\r",sr,buf[0],buf[1]);



    while (1) {
		//print("begining of loop");
    	int i;
		for (i=0; i<16; ++i) {

			int temp = outdata[(15-i)] >> 24;
			send = outdata[(15-i)] << 8;
			send += temp;


			putfslx(send, 0, FSL_DEFAULT);
			//xil_printf("sent: %d\n\r", send);
		}

	   sr = XIic_Recv(XPAR_AXI_IIC_0_BASEADDR,CAM_SLAVE_ADDR,buf,512,0);

	   for(i = 0; i < 512; i = i + 2){
		   indata[i/2] = (buf[i] << 8) + buf[i+1] -2048;
		   //xil_printf("Status %d:: Value:: %d \n\r",sr,h);
		   //xil_printf("%d \n\r",h);
	   }

	   for(i = 0; i < 20; ++i){
		   indata[i] /= (20 - i);
	   }

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

//		print("step1 of loop");

		for (i=0; i<256; ++i) {
			send = indata[i] << 16;
			putfslx(send, 1, FSL_DEFAULT);
//			xil_printf("sent(%d): %d\n\r", i, send);
		}
//		print("step2 of loop");

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

			if (i%8 == 0) { outdata[i/8] = 0; indata[i/8] = 0; }

			int temp = (real*real+imag*imag);
			int t2 = temp >> 15;
			t2 = t2>15? 15: t2;
			outdata[i/8] += (t2 << ((7-(i%8))*4));
			indata[i/8] += (t2 << ((7-(i%8))*4));
			//xil_printf("got(%d): %d + %d i; became: abs: %d, shift: %d\n\r", i, real, imag, temp, t2);
//			if (i%8 == 7) {
//				xil_printf("sum(%d): %x\r\n", i, outdata[i/8]);
//			}
		}
/*
		int t;
		for(i = 0; i < 16; ++i){

			t = outdata[i] & 0xF0F0F0F0 >> 4;
			outdata[i] = (outdata[i] & 0x0F0F0F0F << 4) | t;
			outdata[i] = 0;
		}
		outdata[8] = 0x00000F00;*/
		//print("end of loop");
    }
    return 0;


}
