#include <stdio.h>
#include "platform.h"
#include "xil_printf.h"
#include <xparameters.h>
#include "xgpio.h"
#include "xscugic.h"
#include "xscutimer.h"
#include "sleep.h"

//GPIO definitions
static  XGpio 					PL_REGS_Instance;
#define PL_REGS_DEVICE_ID		XPAR_OL772_PL_REGISTERS_DEVICE_ID
#define PL_REGS_ADDR_DATA0		XPAR_OL772_PL_REGISTERS_BASEADDR
#define PL_REGS_ADDR_DATA1		XPAR_OL772_PL_REGISTERS_BASEADDR+8

static  XGpio 					PL_DATA_Instance;
#define PL_DATA_DEVICE_ID		XPAR_OL772_PL_DATA_DEVICE_ID
#define PL_DATA_ADDR_DATA0		XPAR_OL772_PL_DATA_BASEADDR

//Interrupt definitions
//static 	XScuGic 				IntcInstance;
//static 	XScuGic_Config 			*IntcConfig;
//static 	XScuTimer 				PrivateTimerInstancePtr;
//static 	XScuTimer_Config 		*PrivateTimerConfig;
XScuGic 				IntcInstance;
//XScuGic_Config 			*IntcConfig;
XScuTimer 				PrivateTimerInstancePtr;
//XScuTimer_Config 		*PrivateTimerConfig;

static u32 PrivateTimerIntrCnt = 0;
static u32 PLIntrCnt = 0;

const u32 OL772PL_Regs_Address_Address 	= 	XPAR_OL772_PL_REGISTERS_BASEADDR;
const u32 OL772PL_Regs_Data_Address 	=  	XPAR_OL772_PL_REGISTERS_BASEADDR+8;
const u32 OL772PL_Data_Address_Address 	= 	XPAR_OL772_PL_DATA_BASEADDR;
const u32 OL772PL_Data_Data_Address 	=  	XPAR_OL772_PL_DATA_BASEADDR;
const u16 CtlStrobe	=		0x0400;
const u16 CtlWE	=			0x0200;
const u16 CtlRE	=			0x0100;

// function prototypes
//void Write_creg (unsigned int Address, unsigned int Data);
void Write_creg (u16 Address, u32 Data);	// Write_creg takes ~1.9us
u32 Read_creg (u16 Address);				// Read_creg takes ~1.9us
u32 Read_dreg (u16 Address);				// Read_dreg takes ~1.9us
// printf takes ~ 1.1ms!!
static void PLIntrHandler(void *CallBackRef, u32 Mask);
static void PrivateTimerIntrHandler(void *CallBackRef, u32 Mask);
XStatus Initialize(void);

int main()
{
XStatus Status;
u32 ReadData;

	Status = Initialize();
	if (Status != XST_SUCCESS)
		printf("Initialization Failed!!!");



    // Set Simulation mode
    Write_creg (0x0001, 0x00000020);	// set bit 5 = sim_data
//    Write_creg (0x0001, 0x00000030);	// set bit 4,5 = width, sim_data
    ReadData=Read_creg(0x0001);			//SystemCon
    printf("SystemCon=%08lX\n", ReadData);


    // Set Exposure Time
//    Write_creg (0x0003, 0x00000064);    //Exposure = 100us
//    Write_creg (0x0003, 0x00004E20);    //Exposure = 20ms
    Write_creg (0x0003, 0x00002710);    //Exposure = 10ms
    ReadData=Read_creg(0x0003);
    printf("Exposure=%08lX\n", ReadData);

    // continuous meas
//    Write_creg (0x0010, 0x00000D30);    //MeasCon = start measdone, continuous, dark every, autoexp every
    Write_creg (0x0010, 0x00000D00);    //MeasCon = start measdone, continuous, dark every, autoexp none
    ReadData=Read_creg(0x0010);
    printf("MeasCon=%08lX\n\r", ReadData);

    // MeasCon = set meas_start_sw
    Write_creg (0x0010, 0x08000D00);    //MeasCon = set meas_start_sw
    ReadData=Read_creg(0x0010);
    printf("MeasCon=%08lX\n", ReadData);

    // while loop
    while(1)
    {
/*    	// Toggle an LED on the KR module in response to PLIntrCnt
    	ReadData=Read_creg(0x0081);
    	ReadData &= 0x7FFFFFFF;
    	if(PLIntrCnt & 0x00000001)
    		ReadData |= 0x80000000;
       	Write_creg (0x0081, ReadData);
*/
    }

    cleanup_platform();
    return 0;
}


static void PrivateTimerIntrHandler(void *CallBackRef, u32 Mask)
{

	PrivateTimerIntrCnt++;

/*
	u32 ReadData;
 	// Toggle an LED on the KR module
	ReadData=Read_creg(0x0081);
    if(ReadData & 0x40000000)
    	Write_creg (0x0081, ReadData & 0xBFFFFFFF);
    else
    	Write_creg (0x0081, ReadData | 0x40000000);
*/
}

static void PLIntrHandler(void *CallBackRef, u32 Mask)
{
	PLIntrCnt++;

	u32 ReadData;
	// Toggle an LED on the KR module
	ReadData=Read_creg(0x0081);
    if(ReadData & 0x80000000)
    	Write_creg (0x0081, ReadData & 0x7FFFFFFF);
    else
    	Write_creg (0x0081, ReadData | 0x80000000);


   	while((Read_dreg(0x007F) & 0x00000002)==0) {Read_dreg(0x0030);}	//Check empty flag and read out DarkOutFIFO
   	while((Read_dreg(0x007F) & 0x00000008)==0) {Read_dreg(0x0031);}	//Check empty flag and read out RawOutFIFO
   	while((Read_dreg(0x007F) & 0x00000020)==0) {Read_dreg(0x0032);}	//Check empty flag and read out CalOutFIFO

/*
     	//Check and read out RawOutFIFO
       	while((Read_dreg(0x007F) & 0x00000008)==0)
       	{
       		Read_dreg(0x0031);
       	}
       	//Check and read out DarkOutFIFO and maybe CalOutFIFO
       	while((Read_dreg(0x007F) & 0x00000002)==0)
       	{
       		Read_dreg(0x0030);
           	if((Read_dreg(0x007F) & 0x00000020)==0)
           		Read_dreg(0x0032);
       	}
       	//Check and read out CalOutFIFO and maybe DarkOutFIFO
       	while((Read_dreg(0x007F) & 0x00000020)==0)
       	{
       		Read_dreg(0x0032);
           	if((Read_dreg(0x007F) & 0x00000002)==0)
           		Read_dreg(0x0030);
       	}
*/
}

///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
// Do all Initialization here
XStatus Initialize (void)
{
XStatus InitStatus;
XScuGic_Config 			*IntcConfig;
XScuTimer_Config 		*PrivateTimerConfig;

	///////////////////////////////////////////////////////////////////////////
	// Initialize the Cortex platform
	init_platform();

	///////////////////////////////////////////////////////////////////////////
	// Initialize the GPIO IP modules
	InitStatus = XGpio_Initialize(&PL_REGS_Instance, PL_REGS_DEVICE_ID);
	if (InitStatus != XST_SUCCESS)	return XST_FAILURE;
	InitStatus = XGpio_Initialize(&PL_DATA_Instance, PL_DATA_DEVICE_ID);
	if (InitStatus != XST_SUCCESS)	return XST_FAILURE;


	///////////////////////////////////////////////////////////////////////////
	// Initialize IRQ Interrupt
	//	Xil_ExceptionInit(); this is an empty function leftover for backwards compatibility

	// Initialize the interrupt controller driver (IntcConfig is static global)
	IntcConfig = XScuGic_LookupConfig(XPAR_SCUGIC_SINGLE_DEVICE_ID);
	if (NULL == IntcConfig)	return XST_FAILURE;

	InitStatus = XScuGic_CfgInitialize(&IntcInstance, IntcConfig, IntcConfig->CpuBaseAddress);
	if (InitStatus != XST_SUCCESS)	return XST_FAILURE;

	// Connect the IRQ ISR handler to hardware
	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_IRQ_INT,
		(Xil_ExceptionHandler)XScuGic_InterruptHandler, &IntcInstance);

	// Enable IRQ exception in the CPU
	Xil_ExceptionEnableMask(XIL_EXCEPTION_IRQ);

	// Connect a handler function to the IRQ interrupt
	// Use XPS_FPGA0_INT_ID instead of XPS_IRQ_INT_ID for programmable sensitivity
	InitStatus = XScuGic_Connect(&IntcInstance,XPS_FPGA0_INT_ID,
			(Xil_ExceptionHandler)PLIntrHandler,
			(void *)&IntcInstance);
	if (InitStatus != XST_SUCCESS)	return XST_FAILURE;

	// Enable the IRQ_F2P
	XScuGic_Enable(&IntcInstance, XPS_FPGA0_INT_ID);

	// Enable IRQ exception
//same as Xil_ExceptionEnableMask	Xil_ExceptionEnable();




	// Set interrupt priority (instance, port=XPS_FPGA0_INT_ID, level=xa0/8=1, trigger type=rising edge
	XScuGic_SetPriorityTriggerType(&IntcInstance, XPS_FPGA0_INT_ID, 0x08, 3);

/*
	///////////////////////////////////////////////////////////////////////////
	// Initialize Private Timer and Interrupt

	// define the period = TIMER_LOAD_VALUE / 333.333333MHz
	//TIMER_LOAD_VALUE = period(s) * 333.333333MHz
	#define TIMER_LOAD_VALUE 0x01FCA055		// 0.1s

	// Initialize Private Timer
	PrivateTimerConfig = XScuTimer_LookupConfig(XPAR_XSCUTIMER_0_DEVICE_ID);
	if (NULL == PrivateTimerConfig)	return XST_FAILURE;

	InitStatus = XScuTimer_CfgInitialize(&PrivateTimerInstancePtr, PrivateTimerConfig, PrivateTimerConfig->BaseAddr);
	if (InitStatus != XST_SUCCESS)	return XST_FAILURE;

	// Load, Enable reload, enable int, start
	XScuTimer_EnableAutoReload(&PrivateTimerInstancePtr);
	XScuTimer_LoadTimer(&PrivateTimerInstancePtr, TIMER_LOAD_VALUE);
	XScuTimer_SetPrescaler(&PrivateTimerInstancePtr, 1);
	XScuTimer_EnableInterrupt(&PrivateTimerInstancePtr);
	XScuTimer_Start(&PrivateTimerInstancePtr);


	// Connect a handler function to the Private Timer interrupt
	InitStatus = XScuGic_Connect(&IntcInstance,XPS_SCU_TMR_INT_ID,
		(Xil_ExceptionHandler)PrivateTimerIntrHandler,
		(void *)&IntcInstance);
	if (InitStatus != XST_SUCCESS)	return XST_FAILURE;

	// Enable the Private Timer interrupt
	XScuGic_Enable(&IntcInstance, XPS_SCU_TMR_INT_ID);
*/


	///////////////////////////////////////////////////////////////////////////
	// Initialize the FPGA PL
    Write_creg (0x0001, 0x00008000);	// rising_edge triggered, self-clearing pl_rst

    return XST_SUCCESS;
}


void Write_creg (u16 Address, u32 Data)
{
	//OL772PLR_Data
//not tri-stated	XGpio_SetDataDirection(&PL_REGS_Instance, 2, 0); // Set ch2=data of pl_Regs to all outputs
	XGpio_DiscreteWrite(&PL_REGS_Instance, 2, Data);
	//OL772PLR_Address
	XGpio_DiscreteWrite(&PL_REGS_Instance, 1, Address | CtlWE);
	XGpio_DiscreteWrite(&PL_REGS_Instance, 1, Address | CtlWE | CtlStrobe);
	XGpio_DiscreteWrite(&PL_REGS_Instance, 1, Address | CtlWE);
}

u32 Read_creg (u16 Address)
{
	//OL772PL_Regs_Address
	XGpio_DiscreteWrite(&PL_REGS_Instance, 1, Address | CtlRE);
	XGpio_DiscreteWrite(&PL_REGS_Instance, 1, Address | CtlRE | CtlStrobe);
	XGpio_DiscreteWrite(&PL_REGS_Instance, 1, Address | CtlRE);
	//OL772PL_Regs_Data
	return(XGpio_DiscreteRead(&PL_REGS_Instance, 2));
}

u32 Read_dreg (u16 Address)
{
	//OL772PL_Data_Address
	XGpio_DiscreteWrite(&PL_DATA_Instance, 1, Address | CtlRE);
	XGpio_DiscreteWrite(&PL_DATA_Instance, 1, Address | CtlRE | CtlStrobe);
	XGpio_DiscreteWrite(&PL_DATA_Instance, 1, Address | CtlRE);
	//OL772PL_Data_Data
	return(XGpio_DiscreteRead(&PL_DATA_Instance, 1));
}