#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <sys/mman.h>

#include "led.h"


struct LED_State all_led[LED_NUM] = 
{{SYS_RED_MPP , SYS_RED_MPP_FLD , SYS_RED_MPP_VAL , SYS_RED_MPP_PIN} ,
 {SYS_GRN_MPP , SYS_GRN_MPP_FLD , SYS_GRN_MPP_VAL , SYS_GRN_MPP_PIN} ,
 {HD1_RED_MPP , HD1_RED_MPP_FLD , HD1_RED_MPP_VAL , HD1_RED_MPP_PIN} ,
 {HD1_GRN_MPP , HD1_GRN_MPP_FLD , HD1_GRN_MPP_VAL , HD1_GRN_MPP_PIN} ,
 {HD2_RED_MPP , HD2_RED_MPP_FLD , HD2_RED_MPP_VAL , HD2_RED_MPP_PIN} ,
 {HD2_GRN_MPP , HD2_GRN_MPP_FLD , HD2_GRN_MPP_VAL , HD2_GRN_MPP_PIN} ,
 {CPY_RED_MPP , CPY_RED_MPP_FLD , CPY_RED_MPP_VAL , CPY_RED_MPP_PIN} ,
 {CPY_GRN_MPP , CPY_GRN_MPP_FLD , CPY_GRN_MPP_VAL , CPY_GRN_MPP_PIN} ,
 {HD1_GRN_BLINK_MPP , HD1_GRN_BLINK_MPP_FLD , HD1_GRN_BLINK_MPP_VAL , HD1_GRN_BLINK_MPP_PIN} ,
 {HD2_GRN_BLINK_MPP , HD2_GRN_BLINK_MPP_FLD , HD1_GRN_BLINK_MPP_VAL , HD2_GRN_BLINK_MPP_PIN} ,
	
};


unsigned int curr_MPP_REG_VAL;
unsigned int curr_DATA_OUT_ENB;
unsigned int curr_DATA_OUT;
unsigned int curr_BLINK_ENB;


void Read_Regs()
{
	int mem_fd;
	void *gpio_base_addr;
	

	mem_fd = open("/dev/mem", O_RDWR | O_SYNC);

      	if (mem_fd < 0) 
	{
                printf("open /dev/mem error !\n\r");
                return ;
        }
 	
	gpio_base_addr = mmap(0, IO_REG_SIZE, (PROT_READ | PROT_WRITE),MAP_SHARED, mem_fd, IO_REG_BASE);
	
       	if (gpio_base_addr == NULL) 
	{
                printf("mmap error ! \n\r");
                close(mem_fd);
                return ;
        }
	
	curr_DATA_OUT_ENB  = IO_REG32_READ(gpio_base_addr + DATA_OUT_ENB_ADDR - IO_REG_BASE);	
	curr_BLINK_ENB = IO_REG32_READ(gpio_base_addr + BLINK_ENB_ADDR - IO_REG_BASE);
	curr_DATA_OUT  = IO_REG32_READ(gpio_base_addr + DATA_OUT_ADDR - IO_REG_BASE);


	munmap(gpio_base_addr, IO_REG_SIZE);
        close(mem_fd);
			
}


void setLED(int led_num , int led_state)
{
	unsigned int new_value;
	int mem_fd;
        void *gpio_base_addr;


        mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
	
		
	if (mem_fd < 0)
        {
                printf("open /dev/mem error !\n\r");
                return ;
        }

        gpio_base_addr = mmap(0, IO_REG_SIZE, (PROT_READ | PROT_WRITE),MAP_SHARED, mem_fd, IO_REG_BASE);

        if (gpio_base_addr == NULL)
        {
                printf("mmap error ! \n\r");
                close(mem_fd);
                return ;
        }
	

	
	if(led_state == LED_OFF)
	{
		//Turn off GPIO Data Out Enable (active low  -> set high)
		new_value = all_led[led_num].PIN |  curr_DATA_OUT_ENB;
		IO_REG32_WRITE(gpio_base_addr + DATA_OUT_ENB_ADDR - IO_REG_BASE, new_value);
		
		//Turn off blink		
		if(new_value == 3 || new_value == 8)
			new_value = ~all_led[8].PIN & curr_BLINK_ENB;
		else if(new_value == 5 || new_value == 9)			
			new_value = ~all_led[9].PIN & curr_BLINK_ENB;
		else;
		new_value = ~all_led[led_num].PIN & curr_BLINK_ENB;
		IO_REG32_WRITE(gpio_base_addr + BLINK_ENB_ADDR - IO_REG_BASE, new_value);		


		//Turn off data out		
		new_value =  all_led[led_num].PIN | curr_DATA_OUT;
		IO_REG32_WRITE(gpio_base_addr + DATA_OUT_ADDR - IO_REG_BASE , new_value); 		

	}
	else if(led_state == LED_ON)
	{
		// Turn off BLINK	
		new_value = curr_BLINK_ENB;
		if(led_num == 3 || led_num == 8)
			new_value = ~all_led[8].PIN & curr_BLINK_ENB;
                else if(led_num == 5 || led_num == 9)
                        new_value = ~all_led[9].PIN & curr_BLINK_ENB;
                else;
		
                new_value = ~all_led[led_num].PIN & new_value;		
                IO_REG32_WRITE(gpio_base_addr + BLINK_ENB_ADDR - IO_REG_BASE, new_value);
		

		// Turn on MPP
		curr_MPP_REG_VAL = IO_REG32_READ(gpio_base_addr + all_led[led_num].MPP_REG_ADDR - IO_REG_BASE);
		new_value = (all_led[led_num].MPP_REG_FLD | curr_MPP_REG_VAL) & (~all_led[led_num].MPP_REG_FLD | all_led[led_num].MPP_VAL) ;
		IO_REG32_WRITE(gpio_base_addr + all_led[led_num].MPP_REG_ADDR - IO_REG_BASE , new_value);
		
		// Turn on Data out Enable
		new_value = ~all_led[led_num].PIN & curr_DATA_OUT_ENB;
		IO_REG32_WRITE(gpio_base_addr + DATA_OUT_ENB_ADDR - IO_REG_BASE , new_value);		

		// Trun on Data out
		new_value = ~all_led[led_num].PIN & curr_DATA_OUT;
		IO_REG32_WRITE(gpio_base_addr + DATA_OUT_ADDR - IO_REG_BASE , new_value);


	}
	else if(led_state == LED_BLINK)
	{
		// Turn on MPP
		curr_MPP_REG_VAL = IO_REG32_READ(gpio_base_addr + all_led[led_num].MPP_REG_ADDR - IO_REG_BASE);
		new_value = (all_led[led_num].MPP_REG_FLD | curr_MPP_REG_VAL) & (~all_led[led_num].MPP_REG_FLD | all_led[led_num].MPP_VAL);
                IO_REG32_WRITE(gpio_base_addr + all_led[led_num].MPP_REG_ADDR - IO_REG_BASE , new_value);


                // Turn on Data out Enable
		
                new_value = ~all_led[led_num].PIN & curr_DATA_OUT_ENB;
		
		if(led_num == HD1_GRN_BLINK_NUM)
			new_value = ~all_led[HD1_GRN_NUM].PIN & new_value;
		else if(led_num == HD2_GRN_BLINK_NUM )
			new_value = ~all_led[HD2_GRN_NUM].PIN & new_value;
		else;

                IO_REG32_WRITE(gpio_base_addr + DATA_OUT_ENB_ADDR - IO_REG_BASE , new_value);


		

                //Trun off Data out
                new_value = all_led[led_num].PIN | curr_DATA_OUT;
                //IO_REG32_WRITE(gpio_base_addr + DATA_OUT_ADDR - IO_REG_BASE , new_value);


		
		// LED GRN will blink while LED GRN's state would be ON before
		if(led_num == HD1_GRN_BLINK_NUM)
			new_value = ~all_led[HD1_GRN_NUM].PIN & new_value;
		else if(led_num == HD2_GRN_BLINK_NUM)
			new_value = ~all_led[HD2_GRN_NUM].PIN & new_value;
		else ;	
		
		IO_REG32_WRITE(gpio_base_addr + DATA_OUT_ADDR - IO_REG_BASE , new_value);

		// Turn on Blink	
		new_value = all_led[led_num].PIN | curr_BLINK_ENB;
		IO_REG32_WRITE(gpio_base_addr + BLINK_ENB_ADDR - IO_REG_BASE , new_value);


	}
	else
	{
		printf("Unknown LED State\n");

	}		


	
        munmap(gpio_base_addr, IO_REG_SIZE);
        close(mem_fd);

}


inline void show_Msg(void)
{
        printf("Usage: setLED LED_NAME LED_COLOR LED_STATE\n");
        printf("   LED_NAME: The name of LED. (SYS,HD1,HD2,CPY)\n");
        printf("   LED_COLOR: LED color. (RED , GREEN)\n");
        printf("   LED_STATE: ON,OFF,or BLINK\n");

}

int main(int argc , char** argv)
{


	int	led_num;
	
	
	argc--;

	if(argc < 3)
	{
		show_Msg();
		return 0;
	}	
		
	
	if(strcmp(argv[1],"SYS") == 0) led_num = SYS_RED_NUM;
	else if(strcmp(argv[1],"HD1") == 0) led_num = HD1_RED_NUM;
	else if(strcmp(argv[1],"HD2") == 0) led_num = HD2_RED_NUM;
	else if(strcmp(argv[1],"CPY") == 0) led_num = CPY_RED_NUM;
	else
	{
		printf("Unknow LED\n");
		show_Msg();
		return 0;
	}

	if(strcmp(argv[2],"RED") ==0 ) led_num += 0;
	else if(strcmp(argv[2],"GREEN") == 0 ) ++led_num ;
	else 
	{
		printf("Unknow LED color\n");
		show_Msg();
		return 0;
	}
	
	Read_Regs();	
	if(strcmp(argv[3],"OFF") == 0)
	{
		setLED(led_num,LED_OFF);
	}
	else if(strcmp(argv[3],"ON") == 0)
	{
		setLED(led_num,LED_ON);
	}
	else if(strcmp(argv[3],"BLINK") == 0)
	{
		if(strcmp(argv[1],"HD1") == 0 && strcmp(argv[2],"GRN") == 0) 
		{
			setLED(HD1_GRN_BLINK_NUM,LED_BLINK);
		}
		else if(strcmp(argv[1],"HD2") == 0 && strcmp(argv[2],"GRN") == 0)
		{
			setLED(HD2_GRN_BLINK_NUM,LED_BLINK);
		}
		else setLED(led_num,LED_BLINK);
	}
	else 
	{
		printf("Unknow LED State\n");
		show_Msg();
		return 0;
	}


	return 0;

}