/****************************************************************************
*
*   Copyright (c) 2008 www.societyofrobots.com
*   (please link back if you use this code!)
*
*   This program is free software; you can redistribute it and/or modify
*   it under the terms of the GNU General Public License version 2 as
*   published by the Free Software Foundation.
*
*   Alternatively, this software may be distributed under the terms of BSD
*   license.
*
****************************************************************************/

//Add your code here

//enter your code in control here
//photovore is just a default sample program and can be deleted

char cByte;
char cByte; 
char dByte;
char eByte;
int modify_servo = 1;
int sensitivity = 10;	//change this value to move servos in smaller increments.
int speed = 1;			//walking speed
int repeat = 0;			
int i = 0;



void control(void)
{
	rprintfInit(uart1SendByte);
	delay_ms(50);

	welcome_message();			//initial message displayed to user
	delay_ms(50);

	while(1)
	{
		cByte=uart1GetByte(); //get byte from USB on the axon

		if(cByte!=-1)//no new data
		{
			if (cByte == 'o')
				LED_on();

			if (cByte == 'p')
				LED_off();

			if (cByte == 'h')
			{
				rprintf("\n Home position\n");
				home_position();
			}

			if (cByte == 'j')
				jog_command();

			if (cByte == 'w')
				walk_forward_command();


			if (cByte == 's')
			{
				rprintf("\n Walking reverse \n");
				walk_reverse(50);//higher number slows gait down
			}

			if (cByte == 'm')
				modify_servo_command();

			delay_ms(100);
		}

		delay_ms(100);
	}
}


void welcome_message()
{
	rprintf("\nSELECT FROM THE FOLLOWING OPTIONS\n");
	rprintf("\n(o) LED on, \t\t(p) LED off");
	rprintf("\n(h)ome position, \t(w)alk forward, \t(s) walk reverse");
	rprintf("\n(m)odify servos\n");
	delay_ms(100);
}

void modify_servo_message()
{
	rprintf("\nWhich servo would you like to move?\n");
	rprintf("0:left foot \t 1: right foot\n");
	rprintf("2:left ankle \t 3: right ankle\n");
	rprintf("4:left knee \t 5: right knee\n");
	rprintf("6:left thigh \t 7: right thigh\n");
	rprintf("8:left hip \t 9: right hip\n");
	rprintf("\n(p)rint all servo values to screen\n");

	rprintf("\n\nWARNING: Your robot will jolt into the home position \n");
	delay_ms(100);
}

void modify_joint_message()
{
	rprintf("+/- increment/decrement \t(d)one \n");
	delay_ms(50);
}

void modify_finish_message()
{
	rprintf("\n Finished modifying current servo. Select another option\n");
	delay_ms(50);
}

void walk_forward_command()
{
	speed = -1;
	rprintf("what speed?\n");

	while(speed == -48 || speed == -1)
	{
		speed = (uart1GetByte() - 47);
		delay(50);
	}

	rprintf("\n Walking forward \n");
	walk_forward(speed);//higher number slows gait down
}


void jog_command()
{
	repeat = 1;
	speed = -1;
	rprintf("jog forward at what speed?\n");

	while(speed == -48 || speed == -1)
	{
		speed = (uart1GetByte() - 47);
	}


	while(repeat)
	{
		cByte = uart1GetByte();

		if (cByte == 27)
			repeat = 0;

					
		jog_forward(speed);
	}
}

void modify_servo_command()
{
	modify_servo_message();

	while(1)
	{
		delay_ms(100);

		modify_servo = 1;

		cByte = uart1GetByte(); //get byte from USB on the axon;

		if(cByte != -1)//no new data
		{
			modify_servo = 1;

			if (cByte == '0')
			{
				modify_left_foot();
				modify_servo_message();
			}
	
			if (cByte == '1')
			{
				modify_right_foot();
				modify_servo_message();
			}

			if (cByte == '2')
			{
				modify_left_ankle();
				modify_servo_message();
			}

			if (cByte == '3')
			{
				modify_right_ankle();
				modify_servo_message();
			}

			if (cByte == '4')
			{
				modify_left_knee();
				modify_servo_message();
			}

			if (cByte == '5')
			{
				modify_right_knee();
				modify_servo_message();
			}

			if (cByte == '6')
			{
				modify_left_thigh();
				modify_servo_message();
			}

			if (cByte == '7')
			{
				modify_right_thigh();
				modify_servo_message();
			}

			if (cByte == '8')
			{
				modify_left_hip();
				modify_servo_message();
			}

			if (cByte == '9')
			{
				modify_right_hip();
				modify_servo_message();
			}

			if (cByte == 'p')
			{
				print_servo_values();
				modify_finish_message();
			}

			if (cByte == 27)
				exit;

			delay_ms(300);
		}	
	}
}


void modify_left_foot()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_left_foot(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_left_foot(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}

		delay_ms(100);
	}
}

void modify_right_foot()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_right_foot(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_right_foot(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}

		delay_ms(100);
	}
}

void modify_left_ankle()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_left_ankle(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_left_ankle(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}

		delay_ms(100);
	}
}

void modify_right_ankle()
{
	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_right_ankle(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_right_ankle(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}		
															
		delay_ms(100);
	}
}

void modify_left_knee()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_left_knee(sensitivity);
			delay_ms(10);
			biped_move();	
		}								

		if (cByte == '-')
		{
			set_left_knee(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}		
															
		delay_ms(100);
	}
}

void modify_right_knee()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_right_knee(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_right_knee(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}	
								
		delay_ms(100);
	}
}

void modify_left_thigh()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_left_thigh(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_left_thigh(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}	
																
		delay_ms(100);
	}
}

void modify_right_thigh()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_right_thigh(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_right_thigh(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}

		delay_ms(100);
	}
}

void modify_left_hip()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_left_hip(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_left_hip(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}	
																
		delay_ms(100);
	}
}

void modify_right_hip()
{
	modify_joint_message();

	while(modify_servo)
	{
		cByte = uart1GetByte(); // get byte from USB on the axon

		if (cByte == '+')
		{
			set_right_hip(sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == '-')
		{
			set_right_hip(-sensitivity);
			delay_ms(10);
			biped_move();
		}

		if (cByte == 'd')
		{
			delay_ms(100);
			modify_finish_message();
			modify_servo = 0;
		}	
																
		delay_ms(100);
	}
}