#include <stdio.h>
#include "platform.h"

#include "xparameters.h"
#include "xtmrctr.h"
#include  "xscugic.h"
#include  "xil_exception.h"
#include  <xil_io.h>

#define INTC_DEVICE_ID XPAR_PS7_SCUGIC_0_DEVICE_ID
#define TMR_DEVICE_ID XPAR_TMRCTR_0_DEVICE_ID
#define INTC_TMR_INTERRUPT_ID XPAR_FABRIC_AXI_TIMER_0_INTERRUPT_INTR

#define TMR_LOAD 0xFFFFEC77 //Prescaler for 20 KHz frequency of overflow

#define STEP_DATA_LENGTH 40000
#define WAIT_TIME 40000

XScuGic INTCInst;
XTmrCtr TMRInst;

static long int interrupt_count;
static int step_data[STEP_DATA_LENGTH];

static void TMR_Intr_Handler (void *basearrdr_p);
static int IntcInitFunction(u16 DeviceId, XTmrCtr *TmrInstancePtr);

void TMR_Intr_Handler(void *data){			//Interrupt handler
	if(XTmrCtr_IsExpired(&TMRInst,0)){
			XTmrCtr_Stop(&TMRInst,0);
			if (interrupt_count<WAIT_TIME+STEP_DATA_LENGTH){		//Fill array with values
				if (interrupt_count>=WAIT_TIME){
					step_data[interrupt_count-WAIT_TIME]=Xil_In32(0x43C60004);   //Reed from AXI register of Encoder component
				}
				interrupt_count++;
			}
			XTmrCtr_Reset(&TMRInst,0);
			XTmrCtr_Start(&TMRInst,0);
	}
}

int InterruptSystemSetup(XScuGic *XScuGicInstancePtr){				//Interrupt settings
	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT, (Xil_ExceptionHandler) XScuGic_InterruptHandler, XScuGicInstancePtr);
	Xil_ExceptionEnable();

	return XST_SUCCESS;
}

int IntcInitFunction(u16 DeviceId, XTmrCtr *TmrInstancePtr){		//Interrupt initiate
	XScuGic_Config *IntcConfig;
	int status;

	IntcConfig = XScuGic_LookupConfig(DeviceId);
	status = XScuGic_CfgInitialize(&INTCInst,IntcConfig,IntcConfig->CpuBaseAddress);
	if (status!=XST_SUCCESS)
		return XST_FAILURE;

	status = InterruptSystemSetup(&INTCInst);
	if (status!=XST_SUCCESS)
		return XST_FAILURE;

	status = XScuGic_Connect(&INTCInst,INTC_TMR_INTERRUPT_ID,(Xil_ExceptionHandler) TMR_Intr_Handler, (void*) TmrInstancePtr);
	if (status!=XST_SUCCESS)
		return XST_FAILURE;
	XScuGic_Enable(&INTCInst,INTC_TMR_INTERRUPT_ID);

	return XST_SUCCESS;
}

int main()
{
	init_platform();

	    int status,i;

	    xil_printf("Initializing Interrupt...\n");
	    status = IntcInitFunction(INTC_DEVICE_ID, &TMRInst);
	    if (status!=XST_SUCCESS){
	    	xil_printf("Failed Initiate Interrupt");
	    	return XST_FAILURE;
	    }

	    xil_printf("Initializing Timer...\n");
	    status = XTmrCtr_Initialize(&TMRInst, TMR_DEVICE_ID);
	    if (status!=XST_SUCCESS){
	    	xil_printf("Failed Initiate Timer");
	    	return XST_FAILURE;
	    }


	    XTmrCtr_SetHandler(&TMRInst, (XTmrCtr_Handler) TMR_Intr_Handler, &TMRInst);
	    XTmrCtr_SetResetValue(&TMRInst, 0, TMR_LOAD);
	    XTmrCtr_SetOptions(&TMRInst,0,XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION);
	    XTmrCtr_Start(&TMRInst,0);

	    xil_printf("Done!\n");
	    interrupt_count = 0;
	    while(1){

	    	if (interrupt_count==STEP_DATA_LENGTH+WAIT_TIME){	//Send measured data to UART
	    		interrupt_count++;
	    		for (i=0;i<STEP_DATA_LENGTH;i++)
	    			xil_printf("%d\n",step_data[i]);

	    	}
	    }

	    cleanup_platform();
	    return 0;
}
