#include "platform.h"
#include "xbasic_types.h"
#include "xparameters.h"
#include "xil_io.h"
#include "xuartlite.h"
#include "xuartlite_l.h"
#include "microblaze_sleep.h"
#include <stdio.h>
#include "PmodSF3.h"
#include "intc.h"
#include "xil_io.h"
#include "xspi.h"



Xuint32 *baseaddr_p = (Xuint32 *)XPAR_MAIN_VHDL_0_S00_AXI_BASEADDR;

/*****************Initialize Interrupt Driver for PMOD Flash***************/

#ifdef XPAR_INTC_0_DEVICE_ID
#define XPAR_AXI_INTC_0_PMODSF3_0_QSPI_INTERRUPT_INTR
#else
#define INT_ADDR XPAR_FABRIC_PMODSF3_0_QSPI_INTERRUPT_INTR
#endif


PmodSF3 mySF3;
const ivt_t ivt[] = {
		{XPAR_INTC_SINGLE_BASEADDR, (XInterruptHandler) XSpi_InterruptHandler, &mySF3.SF3Spi}
};


/************Declare UART and UART Configuration*************************/

XUartLite UartLite;	/* Instance of the UartLite device */
XUartLite_Config uart_config;//configuration information for the device

/***********Interrupt Handler Function************************/
/***Purpose: Read MUX Code, Read Data from Flash, Write Data to VHDL to Program ASIC**/
void MAIN_INTR_HANDLER(void *baseaddr)
{
	/****Variables within function****/

    u32 Index;
    u32 Address[3];
    Address [0] = 0x000000;
    Address [1] = 0x000100;
    Address [2] = 0x000700;
    u32 mux_data_select;
    int j;

    Xuint32 *baseaddr_p = (Xuint32 *)XPAR_MAIN_VHDL_0_S00_AXI_BASEADDR;
//	xil_printf("interrrupttttted1\r\n"); //debug

/********Main Body of Function***********/
	mux_data_select = *(baseaddr_p+4); //read mux code from VHDL register
	xil_printf("mux_data_select = %d", mux_data_select);
	*(baseaddr_p+0)= 105 ; // toggle data write before writing data to VHDL

/*int Status = SF3_begin(&mySF3, &ivt[0], XPAR_PMODSF3_0_AXI_LITE_SPI_BASEADDR);

/*
//Clear Read buffer
	 for (Index = 0; Index < PAGE_SIZE + READ_WRITE_EXTRA_BYTES; Index++)
	 {
	        	mySF3.ReadBuffer[Index] = 0x0;
	        	  	}
 //Read 256 points from Flash (can make less)
	 SF3_FlashRead(&mySF3, Address[0], PAGE_SIZE, COMMAND_RANDOM_READ);
	    		for (Index = 0; Index < PAGE_SIZE ; Index++) {
	    		   xil_printf("\r\n read val = %d", mySF3.ReadBuffer[Index+READ_WRITE_EXTRA_BYTES]);
	    	  }
 //Write correct data points to VHDL
	 for (j = 0; j < 5 ; j++) {
		 *(baseaddr_p+0) = mySF3.ReadBuffer[(mux_data_select*5)+j];

		 xil_printf("\n\rdata = %d", *(baseaddr_p+8));
		 xil_printf(" transmit val = %d", mySF3.ReadBuffer[(mux_data_select*5)+j+READ_WRITE_EXTRA_BYTES]);
	    }*/

	 //xil_printf("data = %d\r\n", *(baseaddr_p+8));

}


/*****************************Main Function***********************************/
int main()
{
	init_platform();

/****Variables*****/
	XStatus Status;
	Xuint32 baseaddr;
	Xuint32 IpStatus;
	unsigned char verbose = 1;
	u32 BaseAddress = 0x000000;
	u32 ByteCount = 256;
	u8 WriteCmd = 0x02;
	u8 ReadCmd = 0x03;
	u32 Index;
	u32 Address[3];
	Address [0] = 0x000000;
	Address [1] = 0x000100;
	Address [2] = 0x000700;
	u8 offset[3];
	offset[0] = 0x00;
	offset[1] = 0x0a;
	offset[2]= 0x20;
	u32 output_data;

	int status_uart;
	int select_state;
	int x[8];
	int y_val;
	int light_status = 1;
	int i;
	int n;
	int j;
	int l;
	int a = 1;
	int mux_code = 20;// eventually read from verasonics
	int bit;
	int z=0;
	unsigned short number[128];
	unsigned short comp;
	int again = 0;
	unsigned int RS232Recv[5];
	int count=0; //for uart receive from matlab
	int ASIC_controls[2];
	int mux_reset;
	int set_number = 0;
	int mux_code_1;
	int test_val;
	int trig_out_ack;
	int trig_out_status;
	int status_req;
	int tx_en_sent;
	int sets;

/*********Structure for UART Configuration***********/
	    uart_config.RegBaseAddr = XPAR_AXI_UARTLITE_0_BASEADDR;
	    uart_config.DeviceId = XPAR_AXI_UARTLITE_0_DEVICE_ID;
	    uart_config.BaudRate = 9600;
	    uart_config.UseParity = 0;
	    uart_config.ParityOdd = 0;
	    uart_config.DataBits = 8;

//*************Initializae UART*****************/
	   status_uart = XUartLite_Initialize(&UartLite, XPAR_UARTLITE_0_DEVICE_ID);

	   if (status_uart!=XST_SUCCESS){
	   xil_printf("UART failed to initialize. \r\n");
	   return XST_FAILURE;
	     }


	   status_uart = XUartLite_SelfTest(&UartLite);
	   if (status_uart != XST_SUCCESS) {
	   xil_printf("UART failed to self test. \r\n");
	   return XST_FAILURE;
	        }

/***********Enable Interrupts and Register Interrupt Handler*******/

	   //REGISTER INTERRUPT HANDLER
	   xil_printf("  - Registering interrupt handler\r\n");
	   XIntc_RegisterHandler(XPAR_INTC_0_BASEADDR,
	    		   XPAR_AXI_INTC_0_MAIN_VHDL_0_MYINTR_PORT_INTR,
	   			MAIN_INTR_HANDLER,
	        	    (void *)XPAR_MAIN_VHDL_0_S00_AXI_BASEADDR);

	   //ENABLE THE INTERRUPTS
	   xil_printf("  - Enabling interrupts\r\n");
	   XIntc_MasterEnable(XPAR_INTC_0_BASEADDR);
	   XIntc_EnableIntr(XPAR_INTC_0_BASEADDR,
	    		   XPAR_MAIN_VHDL_0_MYINTR_PORT_MASK);

	   // Enable Microblaze interrupts
	   microblaze_enable_interrupts();

/*************Menu of Functions controlled by MATLAB/VERASONICS****************/

	   while(1){
		   /***Select section****/
	   	    select_state = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);
	   	    printf("what is selected?? %d\r\n", select_state);



	   	    switch(select_state) {

	   	   /********Program Data into PMOD Flash********/
	   	    case 1  : //program all the data in
  	   	   	  sets = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);

  	   	   	    for ( i = 0; i <(sets); i++){
  	   	   	    	count = 0;
  	   	   	    	 while(count!=5){

  	   	   	    	    RS232Recv[count] =  XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);
  	   	   	    	    xil_printf("what this = %c\n", RS232Recv[count]);
  	   	   	    	    count +=1;

  	   	   	    		          }

  	   	   	  //Fill in the data that is to be written into the Micron Serial Flash
  	   	   	    for (Index = 4; Index < 9 ; Index++) {
  	   	   		    	mySF3.WriteBuffer[Index+set_number] = (u8) RS232Recv[Index-4] ;
  	   	   			  	}

  	   	   	    set_number +=5;

  	   	   	    }

  	   	   	    SF3_begin(&mySF3, &ivt[0], XPAR_PMODSF3_0_AXI_LITE_SPI_BASEADDR);
  	   	   	    SF3_FlashWriteEnable(&mySF3);
  	   	   	    SF3_SectorErase(&mySF3, Address[0]);
  	   	   	    SF3_WaitForFlashReady(&mySF3);
  	   	   	    SF3_FlashWriteEnable(&mySF3);
  	   	   	    SF3_FlashWrite(&mySF3,Address[0], PAGE_SIZE, COMMAND_PAGE_PROGRAM);
  	   	   	    set_number =0;

  	   	   	    Status = SF3_FlashRead(&mySF3, Address[0], PAGE_SIZE, COMMAND_RANDOM_READ);
  	   	       		for (Index = 0; Index < PAGE_SIZE ; Index++) {
  	   	       		   xil_printf("\r\n read val = %d", mySF3.ReadBuffer[Index+READ_WRITE_EXTRA_BYTES]);
  	   	       	  }

  	   	   	       break ;


  	   	   	/*******Toggle ASIC Controls***********/
	   	    case 2  :  //turn on the leds
	   	    	ASIC_controls[0] = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);

	   	    	if (ASIC_controls[0]!= 116 && ASIC_controls[0]!= 109 && ASIC_controls[0]!= 114){
	   	    		// Write multiplier inputs to register 0
	   	    		*(baseaddr_p+0) = ASIC_controls[0];
	   	    		printf("%d",ASIC_controls[0]);
	   	    		}
	   	    	else {
	   	    		ASIC_controls[1] = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);
	   	    		*(baseaddr_p+0) = ASIC_controls[0];
	   	    		*(baseaddr_p+0) = ASIC_controls[1];
	   	    		printf("%d %d",ASIC_controls[0],ASIC_controls[1]);
	   	    	}
	   	    	break;

/*

	   	    case 3: //reset for new mux code receive
	   	    	mux_reset = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);
	   	    	*(baseaddr_p+0) = 0x15;


	   	    	break; */
/*
	   	    case 4: // indicate y position for mux code (eventually will become obsolete)
	   	    	y_val = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);
	   	    	*(baseaddr_p+0) = 0x79;
	   	    	*(baseaddr_p+0) = y_val;
	   	    	*(baseaddr_p+0) = 0x77;
	   	    	break;
*/

	   	  /********Check Version*********/
	   	    case 5 :
	   		  XUartLite_SendByte(XPAR_UARTLITE_0_BASEADDR,1);
	   		*(baseaddr_p+0) = 0x2b; //turn off red led

	   	    	break;

	   	  /******Verify TX_OUT**********/
	   	    case 6: //Verify TX_Trig
	   	    	*(baseaddr_p+0) = 0x67;
	   	    	trig_out_ack = *(baseaddr_p+1);
	   	    	xil_printf("trig_out_ack = %d\r\n", trig_out_ack);

	   	    	if (trig_out_ack == 65) {
	   	    		XUartLite_SendByte(XPAR_UARTLITE_0_BASEADDR,trig_out_ack);
	   	    	}
	   	    	else {
	   	    		XUartLite_SendByte(XPAR_UARTLITE_0_BASEADDR,0x0d);
	   	    	}

	   	    	status_req = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);
	   	    	if (status_req == 10){
	   	    		trig_out_status = *(baseaddr_p+1);
	   	    		XUartLite_SendByte(XPAR_UARTLITE_0_BASEADDR,trig_out_status);
	   	    	}
	   	    	break;

	   	   /********Verify TX_EN*************/
	   	    case 7: //Verify TX_EN
	   	    	*(baseaddr_p+0) = 0x58; //X

	   	    	status_req = XUartLite_RecvByte(XPAR_UARTLITE_0_BASEADDR);
	   	    	 if (status_req != 20){
	   	    		*(baseaddr_p+0) = 0x3f;
	   	    	 }

	   	    	break;

	   	  /************Imaging****************/
	   	    case 49: //Imaging
	   	    	*(baseaddr_p+0) = 0x30; //ask mux to hold



	   	    	break;
	   	    }
	   }
	   cleanup_platform();

return 0;
}