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

waypoint contains all functions required to determine bearing and distance to a
given waypoint from the current position and heading
****************************************************************************/
#include "waypoint.h"

/* Init_Wplist initialises a wplist struct
* returns: an empty wplist structure
*/
wplist Init_Wplist()
{
	wplist wps;
	int i;
	
	wps.total_wps = 0;
	for(i=0;i<MAX_WAYPOINTS;i++)
	{
		wps.wps[i] = Init_Pos();
	}
	return wps;
}

/* Get_Wplist_Total retrieves the total number of waypoints from the wplist
* inputs:  waypoints->waypoint list
* returns: total number of waypoints in list
*/
int Get_Wplist_Total(wplist waypoints)
{
	return waypoints.total_wps;
}

/* Get_Wp gets the current waypoint from the waypoint list
* inputs:  waypoints->struct containing list of all waypoints    
*	   curr_wpnum->the number of the current wp (<total_waypoints)
*	   pos->variable for returning the position of the waypoint
* returns: INVALIDWPNUM if curr_wpnum out of range of list
*	   NOERROR otherwise
*/
error Get_Wp(wplist waypoints, int curr_wpnum, position *pos)
{
	if(curr_wpnum < waypoints.total_wps)
	{
		*pos = waypoints.wps[curr_wpnum];
		return NOERROR;
	}
	else
	{
		LCDClear();
		LCDPrintf("Invalid wp num\n");
		OSWait(LONG); 
		return INVALIDWPNUM;
	}
}

/* At_Wp determines whether the current GPS position is within the
* tolerance of a given waypoint
* inputs:  GPSpos->GPS position (lat,long)  
* 	   tolerance->radial distance around waypoint considered to be the
* 		      waypoint (unsure of the dimensions of this variable)
* 	   not_at_wp->variable for returning whether plane is at waypoint
*			TRUE means the plane is not at the waypoint
* returns: DIVZERO if a divide by zero occured
*	   NOERROR otherwise
*/
error At_Wp(planestate plane, double tolerance, bool *not_at_wp)
{
	double dist;
	error error_type;
	error_type = Calc_Distance(Get_Curr_Pos(plane), Get_Curr_Wp(plane), &dist);
	
	if(dist<tolerance) 
	{
		*not_at_wp = FALSE;
	}
	else 
	{
		*not_at_wp = TRUE;
	}
	return error_type;
}

/* Obtain_Wplist_From_File gets all the valid waypoints out of the file
* received on COM port PC_PORTNUM, and puts them into the wplist
* waypoints for later use by the program. Basis of this function was
* written by Nathan Hines. Modified by Peter Mauger.
* inputs:  COM_port->the communications port used for transfer
*	   waypoints->the set of waypoints passed back
* returns: WPFILEERROR if the file could not be received
*	   NOERROR otherwise
*/
error Obtain_Wplist_From_File(wplist *waypoints) {
	int result = 0, timeout;
	char file_string[MAX_FILE_LENGTH];		/* a character string representing the file input stream */
	int file_ptr = 0, file_length = 0;
	char n_s, e_w, temp_char;

	/* set up the Eyebot->PC RS232 connection for data receiving */
	result = OSInitRS232( PC_BAUDRATE, RTSCTS, PC_PORTNUM );
	if( result != 0 ) return WPFILEERROR;
	
	result = OSFlushInRS232( PC_PORTNUM );
	if( result != 0 ) return WPFILEERROR;
	
	/* port has been initialized and flushed, wait for user to press go on PC and for data to arrive */
	timeout = 0;
	LCDPrintf("Port init\nTimeout in: ");
	do
	{
		result = OSRecvRS232( &temp_char, PC_PORTNUM );
		if(result == 0) 
		{
			file_string[0] = temp_char;
			file_ptr++;
		}
		timeout++;
		LCDPrintf("%d ",(WPFILE_TIMEOUT - timeout));
	}while(result == 1 && timeout < WPFILE_TIMEOUT);
	if( timeout >= WPFILE_TIMEOUT )
	{
		LCDPrintf(" Timed Out\n");
		OSWait(LONG);
		return WPFILEERROR;
	}
	if( result != 0 )
	{
		LCDPrintf("didn't get file!%d ",result);
		return WPFILEERROR;
	}
	/* if we get here, file_string now has something in it i.e. we have begun to receive data, store it all */
	do
	{	
		timeout = 0;
		do
		{
			result = OSRecvRS232( &temp_char, PC_PORTNUM );
			if (result) {			
			  	LCDPrintf("upload error%d\n",result);
				OSWait(SHORT);
			}
			else { 
				file_string[file_ptr] = temp_char;
				file_ptr++;
				timeout = 0;
				LCDPrintf(".");
		        }
	 		timeout++;
			OSWait(VSHORT);
		}while(result != 0 && timeout < GPS_TIMEOUT);
		
		if(timeout == GPS_TIMEOUT)
		{
			LCDPrintf("upload timed out\n");
			OSWait(LONG);
			return WPFILEERROR;
		}
			
	}while( file_ptr < MAX_FILE_LENGTH && file_string[file_ptr-1] != '*' );
	LCDPrintf("\nLoop-End\n");

		
	if(file_ptr >= MAX_FILE_LENGTH)
	{
		LCDPrintf("File too large\n");
		return WPFILEERROR;
	}
	file_string[file_ptr] = '\0';
	file_length = file_ptr;
	
	LCDPrintf("File received:\nlength %d\n",file_length);
	OSWait(LONG);
	LCDClear();
	
	file_ptr = 0;
	waypoints->total_wps = 0;
	while(file_ptr < file_length)
	{
		if(file_ptr < file_length) 
		{
			if((file_string[file_ptr] != EOF) && (file_string[file_ptr] != '*'))
			{
				if( Parse_Waypoint( file_string, &file_ptr, waypoints ) != NOERROR ) return WPFILEERROR;
				/* otherwise, the waypoint was correct and has been stored, increment the number of waypoints */
				if(waypoints->wps[waypoints->total_wps].latitude <= 0) n_s = 'S';
				else n_s = 'N';
				if(waypoints->wps[waypoints->total_wps].longitude <= 0) e_w = 'W';
				else e_w = 'E';
				waypoints->total_wps++;
				LCDClear();
				LCDPrintf( "Waypoint %d:\n%f %c\n%f %c\n", waypoints->total_wps, (float)waypoints->wps[waypoints->total_wps-1].latitude, n_s, (float)waypoints->wps[waypoints->total_wps-1].longitude, e_w );
				OSWait(LONG);
			}
			else
			{
				LCDPrintf( "All waypoints\nparsed.\n" );
				return NOERROR;
			}
		}
		else
		{
			if((file_string[file_ptr] != EOF) && (file_string[file_ptr] != '*'))
			{
				LCDPrintf( "WPfile too long\n" );
				return WPFILEERROR;
			}
			else
			{
				LCDPrintf("All waypoints\nparsed.\n");
				return NOERROR;
			}
		}
	}
	return WPFILEERROR;
}

/* Obtain_Wplist_From_HFile finds the default waypoint list stored in the planeconst.h file 
* Currently this is not implemented, however should an emergency default set of 
* waypoints be required, they should be retrieved by this function. (This would only
* occur in the event of a power down and would require knowledge that the power down
* had actually occured so that the menus could be bypassed! Possibly some kind of
* mechanical latch which is set on flight start and only reset by successful completion
* of the program)
* inputs:  waypoints->passes the list of waypoints so they can be returned
*/
void Obtain_Wplist_From_HFile(wplist *waypoints)
{
  	waypoints->total_wps = 0;
}

/* Parse_Waypoint parses each waypoint in the waypoint file. Basis by Nathan Hines.
* Modifed by Peter Mauger.
* inputs:  file_string->the set of characters from the file
*	   file_ptr->marks character currently being looked at in string
*	   WPpos->is the position found
* returns: WPFILEERROR if there was an invalid string in the file
*	   NOERROR otherwise
*/
error Parse_Waypoint( char *file_string, int *file_ptr, wplist *wps ) {
  	char *next_char;	
	double curr_lat = 0, curr_long = 0;			/* waypoint latitude and longitude */
  	int curr_lat_dir = 0, curr_lon_dir = 0;			/* waypoint lat/long direction (-1 or +1) */

	
	/* strtod converts the first 'double looking' number it finds in the string to a double then returns the */
	/* next character after that number (next_char) */
	curr_lat = strtod(&file_string[*file_ptr],&next_char);
	
	/* next_char is returned by strtod and points to the next character after the converted number. */
	/* Finding the difference between the address of the input and the next_char gives the increment required */
	/* to update file_ptr so that it points to the correct character */
	(*file_ptr) += (next_char - &file_string[*file_ptr]);
	
	/* latitude value was fine, get direction (N or S) */
	if( (file_string[*file_ptr] == 'N') || (file_string[*file_ptr] == 'S') || (file_string[*file_ptr] == 'n') || (file_string[*file_ptr] == 's') ) {
	  if( file_string[*file_ptr] == 'n' || file_string[*file_ptr] == 'N' ) curr_lat_dir = NORTH;
	  if( file_string[*file_ptr] == 's' || file_string[*file_ptr] == 'S' ) curr_lat_dir = SOUTH;
	  (*file_ptr)++;
	}
	else return WPFILEERROR;

	/* strtod converts the first 'double looking' number it finds in the string to a double then returns the */
	/* next character after that number (next_char) */
	curr_long = strtod(&file_string[*file_ptr],&next_char); 
	
	/* next_char is returned by strtod and points to the next character after the converted number. */
	/* Finding the difference between the address of the input and the next_char gives the increment required */
	/* to update file_ptr so that it points to the correct character */
	(*file_ptr) += (next_char - &file_string[*file_ptr]);
	
	/* longtitude value was fine, get direction (E (converted to t) or W (still W or w)) */
	if( (file_string[*file_ptr] == 'W') || (file_string[*file_ptr] == 'w') || (file_string[*file_ptr] == 'E') || (file_string[*file_ptr] == 'e') ) {
	  if( (file_string[*file_ptr] == 'W') || (file_string[*file_ptr] == 'w') ) curr_lon_dir = WEST;
	  if( (file_string[*file_ptr] == 'E') || (file_string[*file_ptr] == 'e') ) curr_lon_dir = EAST;
	  (*file_ptr)++;
	}
	else return WPFILEERROR;

	/* if we get here, waypoint is good, store it */
	if(curr_lat_dir != 0)
	{
		wps->wps[wps->total_wps].latitude = curr_lat*curr_lat_dir;
	}
	else return WPFILEERROR;
	if(curr_lon_dir != 0)
	{
		wps->wps[wps->total_wps].longitude = curr_long*curr_lon_dir;
	}
	else return WPFILEERROR;
	
	/* if we get here, waypoint was good and has been stored, return with NOERROR */
	(*file_ptr)++;		/* There will be a \n in the file at the end of each waypoint */
	return NOERROR;		/* which must be skipped over */
}