/****************************************************************************
autopilot.c - Created by Peter Mauger 03/04/01
last modified 12/10/01

autopilot contains the functions required to manipulate the aircraft. For Exp 1
will only contain functions for the rudder, to indicate an error using an
external light, and to check the autopilot switch.
****************************************************************************/
#include "autopilot.h"

/* Wait_For_Switch waits for manual<->autopilot switch to see if eyebot should 
* take/relinquish control. When change occurs, resumes program flow
* inputs:  setting->switch setting to wait for
*/
void Wait_For_Switch(switchstate setting)
{
	switchstate autoswitch = UNDEFINED; 
	while(autoswitch!=setting)
	{
		autoswitch=Check_Switch();		
	}
}

/* Indicate_Error indicates error using external error light. Will flash errornum times then 
* leave light on. Flight is returned to level before indication occurs.
* inputs:  error_type->error type (error)
*	   plane->plane information
* returns: TRUE if error is recoverable
*	   FALSE if no error has occured
*/
bool Indicate_Error(error error_type, errorstage error_stage, planestate *plane)
{
	char error_mesg[MAX_EVENT_LENGTH];
	int i=0;
	
	if(error_type == NOERROR) return FALSE;		/* Make sure there is actually an error */
	if(error_stage == FLIGHT)			/* errorstage FLIGHT means that the flight has begun */
	{									/* Once in flight the only way to indicate an error to the */
	 	if(Get_Desired_Turn(*plane) == STRAIGHT) Turn_Left(plane);	/* pilot is to change the course of the plane and circle */
		else if(Get_Desired_Turn(*plane) == LEFT) Turn_Right(plane);
		else if(Get_Desired_Turn(*plane) == RIGHT) Turn_Left(plane);
		Update_Servos(plane);
		Convert_Tag2Event(error_type,error_mesg);
		while(i<(MAX_EVENT_LENGTH-1) && error_mesg[i]!=' ') i++;	/* find the end of the string */
		error_mesg[i] = '\0';  						/* insert NULL so LCDPrintf terminates string */
	       	if(error_type != FLIGHTCOMPLETE) LCDPrintf("ERROR %s; ",error_mesg);
		else LCDPrintf("\n!!!%s!!!\n",error_mesg);
		Log_Event(plane, error_type);		/* log the event into the data log */
		Wait_For_Switch(OFF);			/* wait until the pilot regains control before */
		Log_Event(plane, SWITCHMANUAL);
		return TRUE;				/* restarting the flight algorithm */
	}
	else if(error_stage == INITIALISE)		/* errorstage INITIALISE means the program is */ 
	{						/* still initialising its state (don't indicate with light or log) */
	        if(error_type == NOWPS)			/* no waypoints were detected during initialisation */
		{
		      	LCDPrintf("No waypoints");
		      	return TRUE;
		}
		else if(error_type == SERVOERROR || error_type == COMPASSERROR || error_type == GPSERROR)
	        {					/* a hardware error has occured during initialisation */
		    	LCDPrintf("Hardware Error");
		    	return TRUE;
	        }
		else if( error_type == INITERROR )	/* used when the auto-startup program needs to be terminated */
		{
		    	LCDPrintf("Reset\n");
		    	return TRUE;
	    	}
	        else					/* shouldn't get any other errors during initialisation */
		{
		    	LCDPrintf("Unknown error %d", error_type);
		    	return TRUE;
		}	
	}
	else return FALSE;
}
	
/* Returns aircraft to level flight, and centres rudder (shouldnt have to worry
* about leveling aircraft for 2001 experiments)
* inputs:  plane->contains plane state information
* returns: an error if a problem has occured
*	   NOERROR otherwise
*/
error Fly_Straight(planestate *plane)
{
	ctrlsurface cs;
	error error_type;
	
	Set_Desired_Turn(plane, STRAIGHT);			/* This is all that is required to make the plane go straight */
	Set_Rudder_State(plane, STRAIGHT);			/* until banking is implemented */
	error_type = Get_Ctrl_Surface(*plane, RUDDER, &cs);	/* Retrieve the control surface so it can be updated */	
	if(error_type == NOERROR)
	{
		error_type = Set_Angle(&cs, Get_Neutral_Point(cs), NORMAL); /* Set the rudder straight */
		if(error_type == NOERROR)
		{
			Set_Ctrl_Surface(plane, cs, RUDDER);		/* Update the control surface information */
			LCDSetPrintf(5,0,"Flying Straight ");
			return error_type;
		}
		else return error_type;
	}
	else return error_type;
}

/* Brings aircraft into left turn (only using rudder in Exp 1)
* inputs:  plane->contains plane state information
* returns: an error if a problem has occured
*	   NOERROR otherwise
*/
error Turn_Left(planestate *plane)
{
	ctrlsurface cs;
	error error_type;
	
	Set_Desired_Turn(plane, LEFT);				/* This is all that is required to make the plane go left */
	Set_Rudder_State(plane, LEFT);				/* until banking is implemented */
	error_type = Get_Ctrl_Surface(*plane, RUDDER, &cs);
	if(error_type == NOERROR)
	{
		error_type = Set_Angle(&cs, Get_Min_Limit(cs), NORMAL);	/* Set the rudder maximum left */
		if(error_type == NOERROR)
		{
			Set_Ctrl_Surface(plane, cs, RUDDER);		/* Update the control surface information */
			LCDSetPrintf(5,0,"Flying Left     ");
			return error_type;
		}
		else return error_type;
	}
	else return error_type;
}

/* Brings aircraft into right turn (only using rudder in Exp 1)
* inputs:  plane->contains plane state information
* returns: an error if a problem has occured
*	   NOERROR otherwise
*/
error Turn_Right(planestate *plane)
{
	ctrlsurface cs;
	error error_type;
	
	Set_Desired_Turn(plane, RIGHT);				/* This is all that is required to make the plane go right */
	Set_Rudder_State(plane, RIGHT);				/* until banking is implemented */
	error_type = Get_Ctrl_Surface(*plane, RUDDER, &cs);
	if(error_type == NOERROR)
	{
		error_type = Set_Angle(&cs, Get_Max_Limit(cs), NORMAL);	/* Set the rudder maximum right */
		if(error_type == NOERROR)
		{
			Set_Ctrl_Surface(plane, cs, RUDDER);		/* Update the control surface information */
			LCDSetPrintf(5,0,"Flying Right    ");
			return error_type;
		}
		else return error_type;
	}
	else return error_type;
}