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

Experiment3 is the main program which will perform experiment 3 of the Automated
Flight Group's project 2001. Experiment 3 will involve most of the control 
procedures and control flows of experiment 2. The extension on experiment 2 will 
be that the plane will reverse a turn if it does not improve its heading after 
a predefined number of control decision. This should overcome the problem of 
turning through a headwind (if the rudder is unable to perform this type of turn).
*****************************************************************************/

#include "include.h"

int main(){

        int curr_wpnum = 0, total_num_wps = 0, compass_count = 0;
        bool not_at_wp = TRUE, new_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 1;
	}

        total_num_wps = Get_Wplist_Total(Get_Wplist(plane));
	LCDPrintf("%d waypoints\n", total_num_wps);
	OSWait(LONG);
	while(TRUE)					/* Continue until an exit state is reached */
        {
                LCDClear();
		error_type = NOERROR;
		Wait_For_Switch(ON);			/* wait for ON == wait for AUTOPILOT */
		Log_Event(&plane, SWITCHAUTO);
		LCDPrintf("autopilot engage; ");
                for(curr_wpnum=0; curr_wpnum<total_num_wps; curr_wpnum++)
                {								/* For all waypoints in the list */
                        LCDPrintf("new wp loop %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 */				
			}
			LCDPrintf("wp set; ");
			new_wp = TRUE;
			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 )
					{
						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);
							LCDPrintf("atwp; ");
                                		        break;                                          	/*If at current wp exit loop to retrieve next wp*/
                                		}	
					}
					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 */
                                
				if(new_wp == TRUE)            				/* Having just reached a waypoint we must determine a new turn */
                                {
                                        new_wp = FALSE;
					if(correction == 180 || correction == -180)
                                        {
                                                if(Get_Desired_Turn(plane) == STRAIGHT)      	/* If plane going straight turn left */
                                                {                                               /* otherwise just keep going the way */
                                                        Turn_Right(&plane);                      /* its going */
                                                }
                                        }
                                        else if(correction < -HEAD_TOL && correction > -180)
                                        {       /*negative correction requires left turn*/
                                                Turn_Left(&plane);
                                        }
                                        else if(correction > HEAD_TOL && correction < 180)
                                        {       /*positive correction requires right turn*/
                                                Turn_Right(&plane);
                                        }
                                        else                            /* bearing is within tolerance of */
                                        {                               /* heading, maintain heading */
                                                Fly_Straight(&plane);
                                        }
                                }
                                else                /* Continue current turn unless heading in the right direction (within HEADTOL) */
                                {                   /* Only other prob could be if wind is cancelling turn */
                                       if(Get_Desired_Turn(plane) != STRAIGHT)    			/* If the plane is currently turning */
                                       {
                                                if(correction <= HEAD_TOL && correction >= -HEAD_TOL)   /* check to see if heading is now correct */
                                                {                                                       
                                                        Fly_Straight(&plane);
						}
                                                else                                                    /* If not make sure plane is */
                                                {                                                       /* improving heading, otherwise */
                                                        Heading_Improvement(&plane, correction);        /* change direction of turn */
                                                }
                                        }
                                        else                                            		/* If the plane IS flying straight, check */
                                        {                                               		/* to see if heading is STILL correct */
                                                if(correction < -HEAD_TOL && correction > -180)  	/* If correction out of -tolerance */
                                                {                                               	/* turn left */
                                                        Turn_Left(&plane);
						}
                                                else if(correction > HEAD_TOL && correction < 180) 	/* If correction out of +tolerance */
                                                {                                                 	/* turn right */ 
                                                        Turn_Right(&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 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;	
				}
			}
		}
        }
}