#include "Navigation.h"

Navigation::Navigation(GPS *withGPS, Waypoints *withWP) : 
		_gps(withGPS),
		_wp(withWP),
		_hold_course(-1)
{
}

void
Navigation::update_gps()
{
	location.alt 		= _gps->altitude;
	location.lng 		= _gps->longitude;
	location.lat 		= _gps->latitude;
	
	// target_bearing is where we should be heading 
	bearing 			= get_bearing(&location, &next_wp);

	// waypoint distance from plane
	distance 			= get_distance(&location, &next_wp);
	
	calc_bearing_error();
	calc_altitude_error();
	altitude_above_home = location.alt - home.alt;
			
	// check if we have missed the WP
	_loiter_delta 		= (bearing - _old_bearing) / 100;
	
	// reset the old value
	_old_bearing 		= bearing;
	
	// wrap values
	if (_loiter_delta > 170) _loiter_delta -= 360;
	if (_loiter_delta < -170) _loiter_delta += 360;
	loiter_sum += abs(_loiter_delta);

	if (distance <= 0){
		distance = -1;
		Serial.print("MSG wp error  ");
		Serial.println(distance, DEC);
	}
}

void
Navigation::load_first_wp(void)
{
	set_next_wp(_wp->get_waypoint_with_index(1));
}

void
Navigation::load_home(void)
{
	home = _wp->get_waypoint_with_index(0);
}

void
Navigation::return_to_home_with_alt(uint32_t alt)
{
	Waypoints::WP loc = _wp->get_waypoint_with_index(0);
	loc.alt += alt;
	set_next_wp(loc);
}

void
Navigation::reload_wp(void)
{
	set_next_wp(_wp->get_current_waypoint());
}

void
Navigation::load_wp_index(uint8_t i)
{
	_wp->set_index(i);
	set_next_wp(_wp->get_current_waypoint());
}

void
Navigation::hold_location()
{
  //	set_next_wp()	XXX needs to be implemented
}

void
Navigation::set_home(Waypoints::WP loc)
{
	_wp->set_waypoint_with_index(loc, 0);
	home 		= loc;
	//location 	= home;
}

void
Navigation::set_next_wp(Waypoints::WP loc)
{
	prev_wp = next_wp;
	next_wp = loc;
	
	if(_scaleLongDown == 0)
	calc_long_scaling(loc.lat);

	total_distance 		= get_distance(&location, &next_wp);
	distance			= total_distance;
	
	bearing 			= get_bearing(&location, &next_wp);
	_old_bearing 		= bearing;

	// clear loitering code
	_loiter_delta 		= 0;
	loiter_sum 			= 0;	

	// set a new crosstrack bearing
	// ----------------------------
	reset_crosstrack();
}

void 
Navigation::calc_long_scaling(int32_t lat)
{
	// this is used to offset the shrinking longitude as we go towards the poles	
	float rads 			= (abs(lat) / T7) * 0.0174532925;
	_scaleLongDown 		= cos(rads);
	_scaleLongUp 		= 1.0f / cos(rads);
}

void
Navigation::set_hold_course(int16_t b)
{
	if(b)
		_hold_course = bearing;
	else
		_hold_course = -1;
}

int16_t
Navigation::get_hold_course(void)
{
	return _hold_course;
}

void
Navigation::calc_distance_error()
{	
	//distance_estimate 	+= (float)_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
	//distance_estimate 	-= DST_EST_GAIN * (float)(distance_estimate - GPS_distance);
	//distance  		= max(distance_estimate,10);
}

void 
Navigation::calc_bearing_error(void)
{
	if(_hold_course == -1){
		bearing_error = wrap_180(bearing - _gps->ground_course);
	}else{
		bearing_error = _hold_course;
	}
}

int32_t
Navigation::wrap_180(int32_t error)
{
	if (error > 18000)	error -= 36000;
	if (error < -18000)	error += 36000;
	return error;
}

int32_t
Navigation::wrap_360(int32_t angle)
{
	if (angle > 36000)	angle -= 36000;
	if (angle < 0)		angle += 36000;
	return angle;
}

void
Navigation::set_bearing_error(int32_t error)
{
	bearing_error = wrap_180(error);
}


/*****************************************
 * Altitude error with Airspeed correction
 *****************************************/
void
Navigation::calc_altitude_error(void) 
{
	// limit climb rates
	_target_altitude = next_wp.alt - ((float)((distance -20) * _offset_altitude) / (float)(total_distance - 20));
	if(prev_wp.alt > next_wp.alt){
		_target_altitude = constrain(_target_altitude, next_wp.alt, prev_wp.alt);
	}else{
		_target_altitude = constrain(_target_altitude, prev_wp.alt, next_wp.alt);
	}
	altitude_error 	= _target_altitude - location.alt;
}

void
Navigation::set_loiter_vector(int16_t v)
{
  //	_vector = constrain(v, -18000, 18000);	XXX needs to be implemented
}

void
Navigation::update_crosstrack(void)
{
	// Crosstrack Error
	// ----------------
	if (abs(bearing - _crosstrack_bearing) < 4500) {	 // If we are too far off or too close we don't do track following
		_crosstrack_error = sin(radians((bearing - _crosstrack_bearing) / 100)) * distance;	 // Meters we are off track line
		bearing += constrain(_crosstrack_error * XTRACK_GAIN, -XTRACK_ENTRY_ANGLE, XTRACK_ENTRY_ANGLE);
	}
}

void
Navigation::reset_crosstrack(void)
{
	_crosstrack_bearing 	= get_bearing(&location, &next_wp);	// Used for track following
}

long
Navigation::get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2)
{
	if(loc1->lat == 0 || loc1->lng == 0) 
		return -1;
	if(loc2->lat == 0 || loc2->lng == 0) 
		return -1;
	if(_scaleLongDown == 0)
		calc_long_scaling(loc2->lat);
	float dlat 	= (float)(loc2->lat - loc1->lat);
	float dlong = ((float)(loc2->lng - loc1->lng)) * _scaleLongDown;
	return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}

long 
Navigation::get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2)
{
	if(loc1->lat == 0 || loc1->lng == 0) 
		return -1;
	if(loc2->lat == 0 || loc2->lng == 0) 
		return -1;
	if(_scaleLongDown == 0)
		calc_long_scaling(loc2->lat);
	long off_x 			= loc2->lng - loc1->lng;
	long off_y 			= (loc2->lat - loc1->lat) * _scaleLongUp;
	long bearing 		=	9000 + atan2(-off_y, off_x) * 5729.57795;
	if (bearing < 0) 
		bearing 		+= 36000;
	return bearing;
}