/***************************************************************************
plane.c - Created by Peter Mauger 17/07/01
last modified 12/10/01

plane contains planestate object (contains all data on plane position, attitude
and control surfaces), as well as the functions required to manipulate it.
***************************************************************************/
#include "plane.h"

/******************** The following functions manipulate the positional settings of the plane */

/* Init_Plane_Position_State initialises the internal variables of the position
* state part of the plane state
* returns:  an initialised planeposstatestate structure
*/
planeposstate Init_Plane_Position_State()
{
	planeposstate planepos;
	wplist wps = Init_Wplist();		/* wps contains a dummy set of waypoints */
	position init_pos = Init_Pos();		/* init_pos contains a dummy set of data */
	int i;
		
	planepos.desired_turn = STRAIGHT;
	planepos.desired_altitude = LEVEL;
	planepos.waypoints = wps;
	planepos.altitude = 0;
	planepos.curr_pos = init_pos;			
	planepos.curr_wp = init_pos;
	planepos.stored_pos = init_pos;
	planepos.curr_heading = 0;
	planepos.req_bearing = 0;
	planepos.stored_heading = 0;
	planepos.pitch = 0;
	planepos.bank_angle = 0;
	planepos.airspeed = 0;
	planepos.best_correction = 0;
	planepos.improve_count = 0;
	planepos.gps_mesg.complete = FALSE;
	planepos.gps_mesg.valid = FALSE;
	planepos.gps_mesg.mesg_length = 0;
	planepos.gps_mesg.mesg_ptr = 0;
	for(i=0;i<MAX_MESSAGE_LENGTH;i++) planepos.gps_mesg.buffer[i] = '\0';
	
	return planepos;
}

/* Init_Plane_Control_State initialises the internal variables of the control
* state part of the plane state
* inputs:  pcs->a variable for passing the plane control state out;
* returns: SERVOERROR if an error has occured in initialising the servo,
*	   NOERROR otherwise
*/
error Init_Plane_Control_State(planectrlstate *pcs)
{
	error error_type;
	ctrlsurface cs;
	
	pcs->rudder_state = STRAIGHT;
	error_type = Init_Ctrl_Surface(RUDDER, &cs);
	if(error_type == NOERROR)
	{
		pcs->rudder = cs;
	}
	else return error_type;
	error_type = Init_Ctrl_Surface(AILERON, &cs);
	if(error_type == NOERROR)
	{
		pcs->banking = cs;
	}
	else return error_type;
	error_type = Init_Ctrl_Surface(ELEVATOR, &cs);
	if(error_type == NOERROR)
	{
		pcs->elevator = cs;
	}
	else return error_type;
	error_type = Init_Ctrl_Surface(THROTTLE, &cs);
	if(error_type == NOERROR)
	{
		pcs->throttle = cs;
	}
	else return error_type;
	return NOERROR;
}

/* Init_Plane_State initialises the internal variables of the plane state structure
* inputs:  ps->the variable for passing the main plane state structure
* returns: SERVOERROR if an error occured when initialising the servos,
*	   NOERROR otherwise
*/
error Init_Plane_State(planestate *ps)
{
	error error_type;
	planectrlstate pcs;
	planeposstate pps;
	
	ps->log.curr_error_num = 0;
	pps = Init_Plane_Position_State();
	error_type = Init_Plane_Control_State(&pcs);
	if(error_type == NOERROR)
	{
		ps->position = pps;
		ps->control = pcs;
		return NOERROR;
	}
	else return error_type;
}


/******************** The following functions manipulate the positional settings of the plane */

/* Get_Desired_Turn returns the desired turn variable stored in the plane state 
* structure
* inputs:  plane->contains positional info
* returns: the desired turn state of the plane (LEFT, RIGHT, STRAIGHT)
*/
turnstate Get_Desired_Turn(planestate plane)
{
	return plane.position.desired_turn;
}

/* Set_Desired_Turn sets the desired turn variable stored in the plane state
* structure. This variable determines which way the plane is trying to turn.
* inputs:  plane->contains positional info
*	   turn->the direction of the turn (LEFT, RIGHT, STRAIGHT)
*/
void Set_Desired_Turn(planestate *plane, turnstate turn)
{
	plane->position.desired_turn = turn;
}

/* Get_Desired_Altitude returns the desired altitude variable stored in the plane 
* state structure
* inputs:  plane->contains positional info
* returns: the desired altitude state of the plane (HIGHER, LOWER, LEVEL)
*/
altitudestate Get_Desired_Altitude(planestate plane)
{
	return plane.position.desired_altitude;
}

/* Set_Desired_Altitude sets the desired altitude variable stored in the plane 
* state structure. This variable determines whether the plane is trying to 
* rise, fall or remain level.
* inputs:  plane->contains positional info
*	   turn->the altitude change required (HIGHER, LOWER, LEVEL)
*/
void Set_Desired_Altitude(planestate *plane, altitudestate altitude)
{
	plane->position.desired_altitude = altitude;
}

/* Get_Wplist returns the list of waypoints stored in the plane state structure
* inputs:  plane->contains positional info
* returns: the list of waypoints the plane is to travel to
*/
wplist Get_Wplist(planestate plane)
{
	return plane.position.waypoints;
}

/* Store_Wplist stores a new set of waypoints in the plane state structure
* inputs:  plane->contains positional info
*	   waypoints->list of waypoints
*/
void Store_Wplist(planestate *plane, wplist waypoints)
{
	plane->position.waypoints = waypoints;
}

/* Get_Altitude retrieves the current altitude information of the plane
* inputs:  plane->contains positional info
* returns: the altitude of the plane
*/
double Get_Altitude(planestate plane)
{
	return plane.position.altitude;
}

/* Set_Altitude sets the current altitude variable information of the plane
* inputs:  plane->contains positional info
*	   altitude->the current altitude of the plane
*/
void Set_Altitude(planestate *plane, double altitude)
{
	plane->position.altitude = altitude;
}

/* Get_GPS_Message retrieves the message fragment stored in the plane structure
* inputs:  plane->contains the message fragment
* returns: the current message fragment
*/
message Get_GPS_Message(planestate plane)
{
	return plane.position.gps_mesg;
}

/* Set_GPS_Message updates the message fragment stored in the plane structure
* inputs:  plane->contains the old message fragment
*	   mesg->the current message fragment
*/
void Set_GPS_Message(planestate *plane, message mesg)
{
	plane->position.gps_mesg = mesg;
}

/* Get_Curr_Pos retrieves the current coordinate information of the plane
* inputs:  plane->contains positional info
* returns: the current coordinates of the plane
*/
position Get_Curr_Pos(planestate plane)
{
	return plane.position.curr_pos;
}

/* Set_Curr_Pos sets the current coordinate information of the plane
* inputs:  plane->contains positional info
*	   pos->the current coordinates of the plane
*/
void Set_Curr_Pos(planestate *plane, position pos)
{
	plane->position.curr_pos = pos;
}

/* Get_Curr_Wp retrieves the coordinates of the current waypoint the plane
* is heading to.
* inputs:  plane->contains positional info
* returns: the coordinates of the current waypoint
*/ 
position Get_Curr_Wp(planestate plane)
{
	return plane.position.curr_wp;
}

/* Set_Curr_Wp establishes the next waypoint the plane is to fly to
* inputs:  plane->contains positional info
* 	   curr_wpnum->the number of the next waypoint
* returns: INVALIDWPNUM if curr_wpnum out of range of list
*          NOERROR otherwise
*/
error Set_Curr_Wp(planestate *plane, int curr_wpnum)
{
	position pos;
	error error_type;
	
	error_type = Get_Wp(plane->position.waypoints, curr_wpnum, &pos);
	if(error_type == NOERROR)
	{
		plane->position.curr_wp = pos;
		return NOERROR;
	}
	else return error_type;
}

/* Store_Old_Position stores the old position so that it can be retrieved later
* (if an error occurs in getting the new position)
* inputs:  plane->contains positional info
*/
void Store_Old_Position(planestate *plane)
{
	plane->position.stored_pos = plane->position.curr_pos;
}

/* Restore_Old_Position restores the previously known position of the plane in
* the event that an error has ocurred in determining the new position
* inputs:  plane->contains positional info
*/
void Restore_Old_Position(planestate *plane)
{
	plane->position.curr_pos = plane->position.stored_pos;
}

/* Get_Curr_Heading retrieves the current heading information of the plane
* inputs:  plane->contains positional info
* returns: the currently known heading of the plane
*/
int Get_Curr_Heading(planestate plane)
{
	return plane.position.curr_heading;
}

/* Set_Curr_Heading sets the current heading information of the plane
* inputs:  plane->contains positional info
* 	   heading->the current heading of the plane
*/
void Set_Curr_Heading(planestate *plane, int heading)
{
	plane->position.curr_heading = heading;
}

/* Get_Req_Bearing retrieves the required bearing information of the plane
* inputs:  plane->contains positional info
* returns: the currently required bearing of the plane
*/
double Get_Req_Bearing(planestate plane)
{
	return plane.position.req_bearing;
}

/* Set_Req_Bearing sets the required bearing information of the plane
* inputs:  plane->contains positional info
* 	   heading->the currently required bearing of the plane
*/
void Set_Req_Bearing(planestate *plane, double bearing)
{
	plane->position.req_bearing = bearing;
}

/* Store_Old_Heading stores the old heading so that it can be retrieved later
* (if an error occurs in getting the new heading)
* inputs:  plane->contains positional info
*/
void Store_Old_Heading(planestate *plane)
{
	plane->position.stored_heading = plane->position.curr_heading;
}

/* Restore_Old_Heading restores the previously known heading of the plane in
* the event that an error has ocurred in determining the new heading
* inputs:  plane->contains positional info
*/
void Restore_Old_Heading(planestate *plane)
{
	plane->position.curr_heading = plane->position.stored_heading;
}

/* Get_Pitch retrieves the pitch information of the plane
* inputs:  plane->contains positional info
* returns: the pitch of the plane
*/
double Get_Pitch(planestate plane)
{
	return plane.position.pitch;
}

/* Set_Pitch sets the pitch information of the plane
* inputs:  plane->contains positional info
* 	   pitch->the pitch of the plane
*/
void Set_Pitch(planestate *plane, double pitch)
{
	plane->position.pitch = pitch;
}

/* Get_Bank_Angle retrieves the bank angle information of the plane
* inputs:  plane->contains positional info
* returns: the bank angle of the plane
*/
double Get_Bank_Angle(planestate plane)
{
	return plane.position.bank_angle;
}

/* Set_Bank_Angle sets the bank angle information of the plane
* inputs:  plane->contains positional info
* 	   bank_angle->the bank angle of the plane
*/
void Set_Bank_Angle(planestate *plane, double bank_angle)
{
	plane->position.bank_angle = bank_angle;
}

/* Get_Airspeed retrieves the airspeed information of the plane
* inputs:  plane->contains positional info
* returns: the airspeed of the plane
*/
double Get_Airspeed(planestate plane)
{
	return plane.position.airspeed;
}

/* Set_Airspeed sets the airspeed information of the plane
* inputs:  plane->contains positional info
* 	   airspeed->the airspeed of the plane
*/
void Set_Airspeed(planestate *plane, double airspeed)
{
	plane->position.airspeed = airspeed;
}	

/* Get_Best_Correction retrieves the best correction information of the current
* turn of the plane
* inputs:  plane->contains positional info
* returns: the best correction of the current turn of the plane
*/
double Get_Best_Correction(planestate plane)
{
	return plane.position.best_correction;
}

/* Set_Best_Correction sets the best correction information of the current turn
* of the plane
* inputs:  plane->contains positional info
* 	   correction->the current correction required by the plane
*/
void Set_Best_Correction(planestate *plane, double correction)
{
	plane->position.best_correction = correction;
}

/* Get_Improve_Count retrieves the number of times the plane has failed to improve
* its heading
* inputs:  plane->contains positional info
* returns: the bearing improvement count of the plane
*/
int Get_Improve_Count(planestate plane)
{
	return plane.position.improve_count;
}

/* Set_Improve_count updates the improve count
* inputs:  plane->contains positional info
* 	   count->the current number of times the plane has failed to improve its bearing
*/
void Set_Improve_Count(planestate *plane, int count)
{
	plane->position.improve_count = count;
}

/* Heading_Improvement determines whether the plane is making any improvement in heading
* given the current turn direction
* inputs:  plane->plane information     
*	   curr_correction->the current correction the plane needs to make
*/
void Heading_Improvement(planestate *plane, double curr_correction)
{
	if(Get_Desired_Turn(*plane)==RIGHT)				/* The new corr must be > the best corr to improve */
	{
		if(curr_correction <= plane->position.best_correction)
		{
			if(plane->position.best_correction > 170 && curr_correction < -170)	/* In this case it is deemed */
			{									/* likely that the plane has */
				plane->position.best_correction = curr_correction;		/* just passed the 180 180->(-180) */
				plane->position.improve_count = 0;				/* boundary. Has improved. */
			}
			else								/* Otherwise the plane has not improved */
			{
				plane->position.improve_count++;			/* Increase count by 1 */
				if(plane->position.improve_count >= 20)			/* If no improvement for 20 cycles */
				{							/* then turn the other direction */
					Turn_Left(plane);		
					plane->position.improve_count = 0;		/* reset improvement count for new turn */
				}
			}
		}
		else									/* Else the plane has improved */
		{
			plane->position.best_correction = curr_correction;		/* Update the best correction */
			plane->position.improve_count = 0;				/* Reset the improvement count */
		}
	}
	if(Get_Desired_Turn(*plane)==LEFT)				/* The new corr must be < the best corr to improve */
	{
		if(curr_correction >= plane->position.best_correction)
		{
			if(plane->position.best_correction < -170 && curr_correction > 170)	/* In this case it is deemed */
			{									/* likely that the plane has */
				plane->position.best_correction = curr_correction;		/* just passed the (-180)->180 */
				plane->position.improve_count = 0;				/* boundary. Has improved. */
			}									/* Assuming plane cant turn */
												/* >340 degrees, in wrong */
												/* direction, in 1 cycle :) */
			else				/* Otherwise the plane has not improved */
			{
				plane->position.improve_count++;
				if(plane->position.improve_count >= 20)
				{
					Turn_Right(plane);
					plane->position.improve_count = 0;
				}
			}
		}
		else									/* Else the plane has improved */
		{
			plane->position.best_correction = curr_correction;		/* Update the best correction */
			plane->position.improve_count = 0;				/* Reset the improvement count */
		}
	}
}

/* Get_Log returns the error log so new messages can be added 
* inputs:  plane->contains the error log
* returns: errorlog of all errors to date
*/
errorlog Get_Log(planestate plane)
{
	return plane.log;
}

/* Set_Log updates the messages in the error log with new messages
* inputs:  plane->contains the old log for overwriting
* 	   log->conatins the new set of error messages
*/
void Set_Log(planestate *plane, errorlog log)
{
	plane->log = log;
}

/* Log_Event performs the procedure required to log an event

* inputs:  plane->contains old log and positional data

*	   error_type->identifies the event that occured

*/

void Log_Event(planestate *plane, error event)

{

	errorlog log;

	

	log = Get_Log(*plane);		/* retrieve the log from the plane data structure */

	

	/* update the log with the event, position, and heading */

	Add_Event(&log, event, plane->position.curr_pos, plane->position.curr_wp, plane->position.curr_heading, plane->position.req_bearing);	

	

	Set_Log(plane, log);		/* store the new information in the plane data struct */

}



/* Add_Event adds an event to the message log. Message is not added if capacity
* has been reached.
* inputs:  log->contains the old error messages (will be added to)
*	   tag->an identifier for the event (type error)
*	   gps_pos->current position of the aircraft
*	   wp_pos->position of current waypoint
*	   heading->current heading of the aircraft
*	   bearing->bearing required to reach current waypoint
*/
void Add_Event(errorlog *log, error tag, position gps_pos, position wp_pos, int heading, double bearing)
{
	if(log->curr_error_num < MAX_ERROR_MESGS )
	{
		log->tag[log->curr_error_num] = tag;
		log->gps_pos[log->curr_error_num] = gps_pos;
		log->wp_pos[log->curr_error_num] = wp_pos;
		log->heading[log->curr_error_num] = heading;
		log->bearing[log->curr_error_num] = bearing;
		log->curr_error_num++;
	}
}

/* Upload_Error_Log uploads the error log to a PC
* inputs:  plane->contains the log of all events that have occured
*	   type->the grouping/order of the upload (chronological order or event grouping)
* returns: SENDERROR if an error occured during the send (resend the data!)
*	   NOERROR otherwise
*/
error Upload_Error_Log(planestate plane, upltype type)
{
	char curr_mesg[MAX_LOG_SIZE];
	char event[MAX_EVENT_LENGTH];
	int mesg_ptr = 0;
	int i;
	int result = 0;
	errorlog temp_log;
	
	temp_log.curr_error_num = 0;
	
	if(type == TIME)
	{
		for(i=0;i<plane.log.curr_error_num;i++)
		{
			/* write each of the stored messages to the curr_mesg array */
			LCDSetPrintf(0,0,".");
			Convert_Tag2Event(plane.log.tag[i], event);
			mesg_ptr += sprintf( curr_mesg + mesg_ptr, "%s%d pos %f,%f wp %f,%f h: %d, b: %f\n",
									event,
									plane.log.tag[i],
									plane.log.gps_pos[i].latitude,
									plane.log.gps_pos[i].longitude,
									plane.log.wp_pos[i].latitude,
									plane.log.wp_pos[i].longitude,
									plane.log.heading[i],
									plane.log.bearing[i]);
			LCDSetPrintf(0,0,"_");
		}
	}
	else if(type == EVENT)
	{
		for(i=0;i<plane.log.curr_error_num;i++)
		{
			if(plane.log.tag[i] == LOGPOSITION)
			{
				temp_log.tag[temp_log.curr_error_num] = plane.log.tag[i];
				temp_log.gps_pos[temp_log.curr_error_num] = plane.log.gps_pos[i];
				temp_log.wp_pos[temp_log.curr_error_num] = plane.log.wp_pos[i];
				temp_log.heading[temp_log.curr_error_num] = plane.log.heading[i];
				temp_log.bearing[temp_log.curr_error_num] = plane.log.bearing[i]; 
				temp_log.curr_error_num++;
			}
		}
		LCDPrintf("logs sorted\n");
		for(i=0;i<plane.log.curr_error_num;i++)
		{
			if(plane.log.tag[i] == SWITCHMANUAL || plane.log.tag[i] == SWITCHAUTO)
			{
				temp_log.tag[temp_log.curr_error_num] = plane.log.tag[i];
				temp_log.gps_pos[temp_log.curr_error_num] = plane.log.gps_pos[i];
				temp_log.wp_pos[temp_log.curr_error_num] = plane.log.wp_pos[i];
				temp_log.heading[temp_log.curr_error_num] = plane.log.heading[i];
				temp_log.bearing[temp_log.curr_error_num] = plane.log.bearing[i]; 
				temp_log.curr_error_num++;
			}
		}
		LCDPrintf("switches sorted\n");
		for(i=0;i<plane.log.curr_error_num;i++)
		{
			if(plane.log.tag[i] != SWITCHMANUAL && plane.log.tag[i] != SWITCHAUTO && plane.log.tag[i] != LOGPOSITION)
			{
				temp_log.tag[temp_log.curr_error_num] = plane.log.tag[i];
				temp_log.gps_pos[temp_log.curr_error_num] = plane.log.gps_pos[i];
				temp_log.wp_pos[temp_log.curr_error_num] = plane.log.wp_pos[i];
				temp_log.heading[temp_log.curr_error_num] = plane.log.heading[i];
				temp_log.bearing[temp_log.curr_error_num] = plane.log.bearing[i]; 
				temp_log.curr_error_num++;
			}
		}
		LCDPrintf("other sorted\n");
		if(temp_log.curr_error_num != plane.log.curr_error_num)
		{
			curr_mesg[0] = '!';
			mesg_ptr = 1;
			LCDClear();
			LCDPrintf("transfer incomplete!\nlog:%d\ntemplog:%d\n",temp_log.curr_error_num, plane.log.curr_error_num);
			OSWait(VLONG);
		}
		else
		{
			LCDPrintf("number of logs:%d\n",temp_log.curr_error_num);
			OSWait(LONG);
			for(i=0;i<temp_log.curr_error_num;i++)
			{
				/* write each of the stored messages to the curr_mesg array */
				LCDSetPrintf(0,0,".");
				Convert_Tag2Event(temp_log.tag[i], event);
				mesg_ptr += sprintf( curr_mesg + mesg_ptr, "%s%d pos %f,%f wp %f,%f h: %d, b: %f\n",
									event,
									temp_log.tag[i],
									temp_log.gps_pos[i].latitude,
									temp_log.gps_pos[i].longitude,
									temp_log.wp_pos[i].latitude,
									temp_log.wp_pos[i].longitude,
									temp_log.heading[i],
									temp_log.bearing[i]);
				LCDSetPrintf(0,0,"_");
			}
		}
	}	
	else
	{
		curr_mesg[0] = '!';
		mesg_ptr = 1;
		LCDClear();
		LCDPrintf("Upload type incorrect!!!\n");
		OSWait(VLONG);
	}
	
	result = OSInitRS232(PC_BAUDRATE, RTSCTS, PC_PORTNUM);
	if(result != 0)
	{
	    LCDPrintf("RS232 init fail");
	    return SENDERROR;
	}
	
	result = OSFlushOutRS232( PC_PORTNUM );
	if( result != 0 ) {
		if( result == 10 ) {
			LCDPrintf( "Bad interface\n" );
			return SENDERROR;
		}
		LCDPrintf( "Unknown error\n" );
		return SENDERROR;
	}
	
	for(i=0;i<mesg_ptr;i++)		/* Send each character in the curr_mesg array */
	{
		result = OSSendCharRS232(curr_mesg[i], PC_PORTNUM);
		LCDPrintf("%c",curr_mesg[i]);
		if( result != 0 ) {
			if( result == 3 ) {
				LCDPrintf( "Send timed out." );
				return SENDERROR;
			}
			if( result == 10 ) {
				LCDPrintf( "Bad interface\n" );
				return SENDERROR;
			}
			LCDPrintf( "Unknown error\n" );
			return SENDERROR;
		}
		LCDPrintf("%c",curr_mesg[i]);

	}		
	return NOERROR;		
}

/* Convert_Tag2Event changes an error tag into an event string
* If any new errors are added, this function must be updated
* inputs:  tag->the event label
*	   event_str->the string which defines the event
*/
void Convert_Tag2Event(error tag, char *event_str)
{
	char *temp_event_str = NULL;
	
	switch(tag)
	{
		case NOERROR:
			temp_event_str = "noerror ";
			break;
		case SWITCHDETECT:
			temp_event_str = "switchnotdetected ";
			break;
		case SERVOERROR:
			temp_event_str = "servoerror ";
			break;
		case INITERROR:
			temp_event_str = "errorduringinitialisation";
			break;
		case IDERROR:
			temp_event_str = "servoidincorrect ";
			break;
		case INVALIDSERVO:
			temp_event_str = "servohandleincorrect ";
			break;
		case LIMITERROR:
			temp_event_str = "servoangleoutofrange ";
			break;
		case LIMITCROSS:
			temp_event_str = "minlimit>maxlimit ";
			break;
		case ABSOLUTELIMITERROR:
			temp_event_str = "limitsetoutsideabsolute ";
			break;
		case COMPASSERROR:
			temp_event_str = "compassiniterror ";
			break;
		case COMPASSHEADINGERROR:
			temp_event_str = "headingerror ";
			break;
		case NOHEADING:
			temp_event_str = "noheading ";
			break;
		case GPSERROR:
			temp_event_str = "GPSiniterror ";
			break;
		case GPSPOSERROR:
			temp_event_str = "GPSpositionerror ";
			break;
		case MESGTOOLONG:
			temp_event_str = "GPSmesgtoolong ";
			break;
		case NOPOSITION:
			temp_event_str = "noposition ";
			break;
		case NOWPS:
			temp_event_str = "nowaypoints ";
			break;
		case INVALIDWPNUM:
			temp_event_str = "waypointnumoutofrange ";
			break;
		case WPFILEERROR:
			temp_event_str = "waypointfileerror ";
			break;
		case DIVZERO:
			temp_event_str = "dividebyzero ";
			break;
		case NEGVAL:
			temp_event_str = "-vevalue(shouldbe+ve) ";
			break;
		case COMMERROR:
			temp_event_str = "communicationserror ";
			break;
		case SENDERROR:
			temp_event_str = "sendingerror ";
			break;
		case SWITCHMANUAL:
			temp_event_str = "manualswitch ";
			break;
		case SWITCHAUTO:
			temp_event_str = "autopilotswitch ";
			break;
		case LOGPOSITION:
			temp_event_str = "log ";
			break;
		case ATWP:
			temp_event_str = "waypointreached ";
			break;
		case FLIGHTCOMPLETE:
			temp_event_str = "flightcompleted ";
			break;
		default: temp_event_str = "unknownerror ";
	}
	strncpy(event_str, temp_event_str, MAX_EVENT_LENGTH);
}


/******************** The following functions update the servo states and settings of the plane */

/* Get_Ctrl_Surface returns the relevant control surface information from the 
* plane control struct
* inputs:  plane->contains the control information
*	   ctrl_surface_name->the label of the control surface that is to be returned
*	   ctrl_surface->the control surface being returned
* returns: INVALIDSERVO if the servo label is out of range
*	   NOERROR otherwise
*/
error Get_Ctrl_Surface(planestate plane, servoconsts ctrl_surface_name, ctrlsurface *ctrl_surface)
{
	if(ctrl_surface_name == RUDDER)
	{
		*ctrl_surface = plane.control.rudder;
		return NOERROR;
	}
	else if(ctrl_surface_name == AILERON) 
	{
		*ctrl_surface = plane.control.banking;
		return NOERROR;
	}
	else if(ctrl_surface_name == ELEVATOR) 
	{
		*ctrl_surface = plane.control.elevator;
		return NOERROR;
	}
	else if(ctrl_surface_name == THROTTLE) 
	{
		*ctrl_surface = plane.control.throttle;
		return NOERROR;
	}
	else return INVALIDSERVO;
}

/* Set_Ctrl_Surface stores new control surface information for the surface
* inputs:  plane->contains the control information
* 	   ctrl_surface->the control surface to be updated
*	   ctrl_surface_name->the name of the ctrl_surface to be updated
* returns: INVALIDSERVO if the servo label is out of range
*	   NOERROR otherwise
*/
error Set_Ctrl_Surface(planestate *plane, ctrlsurface ctrl_surface, servoconsts ctrl_surface_name)
{
	if(ctrl_surface_name == RUDDER)
	{
		plane->control.rudder = ctrl_surface;
		return NOERROR;
	}
	else if(ctrl_surface_name == AILERON) 
	{
		plane->control.banking = ctrl_surface;
		return NOERROR;
	}
	else if(ctrl_surface_name == ELEVATOR) 
	{
		plane->control.elevator = ctrl_surface;
		return NOERROR;
	}
	else if(ctrl_surface_name == THROTTLE) 
	{
		plane->control.throttle = ctrl_surface;
		return NOERROR;
	}
	else return INVALIDSERVO;
}

/* Get_Rudder_State retrieves the current state of the rudder
* inputs:  plane->contains the control information
* returns: the turn state of the rudder (LEFT, RIGHT, STRAIGHT);

*/
turnstate Get_Rudder_State(planestate plane)
{
	return plane.control.rudder_state;
}

/* Set_Rudder_State sets the current state of the rudder
* inputs:  plane->contains control information
*	   rudder->the turn state required of the rudder
*/
void Set_Rudder_State(planestate *plane, turnstate rudder)
{
	plane->control.rudder_state = rudder;
}

/* This function will change the servos to reflect the current states in the 
* plane control structure. If any new surfaces are required then this function
* must be updated.
* inputs:  plane->plane structure containing the required servo settings
* returns: error occured or not (SERVOERROR if failed, NOERROR otherwise)
*/
error Update_Servos(planestate *plane)
{
	error error_type;

	error_type = Move_Ctrl_Surface_Servo(plane->control.rudder);
	return error_type;
}