#include <stdio.h>
#include "platform.h"
#include "xil_printf.h"
#include "xgpiops.h"
#include "xparameters.h"

XGpioPs_Config *gpio_config;
XGpioPs gpio;

void init(){
	int status;
	gpio_config = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
	status = XGpioPs_CfgInitialize(&gpio, gpio_config, XPAR_PS7_GPIO_0_BASEADDR);
	if(status == XST_SUCCESS)
		xil_printf("Device Init Successful\n");
	else
		xil_printf("Device Init Failed\n");
}

int main()
{
	u32 sw0 = 0, sw1 = 0,sw2 = 0 ,sw3 = 0;
    init_platform();
    init();

    print("Starting GPIO EMIO Test\n\r");

    /////////Setting 54 55 56 57 as input
    XGpioPs_SetDirectionPin(&gpio, 54, 0);
    XGpioPs_SetDirectionPin(&gpio, 55, 0);
    XGpioPs_SetDirectionPin(&gpio, 56, 0);
    XGpioPs_SetDirectionPin(&gpio, 57, 0);

    /////////Setting 58 59 60 and 61 as Output
    XGpioPs_SetDirectionPin(&gpio, 58, 1);
    XGpioPs_SetDirectionPin(&gpio, 59, 1);
    XGpioPs_SetDirectionPin(&gpio, 60, 1);
    XGpioPs_SetDirectionPin(&gpio, 61, 1);

    XGpioPs_SetOutputEnablePin(&gpio, 58, 1);
    XGpioPs_SetOutputEnablePin(&gpio, 59, 1);
    XGpioPs_SetOutputEnablePin(&gpio, 60, 1);
    XGpioPs_SetOutputEnablePin(&gpio, 61, 1);

    while(1){
    	sw0 = XGpioPs_ReadPin(&gpio,54);
    	sw1 = XGpioPs_ReadPin(&gpio,55);
    	sw2 = XGpioPs_ReadPin(&gpio,56);
    	sw3 = XGpioPs_ReadPin(&gpio,57);
    	XGpioPs_WritePin(&gpio, 58, sw0);
    	XGpioPs_WritePin(&gpio, 59, sw1);
    	XGpioPs_WritePin(&gpio, 60, sw2);
    	XGpioPs_WritePin(&gpio, 61, sw3);
    	xil_printf("Value read sw0 : %0d sw1 : %0d sw2 : %0d sw3 : %0d\n ",sw0,sw1,sw2,sw3);
    	sleep(2);
    }

    cleanup_platform();
    return 0;
}