/*******************************************************************************
position.c - Created by Peter Mauger 30/07/01
Last Modified 12/10/01

position contains all functions required to manipulate the p_pos structure.
It should be noted that all latitudes and longitudes are stored in the form
	dddmm.mmmmmm - where d represents the degrees values of the number
		       and m represents the minutes values of the number
A function (Convert_GPS2deg) has been provided to convert these values into degrees
only (ie to ddd.ddddddddd).		       
*******************************************************************************/

#include "position.h"

/* Init_Pos initialises the internal variables of the position struct
* returns: initialised position
*/
position Init_Pos()
{
	position pos;
	
	pos.latitude = 0;
	pos.longitude = 0;
	
	return pos;
}

/* Get_Latitude retrieves the latitudinal component of the position
* inputs:  pos->position (contains latitude info)
* returns: latitude as double (+ve -> North, -ve -> South)
*/
double Get_Latitude(position pos)
{
	return pos.latitude;
}

/* Set_Latitude sets the latitudinal component of the position
* inputs:  pos->position (contains latitude info)
*	   latitude->the latitude to be stored (double)
*	   	     (+ve -> North, -ve -> South)
*/
void Set_Latitude(position *pos, double latitude)
{
	pos->latitude = latitude;	
}

/* Get_Longitude retrieves the longtitudinal component of the position
* inputs:  pos->position (contains longitude info)
* returns: longitude as double (+ve -> East, -ve -> West)
*/
double Get_Longitude(position pos)
{
	return pos.longitude;
}
	
/* Set_Longitude sets the longitudinal component of the position
* inputs:  pos->position (contains longitude info)
*	   longitude->the longitude to be stored (double)
*		      (+ve -> East, -ve -> West)
*/
void Set_Longitude(position *pos, double longitude)
{
	pos->longitude = longitude;
}

/* Calc_Distance calculates the distance in metres between two navigational points
* (lat, long) 
* inputs:  p1->first point   
*	   p2->second point   
* returns: distance in metres {but not quite! There is some error here...})
*/
double Calc_Distance(position p1, position p2, double *dist)
{
	double lat1 = Convert_deg2rad(Convert_GPS2deg(Get_Latitude(p1))); 	/* at the first point (degrees) */
	double lat2 = Convert_deg2rad(Convert_GPS2deg(Get_Latitude(p2))); 	/* at the second point (degrees) */
	double long1 = Convert_deg2rad(Convert_GPS2deg(Get_Longitude(p1))); 	/* at the first point (degrees) */
	double long2 = Convert_deg2rad(Convert_GPS2deg(Get_Longitude(p2))); 	/* at the second point (degrees) */
	double d_long = long2 - long1; 		/* longitude of the second point minus longitude of the first point (degrees) */
	double d_lat = lat2 - lat1; 		/* latitude of the second point minus latitude of the first point (degrees) */
	
	*dist = sqrt( (d_long*d_long) + (d_lat*d_lat) ) * EARTH_RADIUS;		/* Works out the distance in radians then converts to metres */
	LCDSetPrintf(2,0,"D:%f  ",*dist);						/* by multiplying by the radius of the earth (can be modified */
	return NOERROR;								/* for local deviation in radius) */
}

/* Calc_Bearing calculates the bearing required to reach the waypoint from
* the GPS position (Adaptation of Russell Rodgers function CEarth::direction)
* inputs:  plane->plane information (contains current position and waypoint)  
* returns: Bearing in degrees from GPSpos to WPpos
*/
double Calc_Bearing(planestate plane)
{
	/* this method uses a spheroidal model for the earth*/
	
	position curr_pos_plane = Get_Curr_Pos(plane);
	position wp_pos = Get_Curr_Wp(plane);
	double lat1 = Convert_deg2rad(Convert_GPS2deg(Get_Latitude(curr_pos_plane)));
	double lat2 = Convert_deg2rad(Convert_GPS2deg(Get_Latitude(wp_pos)));
	double long1 = Convert_deg2rad(Convert_GPS2deg(Get_Longitude(curr_pos_plane)));
	double long2 = Convert_deg2rad(Convert_GPS2deg(Get_Longitude(wp_pos)));
	double t;
	
	/* This calculation was taken from Russel Rodgers CEarth::direction. I do not understand it however it seems to work... */
	t = atan2(sin(long2-long1)*cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(long2-long1));
	
	if(t<0) t = t + (M_PI*2);	/* atan2 returns angle from -PI to PI. Convert to 0 to 2PI */
	t = Convert_rad2deg(t);			/* then convert to degrees */
	LCDSetPrintf(3,0,"B:%f    ",t);	/* print out the true north bearing */
	
	t = t - MAGNETIC_DEVIATION;	/* This takes into account local magnetic deviation (the angle the compass */
	return t;			/* north is, relative to true north {calculated from lat and long}) */
}

/* Calc_Correction calculates the heading correction required to reach the waypoint 
* given the current heading
* inputs:   plane->plane information   
*	    br->bearing required to reach wp (degrees 0->360)
* returns:  Correction in degrees from curr_heading to WP_bearing (degrees (+ve->left)(-ve->right))
*/
double Calc_Correction(planestate plane, double bearing_req)
{
	double correction;
	
	correction = (bearing_req - Get_Curr_Heading(plane));		/*Determine how much heading correction to */
	if(correction > 180)						/*be made (-180 < correction < 180)*/
	{
		correction = correction - 360;
	}
	if(correction < -180) 
	{
		correction = correction + 360;
	}
	return correction;
}

/* Convert_GPS2deg takes a GPS latitude or longitude (dddmm.mmmmmm where d is a degree value and m
* is a minute value) and converts it to degrees only (ddd.ddddddddd where d is a degree value)
* inputs:  gps_angle->the latitude/longitude value to be converted
* returns: the angle in degrees value only
*/
double Convert_GPS2deg(double gps_angle)
{
	double degrees; 
	double minutes; 

	degrees = floor(gps_angle / 100);	 /* isolate the degrees value */
	minutes = (gps_angle - (degrees * 100)); /* isolate the minutes value */
	return (degrees + (minutes/60));	 /* recombine them with the minutes divided by 60 */
}


/* Convert_deg2rad converts a number in degrees to a number in radians 
* (Function provided by Russell Rodgers)
* inputs:  deg->value in degrees
* returns: deg as radians
*/
double Convert_deg2rad(double deg)
{
	return (deg/180.0)*M_PI;
}

/* Convert_rad2deg converts a number in radians to a number in degrees 
* (Function provided by Russell Rodgers)
* inputs:  rad->value in radian
* returns: rad as degrees
*/
double Convert_rad2deg(double rad)
{
	return (rad/M_PI)*180.0;
}