/******************************************************************************
menu.h - Created by Peter Mauger 25/07/01
Last Modified 12/10/01

menu provides the Startup menu functions. The menu will provide the ability to
modify servo limits, download a new set of waypoints, check the values of the
GPS and compass, and possibly some other functions.
******************************************************************************/
#include "menu.h"

/* Start_Menu is the function which initialises the menu system for the aircraft
* before takeoff. When it returns the plane will either be ready for automatic 
* flight, or the program will be terminated. Once the plane is ready for flight
* it is only waiting for the autopilot to be activated by the pilot.
* inputs:  plane->contains the required structures and data
* returns: GO if the program should proceed
*	   TERMINATE if the program should be finished
*/
programstate Start_Menu(planestate *plane)
{
	int key = 0;
	
	while(TRUE)
    	{
		key = 0;
		LCDClear();
		LCDMenu("FLY","SET","TST","END");
		LCDPrintf("FLY begins auto\n");
		LCDPrintf("SET hardware\n");
		LCDPrintf("TST sensors\n");
		LCDPrintf("END terminates\n");
	
        	while(key == 0)
		{
			key = Check_Input();
		}
        	switch(key)
        	{
        		case KEY1:	        /* Begin Flight */
				return GO;
			case KEY2:              /* Set Menu */
                    		Set_Menu(plane);
				break;
            		case KEY3:		/* Test sensors */
				Test_Menu(plane);
				break;
			case KEY4:		/* Check for termination then finish program */
				if(Confirmation_Menu() == TERMINATE) return TERMINATE;
				else break;
			default:
				break;
        }
    }
}

/* Set_Menu provides further sub-menues to set servos, the compass, or upload new 
* waypoints.
* inputs:  plane->control struct contains the servo handles
*/
void Set_Menu(planestate *plane)
{
	int key;

	while(TRUE)
	{
		key = 0;
		LCDClear();
		LCDMenu("SVO","CMP","WP","END");
		LCDPrintf("SVO servo limits");
		LCDPrintf("CMP init compass");
		LCDPrintf("WP upload wps\n");
		LCDPrintf("END returns\n");
		
		while(key == 0)
		{
			key = Check_Input();
		}
		switch(key)
        	{
        		case KEY1:				/* Set the servos */
				Servo_Set_Menu(plane);
				break;
			case KEY2:				/* Initialise the compass */
				Compass_Set_Menu();
				break;
			case KEY3:				/* Upload new waypoints */
				Upload_Waypoints_Menu(plane);
				break;
			case KEY4:				/* Return to the main menu */
				return;
			default:
				break;
		}
	}
}	

/* Servo_Set_Menu provides the ability to set the limits and neutral points of
* all of the servos controlled by the eyebot.
* inputs:  plane->control struct contains the servo handles
*/
void Servo_Set_Menu(planestate *plane)
{
	bool test;
	int key;
	error error_type = NOERROR;

	ctrlsurface ctrl_surface;
	while(TRUE)
	{
		key = 0;
		LCDClear();
		LCDMenu("RUD","","","END");
		LCDPrintf("RUD rudder set\n");
		LCDPrintf("END returns\n");
		
		while(key == 0)
		{
			key = Check_Input();
		}
        	switch(key)
		{
			case KEY1:				/* Set the rudder servo */
				test = FALSE;
				error_type = Get_Ctrl_Surface(*plane, RUDDER, &ctrl_surface);
				if(error_type == NOERROR)
				{
					error_type = Establish_Servo_Limits(&ctrl_surface);
					if(error_type == NOERROR)
					{
						error_type = Test_Servo_Limits(ctrl_surface, &test);
						if(error_type != NOERROR)
						{
							LCDPrintf("Error %d in test process\n", error_type);
							OSWait(LONG);
							break;
						}
					}
					else
					{
						LCDPrintf("Error %d in set process\n", error_type);
						OSWait(LONG);
						break;
					}
				}
				else
				{
					LCDPrintf("cs RUDDER not found!\n");
					break;
				}
				
				if(error_type == NOERROR && test == TRUE) Set_Ctrl_Surface(plane, ctrl_surface, RUDDER);
				break;
			case KEY4:				/* Return to the set menu */
				return;
			default:
				break;
		}
	}		                
}

/* Compass_Set_Menu allows the compass to be reinitialised (if it is incorrectly
* aligned)
*/
void Compass_Set_Menu()
{
	bool valid;
	valid = Calibrate_Compass();
	if(valid == FALSE)
	{
		LCDPrintf("No compass data.Recalibrate");
	}
}

/* Upload_Waypoints_Menu allows the user to overwrite the old set of waypoints
*with a new set.
*inputs:  plane->position struct contains the waypoint list
*/
void Upload_Waypoints_Menu(planestate *plane)
{
	wplist waypoints;
	error error_type;
	int num_waypoints;
	int key = 0;
	
	while(TRUE)
	{
		key = 0;
		LCDClear();
		LCDMenu("UPL","","","END");
		LCDPrintf("Connect SERIAL1\n");
		LCDPrintf("Then press UPL\n");
		LCDPrintf("END returns\n");
		
		while(key == 0)
		{
			key = Check_Input();
		}
        	switch(key)
		{
			case KEY1:				/* Begin uploading new waypoints */
				waypoints = Init_Wplist();	/* initialise a new waypoint list */
				LCDClear();
				error_type = Obtain_Wplist_From_File(&waypoints);	/* receive the file and store the waypoints */
				OSWait(LONG);
				if(error_type != NOERROR)
				{ 
				     LCDPrintf("Upload error\n");
				     LCDPrintf("No wps saved\n");
				     OSWait(LONG);
				     return;
				}
				Store_Wplist(plane, waypoints);	/* store the waypoints in the plane structure */
				LCDClear();
				LCDPrintf("Wps received\n");
				num_waypoints = Get_Wplist_Total(waypoints);
				LCDPrintf("Number wps= %d\n",num_waypoints);
				OSWait(LONG);
				return;
			case KEY4:				/* Return to the set menu */
				return;
			default:
				break;
		}
	}
}	

/* Test_Menu allows the user to read the values of the two sensors used, the
* GPS and the compass.
* inputs:  plane->plane struct required to obtain GPS position
*/
void Test_Menu(planestate *plane)
{
	position pos;
	double latitude, longitude;
	char *cardinallat, *cardinallong;
	bool check;
	int key;
	
	LCDClear();
	LCDMenu("","","","END");
	LCDSetPrintf(0,0,"END returns");
	while(TRUE)
	{
		key = 0;
		
		key = Check_Input();
		if(key != KEY4)			/* Keep printing data until END is pressed */
		{
			check = Obtain_GPS_Position(plane);
			if(check == TRUE)
			{
				pos = Get_Curr_Pos(*plane);
				latitude = Get_Latitude(pos);
				if(latitude <= 0) cardinallat = "S";
				else cardinallat = "N";
				longitude = Get_Longitude(pos);
				if(longitude <= 0) cardinallong = "W";
				else cardinallong = "E";
				LCDSetPrintf(1,0,"GPS Pos:\n%f %s,\n%f %s\n",(float)latitude,cardinallat,(float)longitude,cardinallong);
			}
			check = Obtain_Heading(plane);
			if(check == FALSE)
			{
				LCDPrintf("Compass failed\n");
			}
		}
		else return;
		OSWait(SHORT);
	}
}

/* Flight_Menu is the menu that will be available while the flight algorithm is
* underway. It allows the algorithm to be stopped (probably won't be able to 
* restart once stopped)
* inputs:  plane->contains all the data recorded during flight
* returns: GO->continue the flight
*	   TERMINATE->finish the program
*/
programstate Flight_Menu(planestate *plane)
{
	int key = 0;
	
	LCDMenu("STOP","","","END");
	
	key = Check_Input();
	switch(key)
	{
		case KEY1:			/* Download data to a PC */
			Download_Menu(plane);	
			return TERMINATE;
		case KEY4:			/* END program without downloading data */
			return Confirmation_Menu();	/* Must be confirmed */
		default:
			return GO;
	}
} 

/* Download_Menu allows the user to download the error log before terminating 
* the program.
* inputs:  plane->contains the error log
*/
void Download_Menu( planestate *plane )
{
	int key = 0;
	error error_type;
	while(TRUE)
	{
		key = 0;
		LCDClear();
		LCDMenu("TIME","EVNT","","END");
		LCDPrintf("Connect externalserial port\n");
		LCDPrintf("Then press TIME for chronological dload\n");
		LCDPrintf("Or EVNT for event based dload\n");
		LCDPrintf("END terminates\n");
		
		while(key == 0)
		{
			key = Check_Input();
		}
		LCDClear();
        	switch(key)
		{
			case KEY1:				/* Begin downloading data in chronological order */
			  	error_type = Upload_Error_Log(*plane, TIME);
				if(error_type == NOERROR)
				{
					LCDPrintf("Logfile dload\nsuccess!\nPress ctrl-c\n");
					OSWait(LONG);
					break;
				}
				else
				{
					LCDPrintf("Logfile download\nincomplete.\nTry again\n");
					OSWait(LONG);
					break;
				}
			case KEY2:				/* Begin downloading data in event grouped order */
			  	error_type = Upload_Error_Log(*plane, EVENT);
				if(error_type == NOERROR)
				{
					LCDPrintf("Logfile dload\nsuccess!\nPress ctrl-c\n");
					OSWait(LONG);
					break;
				}
				else
				{
					LCDPrintf("Logfile download\nincomplete.\nTry again\n");
					OSWait(LONG);
					break;
				}
			case KEY4:				/* END finishes the program (must confirm) */
				if(Confirmation_Menu() == TERMINATE) return;
				else break;
			default:
				break;
		}
	}
}
		
/* Confirmation_Menu is used when you want to confirm that the user wants to exit
* the program
* returns: TERMINATE if they confirm exit
*	   GO if they say they don't want to exit
*/ 
programstate Confirmation_Menu(void)
{
	int key = 0;
	LCDClear();
	LCDMenu("YES","NO","NO","NO");
	LCDPrintf("Do you really \nwant to quit?\n");
	while(key == 0)
	{
		key = Check_Input();
	}
	if(key == KEY1)
	{
		return TERMINATE;
	}
	else return GO;
}