void fSetCoreParam(void)
{
	//clear ico?
	uint16_t keyValue=0;
	static uint8_t Id=0;

	keyValue = KeyPop();
	switch(keyValue)
	{
	case BTN_UP: fAddCoreParam(Id); break;
	case BTN_DOWN:fSubCoreParam(Id); break;
	case BTN_CLOCK: Id++; 
					if (Id >= CoreParamsMax)
					{
						Id = 0;
					}
					break;
	case BTN_TIMER: Id--;
					if (Id >= 0xff)
					{
						Id = CoreParamsMax-1;
					}
					break;
	case BTN_SHUT: MenuParam.pfmenu = MenuOffStatus;  Id=0; lcd_wr_char(_lcd5_find,0);return;
	case BTN_LOCK:	MenuParam.lockFlag ^= 1;break;//need to add lock ico
	default: break;
	}
	lcd_wr_char(_lcd4_lock, MenuParam.lockFlag);
	fOffStatusPara(Id);
}
/*****Function to Print String on LCD*****/
void lcd_string(char *str)
{
	while(*str != '\0')
	{
		lcd_wr_char(*str);
		str++;
	}
	int i=0;
	for(i=0;i<12;i++) lcd_wr_char(' ');
}
//Function to Print String on LCD
void lcd_string(char *str)
{
	while(*str != '\0')
	{
		lcd_wr_char(*str);
		str++;
	}
}
void main (void)
{
	//Set CPU Clock to 40MHz. 400MHz PLL/2 = 200 DIV 5 = 40MHz
	SysCtlClockSet(SYSCTL_SYSDIV_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);

	lcd_port_config();
	lcd_init();

	lcd_cursor(1,1);
	lcd_wr_char('S');


}
void MenuOffStatus(void)
{
	uint16_t keyValue=0;

	keyValue = KeyPop();
	switch(keyValue)
	{
	case BTN_SETCORE:	MenuParam.pfmenu = fSetCoreParam; lcd_wr_char(_lcd5_find,1);break;
	case BTN_SHUT: MenuParam.pfmenu = MenuOnStatus; 
					lcd_wr_char(_lcd3_run, 1);
					lcd_wr_char(_lcd10_hotWater, 1);
					MenuParam.runFlag ^= 0x01;
					MenuParam.StartParamChange = 1;
					break;
	case BTN_LOCK: MenuParam.lockFlag ^= 1;break;//need to add lock ico
	default: break;
	}
	//lcd_wr_char(_lcd11_waterT, 1);
	//lcd_wr_char(_lcd13_c, 1);
	lcd_wr_char(_lcd4_lock, MenuParam.lockFlag);//lock
	fClockOn(MenuParam.clock.hour, MenuParam.clock.min);
	DispLayWatetT();
	LcdSetWater(0);
}
    /*!
	 Function used for displaying a number on LCD
    */
void lcd_num(int x)
{
	if (current_state == 4) {
		lcd_cursor(2,1);
	}
	else if (current_state == 8) {
		lcd_cursor(1,1);
		current_state = 0;
	}
	current_state++;
	char cVal[10];
	itoa(x,cVal,10);
	lcd_string(cVal);
	char a=" ";
	lcd_wr_char(a);
}
//!Function To Print Any input value upto the desired digit on LCD
void lcd_print (char row, char coloumn, unsigned int value, int digits)
{
	unsigned char flag=0;
	if(row==0||coloumn==0)
	{
		lcd_home();
	}
	else
	{
		lcd_cursor(row,coloumn);
	}
	if(digits==5 || flag==1)
	{
		million=value/10000+48;
		lcd_wr_char(million);
		flag=1;
	}
	if(digits==4 || flag==1)
	{
		temp = value/1000;
		thousand = temp%10 + 48;
		lcd_wr_char(thousand);
		flag=1;
	}
	if(digits==3 || flag==1)
	{
		temp = value/100;
		hundred = temp%10 + 48;
		lcd_wr_char(hundred);
		flag=1;
	}
	if(digits==2 || flag==1)
	{
		temp = value/10;
		tens = temp%10 + 48;
		lcd_wr_char(tens);
		flag=1;
	}
	if(digits==1 || flag==1)
	{
		unit = value%10 + 48;
		lcd_wr_char(unit);
	}
	if(digits>5)
	{
		lcd_wr_char('E');
	}
	
}
void ErrorDisplay(void)
{
	uint16_t keyValue=0;

	keyValue = KeyPop();
	switch(keyValue)
	{
	case BTN_SHUT:	MenuParam.runFlag = 0;
					MenuParam.StartParamChange=1;
					MenuParam.pfmenu =MenuOffStatus; 
					//clear errors then shutdown

					break;
	case BTN_LOCK: MenuParam.lockFlag ^= 1;break;//need to add lock ico
	default: break;
	}

	lcd_wr_num(10, 0, 1);
	lcd_wr_num(0, 1, 1);
	lcd_wr_num(0, 2, 1);
	fClockOn(MenuParam.clock.hour, MenuParam.clock.min);
	LcdSetWater(0);
	lcd_wr_char(_lcd4_lock, MenuParam.lockFlag);//lock
}
//The main invoker routine. It takes as argument the next command to execute and does what is necessary
//Self-explanatory code!
void my_invoker (unsigned char command) {
	if(command == BUZZER_ON){
		buzzer_on();
		return;
	}
	else if(command == BUZZER_OFF){
		buzzer_off();
		return;
	}
	else if(command == MOVE_FORWARD) 
    {
        forward();  //forward
        return;
    }

    else if(command == MOVE_BACKWARD)
    {
        back(); //back
        return;
    }

    else if(command == MOVE_LEFT) 
    {
        left();  //left
        return;
    }

    else if(command == MOVE_RIGHT)
    {
        right(); //right
        return;
    }

    else if(command == STOP) 
    {
        stop(); //stop
        return;
    }
	
	else if(command == SET_VELOCITY) 
    {
        int numargs;
		unsigned char * ch = recieve_args(&numargs);
        
		//assert(numargs == 1);

		int velleft = (int)*(ch);
		int velright = (int)*(ch+1);
		velocity(velleft,velright);

        return;
    }
	
	else if(command == MOVE_BY) 
    {
        int numargs;
		unsigned char * ch = recieve_args(&numargs);
		int pos_a = (int)*(ch);
		int pos_b = (int)*(ch+1);

		//int pos = 10;
		//while (pos_b--) pos *= 10;
		//pos *= pos_a;
		//forward_mm(pos);
		pos_a += (pos_b << 8);

		forward();
		velocity(120,120);

		while (pos_a--) {
			//delay on 5 ms
			stop_on_timer4_overflow = 1;
			start_timer4();
			while (stop_on_timer4_overflow != 0) {;}
		}
		stop();
		send_char(SUCCESS);		
		leftInt = 0;
		rightInt = 0;
		
		return;
    }

	else if(command == MOVE_BACK_BY) 
    {
        int numargs;
		unsigned char * ch = recieve_args(&numargs);
		int pos_a = (int)*(ch);
		int pos_b = (int)*(ch+1);

		//int pos = 10;
		//while (pos_b--) pos *= 10;
		//pos *= pos_a;
		//forward_mm(pos);
		pos_a += (pos_b << 8);

		back();
		velocity(120,120);

		while (pos_a--) {
			//delay on 5 ms
			stop_on_timer4_overflow = 1;
			start_timer4();
			while (stop_on_timer4_overflow != 0) {;}
		}
		stop();
		send_char(SUCCESS);		
		leftInt = 0;
		rightInt = 0;
		
		return;
    }
	
	else if(command == TURN_LEFT_BY) 
    {
        int numargs;
		unsigned char * ch = recieve_args(&numargs);
        already_stopped = 0;
		int pos_a = (int)*(ch);
		int pos_b = (int)*(ch+1);

		pos_a += (pos_b << 8);

		_delay_ms(500);
		left();
		velocity(200,200);

		while (pos_a--) {
			//delay on 5 ms
			stop_on_timer4_overflow = 1;
			start_timer4();
			while (stop_on_timer4_overflow != 0) {;}
		}
		stop();
		send_char(SUCCESS);		
		leftInt = 0;
		rightInt = 0;
		already_modified_stopped = 0;

        return;
    }

	else if(command == TURN_RIGHT_BY) 
    {
        int numargs;
		unsigned char * ch = recieve_args(&numargs);
        
		//assert(numargs == 2);

		int pos_a = (int)*(ch);
		int pos_b = (int)*(ch+1);

		pos_a += (pos_b << 8);

		_delay_ms(500);
		right();
		velocity(200,200);


		while (pos_a--) {
			//delay on 5 ms
			stop_on_timer4_overflow = 1;
			start_timer4();
			while (stop_on_timer4_overflow != 0) {;}
		}		

		stop();
		send_char(SUCCESS);
		leftInt = 0;
		rightInt = 0;
		already_modified_stopped = 0;
        return;
    }

    else if(command == LCD_SET_STRING) 
    {
        int numargs;
		unsigned char * ch = recieve_args(&numargs);
        
        int i =0;
		lcd_clear();
        for(;i<numargs;i++)
        {
            lcd_wr_char(*(ch+i));
        }
        return;
    }
	
	else if (command == SET_PORT){
    	int numargs;
    	unsigned char * ch = recieve_args(&numargs); ; 
    	if (numargs != 2){
   
	    }
    	int portnum = (int) *(ch);
    	unsigned char value = (unsigned char) *(ch+1); 
    
		setPort(portnum,value);
    }

    else if(command == GET_SENSOR_VALUE)
    {
    	int numargs;
    	unsigned char * ch = recieve_args(&numargs); ; 
    	if (numargs != 1){
   
	    }
    	int sensornum = (int) *(ch);
    
		//setPort(portnum,value);
		getSensorValue(sensornum);
       
    }
    else if(command == GET_PORT)
    {
      	int numargs;
    	unsigned char * ch = recieve_args(&numargs); ; 
    	if (numargs != 1){
   
	    }
    	int portnum = (int) *(ch); 
    
		getPort(portnum);
        
    }
    else if (command == WHITELINE_FOLLOW_START) {
		whiteline_follow_start();
	}
	else if(command == PRINT_STATE){
		buzzer_on();
		lcd_num(state);
		_delay_ms(1000);
		buzzer_off();
	}
	else if (command == WHITELINE_FOLLOW_END) {
		whiteline_follow_end();
	}
	else if (command == WHITELINE_STOP_INTERSECTION) {
		whiteline_stop_intersection_flag = 1;
	}
    else if(command == ACC_START) {
   		acc_flag = 1;
		
   
    }
	else if(command == ACC_STOP) {
		acc_flag = 0;
		acc_modified_flag = 0;
		buzzer_off();
	}
	else if(command == ACC_MODIFIED){
		acc_modified_flag = 1;
		already_modified_stopped = 0;
	}
	else if(command == ACC_CHECK){
		if (acc_modified_flag == 1 && already_modified_stopped == 1){
			send_char((char)1);
		}
		else {
			char value = PORTA;
			if (value == 0) send_char((char)2);
			else send_char((char)0);
		}
	}
	else if (command == ENABLE_LEFT_WHEEL_INTERRUPT) {
		leftInt = 0;
		left_position_encoder_interrupt_init();
	}
	else if (command == ENABLE_RIGHT_WHEEL_INTERRUPT) {
		rightInt = 0;
		right_position_encoder_interrupt_init();
	}
	else if (command == GET_LEFT_WHEEL_INTERRUPT_COUNT) {
		send_int (leftInt);
		leftInt = 0;
	}
	else if (command == GET_RIGHT_WHEEL_INTERRUPT_COUNT) {
		send_int (rightInt);
		rightInt = 0;
	}
	else if (command == SET_TIMER) {
	int numargs;
    	unsigned char * ch = recieve_args(&numargs); ; 
    	if (numargs != 1){
   
	    }
    	int time = (int) *(ch); 
    
		timer4_init2(time);
	}
	else if (command == DISCONNECT) {
		disconnect();
	}
	else { //Error!!! Unrecognized Command
		buzzer_on();
		_delay_ms(1000);
		buzzer_off();
	}
}