/****************************************************************************
compass.c - Created by Peter Mauger 03/04/01
Last Modified 12/10/01

compass contains all the functions required to initialise, test, and retrieve
data from the 2D vector compass which will be connected to the eyebot.
****************************************************************************/
#include "compass.h"

/* Init_Compass initialises the 2D vector compass for operation
* returns: FALSE if the initialisation failed
*	   TRUE if it was successful
*/
bool Init_Compass()
{
	if( COMPASSInit( COMPASS ) == 1 ) 			/* initialises the compass */
	{
		LCDPrintf( "Compass Init Failed\n" );		/* returns 1 if the initialisation failed */
		return FALSE;
	}
	if( COMPASSStart( TRUE ) == 1 ) 			/* tells the compass to start sending data */
	{
		LCDPrintf( "Compass Already Started\n" );	/* returns 1 if the compass was already started */
		return FALSE;
	}

	LCDPrintf( "Compass Found\n" );
	return TRUE;
}			

/* Obtain_Heading stores the current output of the compass in the plane 
* structure
* inputs:  plane->contains the plane state information
* returns: FALSE if the bearing was not within 0 to 360 degrees
*	   TRUE otherwise
*/
bool Obtain_Heading(planestate *plane)
{
	int heading;

	heading = COMPASSGet();				/* get the bearing from the compass */
	heading = (heading + 90)%360;			/* NOTE!!! the compass was mounted 90 degrees anticlockwise */
							/* to the correct orientation. Therefore we need to add 90 degrees */
	if( heading >= 0 && heading < 360 ) {
		plane->position.curr_heading = heading;	/* where the heading is stored for later retrieval */
		LCDSetPrintf(4,0,"H:%d   ",heading);
		return TRUE;
	}

	/* if we get here, heading was out of allowed range, return 0 */
	return FALSE;
}			

/* Test_Sensors obtains the current position and heading for logging purposes
* inputs:  plane->contains the positional data
*/
void Test_Sensors(planestate *plane)
{
	bool valid = FALSE; 
	
	valid = Obtain_Heading( plane );				/* check the heading */
	if(valid != TRUE) LCDPrintf("heading error\n");
	
	valid = Obtain_GPS_Position( plane );			/* check the position */
}
	

/* Calibrate_Compass performs the calibration of the vector compass so that it
* is aligned south->north at 0 degress
* returns: FALSE if the calibration was cancelled (data is cleared, should recalibrate)
*	   TRUE if the calibration was successful
*/
bool Calibrate_Compass()
{
	int key;
	while(TRUE)					/* keep doing this until return */
	{						/* (ie must exit with some state) */ 
		key = 0;				/* reset the key value */	
		LCDClear();				/* establish the initial calibration menu screen */
		LCDPrintf( "Compass Cal\n" );
		LCDPrintf( "RES to reset\n" );
		LCDPrintf( "END returns\n" );
		LCDMenu( "", "", "RES", "END" );
		do 
		{
			key = Check_Input();		/* Wait for a relevant key to be pressed */
		}
		while( key != KEY3 && key != KEY4 );
	
		if( key == KEY1 ) 			/* Start calibration */
		{
			key = 0;			/* reset the key value */
			COMPASSCalibrate(1);		/* first section of calibration phase */
			
			LCDClear();			/* establish the second calibration menu screen */
			LCDPrintf( "Align plane EAST" );
			LCDPrintf( "GO when ready\n" );
			LCDPrintf( "END return\n" );
			LCDMenu( "GO", "", "", "END" );
			do {
				key = Check_Input();	/* Wait for a relevant key to be pressed */
			}
			while( key != KEY1 && key != KEY4 );
		
			if( key == KEY1 ) 		/* Continue Calibration */
			{		
				COMPASSCalibrate(1);	/* second section of calibration phase */
				
				LCDClear();		/* acknowledge successful calibration */
				LCDPrintf( "Compass\ncalibrated\n" );
				OSWait(LONG);
				return TRUE;
			}
			if( key == KEY4 )
			{
				COMPASSCalibrate(0);	/* reset the data (partial calibration useless) */
				LCDPrintf( "Calibration\ncancelled.\nData cleared.\n" );
				LCDPrintf( "Please recalibrate" );
				OSWait(LONG);
				return FALSE;
			}
		}
		if( key == KEY3 ) 			/* Reset the data */
		{
			COMPASSCalibrate(0);		/* reset calibration function */
			
			LCDClear();			/* acknowledge reset */
			LCDPrintf( "Data cleared\n" );
			LCDPrintf( "Please recalibrate" ); 
			OSWait(LONG);			/* bring menu up again so calibration can occur */
		}
		if( key == KEY4 ) return FALSE;		/* end returns to the previous menu */
	}
}