/*****************************************************************************
Experiment2.c - Created by Peter Mauger 03/04/01
Last modified 10/10/01

Experiment2 is the main program which will perform experiment 2 of the Automated
Flight Group's project 2001. Experiment 2 will involve the eyebot control of the
aircraft's rudder only. Level flight will be maintained by a piezo gyro and the
pilot, who will also perform take off and landing of the aircraft. A set of
waypoints will be entered into the eyebot and, once activated by the pilot, the
eyebot will fly the aircraft through the sequence of waypoints before circling
the last. Sensors used will be the Magellen GPS 315 and a 2D digital vector
compass.
*****************************************************************************/

#include "include.h"

int main(){

        int curr_wpnum = 0, total_num_wps = 0, compass_count = 0;
        bool not_at_wp = TRUE, valid = FALSE;
	error error_type = NOERROR;
        double bearing_req = 0, correction = 0;
	programstate cont = TERMINATE;
        planestate plane;

        error_type = Initialise_Plane(&plane);      			/*This function will contain all the*/
        if(error_type != NOERROR)              				/*required functions to initialise the*/
        {                                        			/*aircraft and its components. Establish*/
                valid = Indicate_Error(error_type, INITIALISE, &plane);	/*current pos and bearing*/
        }                                        			/*and desired turn = STRAIGHT*/
		
	if(valid == TRUE) 		/* If an error occured during init then shut down */
	{				/* and return */
	        Shut_Down(plane);
	        return 1;
	}
	LCDPrintf("Initialisation\nComplete\n");
	OSWait(LONG);
	LCDClear();
	
	cont = Start_Menu(&plane);
	if(cont == TERMINATE) 		/* If terminate program returned then shut down and return */
	{
	        Shut_Down(plane);
		return 0;
	}

        total_num_wps = Get_Wplist_Total(Get_Wplist(plane));
	while(TRUE)					/* Continue until an exit state is reached */
        {
                LCDClear();
		error_type = NOERROR;
		LCDPrintf("Waiting for Auto\n");
		Wait_For_Switch(ON);			/* wait for ON == wait for AUTOPILOT */
		Log_Event(&plane, SWITCHAUTO);
		LCDClear();
		for(curr_wpnum=0; curr_wpnum<total_num_wps; curr_wpnum++)
                {								/* For all waypoints in the list */
                        LCDSetPrintf(0,0,"Wp %d of %d ",curr_wpnum, total_num_wps);
			if(error_type != NOERROR) break;			/* If an error occured previously break so that a restart can occur */
			error_type = Set_Curr_Wp(&plane, curr_wpnum);		/* stores next waypoint as current */
                        if(error_type != NOERROR) 	
			{
				if(Indicate_Error(error_type, FLIGHT, &plane)==TRUE) break;	/* indicates that an incorrect waypoint number has been entered */				
			}
			not_at_wp = TRUE;                               /*Reset not_at_wp (ie not yet at the next waypoint)*/
                        
			while(not_at_wp)				/* Until the waypoint is reached */
                        {
				if(Flight_Menu(&plane) == TERMINATE) 	/* Bring up the flight menu */
				{					/* If termination is required */
				        Shut_Down(plane);		/* shut down and return */
				        return 0;
				}
                                
				if(Check_Switch() == OFF)
				{
					error_type = SWITCHMANUAL;
					Log_Event(&plane, SWITCHMANUAL);
					break;
				}
				
				Store_Old_Position(&plane);             /* store current valid data in case it needs to be retreived */

                                valid = Obtain_GPS_Position(&plane);   	/*get GPS position return valid if successful*/
                                
                                if(valid == TRUE)
				{           
					Log_Event(&plane, LOGPOSITION);
					error_type = At_Wp(plane, DIST_TOL, &not_at_wp);             		/* See if curr_pos~=curr_wp. */
					if( error_type != NOERROR )
					{
						LCDClear();
						LCDPrintf("atwp err; ");
						if(Indicate_Error(error_type, FLIGHT, &plane)==TRUE) break;	/* Error has occured. Indicate and wait */		
					}									/* for manual control to resume. */
					else
					{
                                		if( not_at_wp == FALSE )
                                		{
							Log_Event(&plane, ATWP);
							LCDSetPrintf(1,0,"WpStat: atwp   ");
							OSWait(SHORT);
							break;
                                		}
						else LCDSetPrintf(1,0,"WpStat: notatwp");	
					}
					bearing_req = Calc_Bearing(plane);    		/* Determine bearing required 0->360 from positional data */
                                	Set_Req_Bearing(&plane, bearing_req);		/* Set the bearing required in the plane information */
				}
				
                                Store_Old_Heading(&plane);                              /* Store old heading do determine if */
                                compass_count = 0; 					/* heading drift is occuring (plane not */
				valid = FALSE;                                          /* turning how it should). */
                                                                                        
                                while(compass_count<5 && valid == FALSE)                /* 5 attempts to get compass data */
                                {
                                        valid = Obtain_Heading(&plane);                    /* get compass data 0->360 degrees */
                                        compass_count++;
                                }

                                if(valid == FALSE)                                      		/* If no heading could be found */
                                {                                                       		/* indicate error (using external */
                                        Restore_Old_Heading(&plane);                    		/* light2) and fly straight until */
                                        error_type = NOHEADING;			     			/* control is regained by Pilot */
                                	if(Indicate_Error(NOHEADING, FLIGHT, &plane)==TRUE) break;    	/* (wait for restart) */
				}
				
				correction = Calc_Correction(plane, bearing_req);       /* Determine how much heading correction to */
                                							/* be made (-180 < correction < 180) */
                                if(correction == 180 || correction == -180)
                                {
                                	if(Get_Desired_Turn(plane) == STRAIGHT)      	/* If plane going straight turn right */
                                        {                                               /* otherwise just keep going the way */
                                        	Turn_Right(&plane);                     /* its going */
                                        }
                                }
                                else if(correction < -HEAD_TOL && correction > -180)
                                {       /*negative correction requires right turn*/
                                        Turn_Left(&plane);
                                }
                                else if(correction > HEAD_TOL && correction < 180)
                                {       /*positive correction requires left turn*/
                                        Turn_Right(&plane);
                                }
                                else                            /* bearing is within tolerance of */
                                {                               /* heading, maintain heading */
                                        Fly_Straight(&plane);
                                }
                        	Update_Servos(&plane);                  /* Updates servos given currently stored states */
                        }                                       	/* Take next measurement */
                }
                if(error_type == NOERROR)
                {
                        Indicate_Error(FLIGHTCOMPLETE, FLIGHT, &plane); /* At final waypoint; indicate that no more */
                        Log_Event(&plane, FLIGHTCOMPLETE);		/* waypoints exist. maintain level flight */
				                			/* until pilot regains control */
			while(TRUE)					/* Wait for the menu to exit */
			{
				if(Flight_Menu(&plane) == TERMINATE)
		     	   	{ 
				      Shut_Down(plane);
				      return 0;	
				}
			}
		}
        }
}