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

hardware contains all of the functions that manipulate hardware on the eyebot.
***************************************************************************/
#include "hardware.h"

/************************* Functions to manipulate the control surface structure */

/* Init_Ctrl_Surface initialises a servo and sets the limits and neutral point
* inputs:  ctrl_surface_name->the name of the servo you wish to initialise (servoconsts)
* returns: SERVOERROR if the servo was not correctly initialised, 
*	   IDERROR if the identifier was not valid
*	   NOERROR otherwise
*/
error Init_Ctrl_Surface(servoconsts ctrl_surface_name, ctrlsurface *ctrl_surface)
{
	ctrl_surface->angle = 0;
	ctrl_surface->id = ctrl_surface_name;

	if(ctrl_surface->id == RUDDER)
	{
		ctrl_surface->max_limit = MAX_ANGLE_RUDDER;	/* set the limits to those defined in planeconst.h */
		ctrl_surface->min_limit = MIN_ANGLE_RUDDER;	/* with neutral half way between */
		ctrl_surface->neutral_point = 0;
		ctrl_surface->servo = SERVOInit(SERVO1); 	/* initialise the servo handle */
		if(ctrl_surface->servo == 0) 			/* returns 0 if it failed */
		{
			LCDPrintf("ServoRudd Error ");
			return SERVOERROR;
		}
		else return NOERROR;
	}
	else if(ctrl_surface->id == AILERON)
	{
		ctrl_surface->max_limit = MAX_ANGLE_AILERON;
		ctrl_surface->min_limit = MIN_ANGLE_AILERON;
		ctrl_surface->neutral_point = 0;
		ctrl_surface->servo = SERVOInit(SERVO2); 
		if(ctrl_surface->servo == 0)
		{
			LCDPrintf("ServoAiler Error\n");
			return SERVOERROR;
		}
		else return NOERROR;
	}
	else if(ctrl_surface->id == ELEVATOR)
	{
		ctrl_surface->max_limit = MAX_ANGLE_ELEVATOR;
		ctrl_surface->min_limit = MIN_ANGLE_ELEVATOR;
		ctrl_surface->neutral_point = 0;
		ctrl_surface->servo = SERVOInit(SERVO3); 
		if(ctrl_surface->servo == 0)
		{
			LCDPrintf("ServoElev Error\n");
			return SERVOERROR;
		}
		else return NOERROR;
	}
	else if(ctrl_surface->id == THROTTLE)
	{
		ctrl_surface->max_limit = MAX_SETTING_THROTTLE;
		ctrl_surface->min_limit = MIN_SETTING_THROTTLE;
		ctrl_surface->neutral_point = 0;
		ctrl_surface->servo = SERVOInit(SERVO4); 
		if(ctrl_surface->servo == 0)
		{
			LCDPrintf("ServoThrott Error\n");
			return SERVOERROR;
		}
		else return NOERROR;
	}
	else
	{
		LCDPrintf("CtrlS ID invalid");
		return IDERROR;
	}
}

/* Get_Angle retrieves the angle the control surface is currently set at
* inputs:  ctrl_surface->structure containing the control surface info
* returns: angle the surface is set to
*/
double Get_Angle(ctrlsurface ctrl_surface)
{
	return ctrl_surface.angle;
}

/* Set_Angle sets the angle the control surface should be set to. This will be 
* updated to an actual servo value when Update_Servo is called.
* inputs:  ctrl_surface->structure containing the control surface info
*	   angle->the angle the control surface should be set to
*	   mode->if the mode is SET then use the absolute limits
*		 if NORMAL then use the user defined limits
* returns: LIMITERROR if the angle is outside of the servo's MAX/MIN limit, 
*	   NOERROR otherwise
*/
error Set_Angle(ctrlsurface *ctrl_surface, double angle, anglesetmode mode)
{
	double max_limit = 0, min_limit = 0;
	
	if(mode == SET)
	{
		if(ctrl_surface->id == RUDDER)
		{
			max_limit = MAX_ANGLE_RUDDER;
			min_limit = MIN_ANGLE_RUDDER;
		}
		if(ctrl_surface->id == AILERON)
		{
			max_limit = MAX_ANGLE_AILERON;
			min_limit = MIN_ANGLE_AILERON;
		}
		if(ctrl_surface->id == ELEVATOR)
		{
			max_limit = MAX_ANGLE_ELEVATOR;
			min_limit = MIN_ANGLE_ELEVATOR;
		}
		if(ctrl_surface->id == THROTTLE)
		{
			max_limit = MAX_SETTING_THROTTLE;
			min_limit = MIN_SETTING_THROTTLE;
		}
	}
	else
	{
		max_limit = ctrl_surface->max_limit;
		min_limit = ctrl_surface->min_limit;
	}
	if(angle<=max_limit && angle>=min_limit)
	{			/* make sure it's within the user set limits */
		ctrl_surface->angle = angle;
		return NOERROR;
	}
	else return LIMITERROR;
}

/* Get_Servo returns the handle for the servo so that it can be manipulated
* inputs:  ctrl_surface->the control surface you wish to retrieve the servo from
* returns: the handle of the servo that was requested.
*/
ServoHandle Get_Servo(ctrlsurface ctrl_surface)
{
	return ctrl_surface.servo;
}

/* Get_Min_Limits returns the minimum value the servo should be set to
* inputs:  ctrl_surface->structure containing the control surface info
* returns: the minimum limit of that servo
*/
double Get_Min_Limit(ctrlsurface ctrl_surface)
{
	return ctrl_surface.min_limit;
}

/* Set_Min_Limits sets the minimum value the servo can be set to
* inputs:  ctrl_surface->structure containing the control surface info
*	   min_limit->the minimum value the servo should be set to
* returns: ABSOLUTELIMITERROR if the limit entered is lower than the possible minimum,
*	   LIMITCROSS if the limit entered is higher than the current maximum limit,
*	   IDERROR if the identifier was not valid
*	   NOERROR otherwise
*/
error Set_Min_Limit(ctrlsurface *ctrl_surface, double min_limit)
{
	if(ctrl_surface->id == RUDDER)
	{
		if(min_limit>=MIN_ANGLE_RUDDER)				/* make sure limit is above absolute min */
		{
			if(min_limit<ctrl_surface->max_limit && min_limit<ctrl_surface->neutral_point)
			{						/* make sure it is below the max limit and neutral */
				ctrl_surface->min_limit = min_limit;
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	else if(ctrl_surface->id == AILERON)
	{
		if(min_limit>=MIN_ANGLE_AILERON)
		{
			if(min_limit<ctrl_surface->max_limit && min_limit<ctrl_surface->neutral_point)
			{
				ctrl_surface->min_limit = min_limit;
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	else if(ctrl_surface->id == ELEVATOR)
	{
		if(min_limit>=MIN_ANGLE_ELEVATOR)
		{
			if(min_limit<ctrl_surface->max_limit && min_limit<ctrl_surface->neutral_point)
			{
				ctrl_surface->min_limit = min_limit;
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	else if(ctrl_surface->id == THROTTLE)
	{
		if(min_limit>=MIN_SETTING_THROTTLE)
		{
			if(min_limit<ctrl_surface->max_limit && min_limit<ctrl_surface->neutral_point)
			{
				ctrl_surface->min_limit = min_limit;
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	else return IDERROR;
	return NOERROR;
}

/* Get_Max_Limits returns the maximum value the servo should be set to
* inputs:  ctrl_surface->structure containing the control surface info
* returns: the maximum limit of that servo
*/
double Get_Max_Limit(ctrlsurface ctrl_surface)
{
	return ctrl_surface.max_limit;
}

/* Set_Max_Limit sets the maximum value the servo can be set to
* inputs:  ctrl_surface->structure containing the control surface info
*	   max_limit->the maximum value the servo should be set to
* returns: ABSOLUTELIMITERROR if the limit entered is higher than the possible maximum,
*	   LIMITCROSS if the limit entered is lower than the current minimum limit,
*	   IDERROR if the identifier was not valid
*	   NOERROR otherwise
*/
error Set_Max_Limit(ctrlsurface *ctrl_surface, double max_limit)
{
	if(ctrl_surface->id == RUDDER)
	{
		if(max_limit<=MAX_ANGLE_RUDDER)				/* make sure limit is below absolute max */
		{
			if(max_limit>ctrl_surface->min_limit && max_limit>ctrl_surface->neutral_point)
			{						/* make sure it is above the min limit and neutral */
				ctrl_surface->max_limit = max_limit;	
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	else if(ctrl_surface->id == AILERON)
	{
		if(max_limit<=MAX_ANGLE_AILERON)
		{
			if(max_limit>ctrl_surface->min_limit && max_limit>ctrl_surface->neutral_point)
			{
				ctrl_surface->max_limit = max_limit;
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	else if(ctrl_surface->id == ELEVATOR)
	{
		if(max_limit<=MAX_ANGLE_ELEVATOR)
		{
			if(max_limit>ctrl_surface->min_limit && max_limit>ctrl_surface->neutral_point)
			{
				ctrl_surface->max_limit = max_limit;
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	else if(ctrl_surface->id == THROTTLE)
	{
		if(max_limit<=MAX_SETTING_THROTTLE)
		{
			if(max_limit>ctrl_surface->min_limit && max_limit>ctrl_surface->neutral_point)
			{
				ctrl_surface->max_limit = max_limit;
			}
			else return LIMITCROSS;
		}
		else return ABSOLUTELIMITERROR;
	}
	return NOERROR;
}

/* Get_Neutral_Point returns the neutral angle of the control surface
* inputs:  ctrl_surface->contains the neutral point
* returns: the neutral point of the control surface
*/
double Get_Neutral_Point(ctrlsurface ctrl_surface)
{
	return ctrl_surface.neutral_point;
}

/* Set_Neutral_Point sets the neutral point of the control surface
* inputs:  ctrl_surface->contains the neutral point
*	   neutral_point->the neutral angle of the control surface
* returns: LIMITERROR if the neutral point is outside of the max and min limits
*	   NOERROR otherwise
*/
error Set_Neutral_Point(ctrlsurface *ctrl_surface, double neutral_point)
{
	if(ctrl_surface->min_limit < neutral_point && neutral_point < ctrl_surface->max_limit)
	{				/* make sure the neutral point is between the other two limits */
		ctrl_surface->neutral_point = neutral_point;
		return NOERROR;
	}
	else return LIMITERROR;	
}

/* Move_Ctrl_Surface_Servo reads the angle of a control surface and sets the servo 
* to that angle.
* inputs:  ctrl_surface->contains the angle and the servo handle
* returns: SERVOERROR if the servo was unable to be set to that value
*	   IDERROR if the identifier was not valid
*	   NOERROR if the servo was set correctly
*/
error Move_Ctrl_Surface_Servo(ctrlsurface ctrl_surface)
{
	int setting, servostate;
	error error_type;
	
	error_type = Convert_Angle2Servo(ctrl_surface, ctrl_surface.angle, &setting);	/* convert the angle to a servo setting */
	if(error_type == NOERROR)
	{
		servostate = SERVOSet(ctrl_surface.servo, setting);		/* set the servo */
		if(servostate == -1) return SERVOERROR;				/* -1 means an error occured */
		else return NOERROR;
	}
	else return error_type;
}

/* Establish_Servo_Limits sets the three limits of the control surface passed in.
* inputs:  ctrl_surface->contains the settings for the control surface
* returns: An error indicating a problem has occured
*	   NOERROR otherwise
*/
error Establish_Servo_Limits(ctrlsurface *ctrl_surface)
{
	double angle;
	int setting, key;
	error error_type;
	bool valid;
	
	/*********** Set the minimum limit ***********/
	
	LCDClear();
	LCDPrintf("Minimum Limit\n");
	LCDMenu("+","-","SET","END");
	key = 0;
	angle = 0;
	setting = 0;
	angle = Get_Min_Limit(*ctrl_surface);			/* get the current minimum limit */
	error_type = Set_Angle(ctrl_surface, angle, SET);	/* set the surface to that angle so the user can see it */
	if(error_type == NOERROR)
	{
		error_type = Move_Ctrl_Surface_Servo(*ctrl_surface);
		if(error_type == NOERROR)
		{
			error_type = Convert_Angle2Servo(*ctrl_surface, angle, &setting);	/* determine the servo setting from the angle */
			if(error_type == NOERROR)
			{
				valid = FALSE;
				do
				{
					do
					{
						key = 0;
						while(key == 0)
						{
							key = Check_Input();
						}
						if(key == KEY1)
						{
							if((setting+5)<=255) setting+=5;	/* if + was pressed increment by 5 */
						}
						if(key == KEY2)
						{
							if((setting-5)>=0) setting-=5;		/* if - was pressed decrement by 5 */
						}
						error_type = Convert_Servo2Angle(*ctrl_surface, setting, &angle);
						if(error_type == NOERROR)			/* convert the servo setting to an angle */
						{	
							error_type = Set_Angle(ctrl_surface, angle, SET);
							LCDPrintf("ang:%f\n",(float)angle);		/* then set the control surface to that angle */
							if(error_type == NOERROR)
							{
								error_type = Move_Ctrl_Surface_Servo(*ctrl_surface);
								if(error_type != NOERROR) return error_type;
							}
							else return error_type;
						}
						else return error_type;
					}while(key!=KEY3 && key!=KEY4);
					if(key == KEY3)						 /* once the user has decided on a setting */
					{
						error_type = Set_Min_Limit(ctrl_surface, angle); /* try and set the limit */
						if(error_type == NOERROR) 
						{
							LCDPrintf("Min Limit Set\n");
							valid = TRUE;
						}
						else if(error_type == LIMITCROSS) 
						{
							LCDPrintf("Min Limit > Max Limit\n");
							valid = FALSE;
						}
						else 
						{
							LCDPrintf("Limit Set outside of absolute\n");
							valid = FALSE;
						}
					}
					if(key == KEY4) return NOERROR;
				}while(valid == FALSE);
			}
			else return error_type;
		}
		else return error_type;
	}

	
	/*********** Set the Neutral Point ***********/
	/* this has an identical procedure to setting the minimum limit */
	
	LCDClear();
	LCDPrintf("Neutral Point\n");
	LCDMenu("+","-","SET","END");
	key = 0;
	angle = 0;
	setting = 0;
	angle = Get_Neutral_Point(*ctrl_surface);
	error_type = Set_Angle(ctrl_surface, angle, SET);
	if(error_type == NOERROR)
	{
		error_type = Move_Ctrl_Surface_Servo(*ctrl_surface);
		if(error_type == NOERROR)
		{
			error_type = Convert_Angle2Servo(*ctrl_surface, angle, &setting);
			if(error_type == NOERROR)
			{
				valid = FALSE;
				do
				{
					do
					{
						key = 0;
						while(key == 0)
						{
							key = Check_Input();
						}
						if(key == KEY1)
						{
							if((setting+5)<=255) setting+=5;
						}
						if(key == KEY2)
						{
							if((setting-5)>=0) setting-=5;
						}
						error_type = Convert_Servo2Angle(*ctrl_surface, setting, &angle);
						if(error_type == NOERROR)
						{	
							error_type = Set_Angle(ctrl_surface, angle, SET);
							LCDPrintf("ang:%f\n",(float)angle);
							if(error_type == NOERROR)
							{
								error_type = Move_Ctrl_Surface_Servo(*ctrl_surface);
								if(error_type != NOERROR) return error_type;
							}
							else return error_type;
						}
						else return error_type;
					}while(key!=KEY3 && key!=KEY4);
					if(key == KEY3)
					{
						error_type = Set_Neutral_Point(ctrl_surface, angle);
						if(error_type == NOERROR) 
						{
							LCDPrintf("Neutral Point Set\n");
							valid = TRUE;
						}
						else 
						{
							LCDPrintf("Neutral Point set outside of limits\n");
							valid = FALSE;
						}
					}
					if(key == KEY4) return NOERROR;
				}while(valid == FALSE);
			}
			else return error_type;
		}
		else return error_type;
	}

	
	/*********** Set the maximum limit ***********/
	/* this has an identical procedure to setting the minimum limit */
	
	LCDClear();
	LCDPrintf("Maximum Limit\n");
	LCDMenu("+","-","SET","END");
	key = 0;
	angle = 0;
	setting = 0;
	angle = Get_Max_Limit(*ctrl_surface);
	error_type = Set_Angle(ctrl_surface, angle, SET);
	if(error_type == NOERROR)
	{
		error_type = Move_Ctrl_Surface_Servo(*ctrl_surface);
		if(error_type == NOERROR)
		{
			error_type = Convert_Angle2Servo(*ctrl_surface, angle, &setting);
			if(error_type == NOERROR)
			{
				valid = FALSE;
				do
				{
					do
					{
						key = 0;
						while(key == 0)
						{
							key = Check_Input();
						}
						if(key == KEY1)
						{
							if((setting+5)<=255) setting+=5;
						}
						if(key == KEY2)
						{
							if((setting-5)>=0) setting-=5;
						}
						error_type = Convert_Servo2Angle(*ctrl_surface, setting, &angle);
						if(error_type == NOERROR)
						{	
							error_type = Set_Angle(ctrl_surface, angle, SET);
							LCDPrintf("ang:%f\n",(float)angle);
							if(error_type == NOERROR)
							{
								error_type = Move_Ctrl_Surface_Servo(*ctrl_surface);
								if(error_type != NOERROR) return error_type;
							}
							else return error_type;
						}
						else return error_type;
					}while(key!=KEY3 && key!=KEY4);
					if(key == KEY3)
					{
						error_type = Set_Max_Limit(ctrl_surface, angle);
						if(error_type == NOERROR) 
						{
							LCDPrintf("Max Limit Set\n");
							valid = TRUE;
						}
						else if(error_type == LIMITCROSS) 
						{
							LCDPrintf("Max Limit < Min Limit\n");
							valid = FALSE;
						}
						else 
						{
							LCDPrintf("Limit Set outside of absolute\n");
							valid = FALSE;
						}
					}
					if(key == KEY4) return NOERROR;
				}while(valid == FALSE);
			}
			else return error_type;
		}
		else return error_type;
	}
	return NOERROR;
}					

/* Convert_Servo2Angle converts a servo setting into a control surface angle
* inputs:  ctrl_surface->contains the limit settings
*	   setting->the servo setting to be converted into an angle
*	   angle->the angle that the setting was converted into
* returns: IDERROR if the servo id was not recognised
*	   NOERROR otherwise
*/
error Convert_Servo2Angle(ctrlsurface ctrl_surface, int setting, double *angle)
{
	double ratio;
	double d_setting = (setting/255.0);					/* first find the ratio of the value*/
	if(ctrl_surface.id == RUDDER)						/* between 0 and 255 */
	{
		ratio = d_setting*(MAX_ANGLE_RUDDER - MIN_ANGLE_RUDDER);	/* multiply by the total angle difference between the 0 and 255 settings */
		*angle = ratio + MIN_ANGLE_RUDDER;				/* then shift it by the 0 value angle */
		return NOERROR;
	}
	else if(ctrl_surface.id == AILERON)
	{
		ratio = d_setting*(MAX_ANGLE_AILERON - MIN_ANGLE_AILERON);
		*angle = ratio + MIN_ANGLE_AILERON;
		return NOERROR;
	}
	else if(ctrl_surface.id == ELEVATOR)
	{
		ratio = d_setting*(MAX_ANGLE_ELEVATOR - MIN_ANGLE_ELEVATOR);
		*angle = ratio + MIN_ANGLE_ELEVATOR;
		return NOERROR;
	}
	else if(ctrl_surface.id == THROTTLE)
	{
		ratio = d_setting*(MAX_SETTING_THROTTLE - MIN_SETTING_THROTTLE);
		*angle = ratio + MIN_SETTING_THROTTLE;
		return NOERROR;
	}
	else return IDERROR;
}

/* Convert_Angle2Servo converts a servo setting into a control surface angle
* inputs:  ctrl_surface->contains the limit settings
*	   angle->the angle to be converted into a setting 
*	   setting->the servo setting that the angle was converted into
* returns: IDERROR if the servo id was not recognised
*	   NOERROR otherwise
*/
error Convert_Angle2Servo(ctrlsurface ctrl_surface, double angle,  int *setting)
{
	double ratio;
	
	if(ctrl_surface.id == RUDDER)
	{
		ratio = (angle - MIN_ANGLE_RUDDER)/(MAX_ANGLE_RUDDER - MIN_ANGLE_RUDDER);	/* determine the ratio of the input angle */
	}											/* between the absolute limits */
	else if(ctrl_surface.id == AILERON)
	{
		ratio = (angle - MIN_ANGLE_AILERON)/(MAX_ANGLE_AILERON - MIN_ANGLE_AILERON);
	}
	else if(ctrl_surface.id == ELEVATOR)
	{
		ratio = (angle - MIN_ANGLE_ELEVATOR)/(MAX_ANGLE_ELEVATOR - MIN_ANGLE_ELEVATOR);
	}
	else if(ctrl_surface.id == THROTTLE)
	{
		ratio = (angle - MIN_SETTING_THROTTLE)/(MAX_SETTING_THROTTLE - MIN_SETTING_THROTTLE);
	}
	else return IDERROR;
	
	*setting = floor(ratio*255+.5);	/* ratio*255 determines the servo setting */	
	return NOERROR;			/* floor(X + 0.5) causes the answer to be rounded before casting to int */
}

/* Test_Servo_Limits tests the accuracy of the limit settings for the control surface
* inputs:  ctrl_surface->contains limit information
*	   valid->passes the success or failure of the test out
* returns: An error if one occured in the test
*	   NOERROR otherwise
*/
error Test_Servo_Limits(ctrlsurface ctrl_surface, bool *valid)
{
	int key;
	double angle;
	error error_type;
	
	LCDClear();
	LCDPrintf("Minimum Limit\n");
	LCDMenu("OK","","","BAD");
	key = 0;
	angle = Get_Min_Limit(ctrl_surface);				/* set the surface to the minimum limit */
	error_type = Set_Angle(&ctrl_surface, angle, NORMAL);
	if(error_type == NOERROR)
	{
		error_type = Move_Ctrl_Surface_Servo(ctrl_surface);
		if(error_type == NOERROR)
		{
			do
			{
				key = Check_Input();
			}while(key != KEY1 && key != KEY4);		/* wait for button 1 or 4 to be pressed */ 
			if(key == KEY4)					
			{
				*valid = FALSE;
				return NOERROR;
			}
		}
		else return error_type;
	}
	else return error_type;
	
	LCDClear();
	LCDPrintf("Neutral Point\n");
	LCDMenu("OK","","","BAD");
	key = 0;
	angle = Get_Neutral_Point(ctrl_surface);			/* set the surface to the neutral point */
	error_type = Set_Angle(&ctrl_surface, angle, NORMAL);
	if(error_type == NOERROR)
	{
		error_type = Move_Ctrl_Surface_Servo(ctrl_surface);
		if(error_type == NOERROR)
		{
			do
			{
				key = Check_Input();
			}while(key!=KEY1 && key!=KEY4);			/* wait for button 1 or 4 to be pressed */ 
			if(key == KEY4)
			{
				*valid = FALSE;
				return NOERROR;
			}
		}
		else return error_type;
	}
	else return error_type;
	
	LCDClear();
	LCDPrintf("Maximum Limit\n");
	LCDMenu("OK","","","BAD");
	key = 0;
	angle = Get_Max_Limit(ctrl_surface);				/* set the surface to the maximum limit */
	error_type = Set_Angle(&ctrl_surface, angle, NORMAL);
	if(error_type == NOERROR)
	{
		error_type = Move_Ctrl_Surface_Servo(ctrl_surface);
		if(error_type == NOERROR)
		{
			do
			{
				key = Check_Input();
			}while(key!=KEY1 && key!=KEY4);			/* wait for button 1 or 4 to be pressed */ 
			if(key == KEY4)
			{
				*valid = FALSE;
				return NOERROR;
			}
		}
		else return error_type;
	}
	else return error_type;
	
	angle = Get_Neutral_Point(ctrl_surface);			/* reset the rudder to the neutral position */
	error_type = Set_Angle(&ctrl_surface, angle, NORMAL);
	if(error_type == NOERROR)
	{
		error_type = Move_Ctrl_Surface_Servo(ctrl_surface);
		if(error_type != NOERROR) return error_type;
	}
	else return error_type;
	
	*valid = TRUE;			/* if it got this far then all buttons pressed were 'OK' */
	return NOERROR;
}


/***************************************** Other hardware manipulating functions */

/* Initialise_Plane performs all of the required initialisation steps for the plane to be ready for flight.
* It will provide all feedback required through the eyebot LCD screen
* inputs:  plane->pointer to plane struct which will be initialised and contain all required info for flight
* returns: the error that occured or 
*	   NOERROR if initialisation was successfull	   
*/
error Initialise_Plane(planestate *plane)
{
	int i, key = 0;
	wplist wps;
	bool valid;
	error error_type;
	
	IRTVInit(SPACE_CODE, 15, 0, 0x3FF, SLOPPY_MODE, 4, -1);		/* First initialise the IR input */
	LCDPrintf("IR initialised\n");
	
	LCDPrintf("Press a button to cancel\n");			/* Then allow 1 sec to cancel autostart */
	OSWait(100);
	key = Check_Input();
	if(key != 0) return INITERROR;
	
	LCDClear();
	LCDPrintf("Starting prog\n");
		
	error_type = Init_Plane_State(plane);				/* Initialise the plane data structure */
	
	if(error_type != NOERROR) return error_type;
	
	
	/*********** Compass Test ************/
	
	valid = FALSE;
	valid = Init_Compass();						/* initialise compass */
	if(valid == FALSE)						/* if FALSE then initialisation */
	{								/* failed */
	     LCDPrintf("Compass Not Init");
	     return COMPASSERROR;
	}

	OSWait(SHORT);			/* MUST wait a short time after start before trying to read data */	
	        
	valid = FALSE;
	i = 0;
	while( i<5 && valid == FALSE )	/* read heading 5 times max to ensure compass is working */					
	{
	  valid = Obtain_Heading(plane);
	  i++;
	}
	if(valid == FALSE) 		/* if it doesn't work, Obtain_Heading has failed, something is wrong with the compass */
	{
		LCDPrintf("No or Bad Compass Data");
		return COMPASSHEADINGERROR;
	}
	Store_Old_Heading(plane);	/* store the last heading */
		
	
	/********** Test GPS *********/
	
	valid = FALSE;
	while(valid == FALSE)
	{
		valid = Init_GPS();
		if(valid == TRUE)
		{
			valid = Test_GPS();
		}  
		if(valid == FALSE) 	/* If either function failed then allow the user to retry or exit */	
		{ 	
			LCDPrintf("GPS not sending\nRed: when ready\nBlue: finish prog");
			while(TRUE)
			{
				key = 0;
				while(key == 0)
				{
					key = Check_Input();
				}
				if(key == KEY1) break;
				if(key == KEY4) return INITERROR; 	
			}
		}
	}
	
	valid = FALSE;
	while( valid == FALSE ) 		/* try to receive positional data */
	{
	  	valid = Obtain_GPS_Position(plane); 
	}
	
	Store_Old_Position(plane);		/* store the last position */

	LCDPrintf( "GPSinit complete" );

	
	/******** Get default waypoints from header file ********/
	
	Obtain_Wplist_From_HFile(&wps);			
	Store_Wplist(plane, wps);

	return NOERROR;
}

/* Check_Switch checks the state of the manual->autopilot switch
* returns: ON->autopilot engaged
*	   OFF->manual engaged
*	   UNDEFINED->switch not detected (CRITICAL ERROR!)
*/
switchstate Check_Switch()
{
	int check;
	
	check = OSGetAD(SWITCH);
	if(check >= 0)
	{
		if(check < SWITCH_THRESHHOLD) return ON;	/* If the switch is below threshhold then autopilot */
		else if(check < SWITCH_MAX) return OFF;		/* If the switch is above threshhold and below max then manual */
		else return UNDEFINED;				/* else an error has occured */
	}
	else return UNDEFINED;					/* else an error has occured */
}

/* Check_Input determines whether an input has been received from either input
* device (buttons or remote) and returns an identifier for the button if it has 
* been pushed
* returns: KEY1 to KEY4 representing the buttons on the eyebot
*/
int Check_Input()
{
	int key = 0;
	int remote = 0;

	key = KEYRead();		/* Read from both inputs */
	remote = IRTVRead();
	
	if(key != 0)			/* if a key was pressed respond to that */
	{
		return key;
	}
	if(remote != 0)			/* if an IR button was pressed respond to that */
	{				/* convert coloured buttons to eyebot button identifiers */
		if(remote == RC_RED) return KEY1;
		else if(remote == RC_GREEN) return KEY2;
		else if(remote == RC_YELLOW) return KEY3;
		else if(remote == RC_BLUE) return KEY4;
	}
	return 0;
}

/* Shut_Down terminates all hardware so that it can be reused later
* inputs:  plane->contains handles for hardware
*/
void Shut_Down(planestate plane)
{
    COMPASSRelease();
	SERVORelease(plane.control.rudder.servo);
	SERVORelease(plane.control.banking.servo);
	SERVORelease(plane.control.elevator.servo);
	SERVORelease(plane.control.throttle.servo);
	IRTVTerm();
}