#include "Utils.h"
#include "math.h"	

double stdAngle(double angle)
{
	while(angle >= 2*PI) angle -= 2*PI;
	while(angle <0 ) angle += 2*PI;
    return angle;
}

double distance(double x1, double y1, double x2, double y2)
{
	return sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
}

double heading(double x1, double y1, double x2, double y2)
{
   	double heading;
    // special check
    double d = distance(x1,y1,x2,y2);
    if( d == 0 ) return 0;
    // normal condition
	heading = acos((x2-x1)/d); //一二象限
	if( y1 > y2 ) heading = 2*PI - heading; //三四象限
    return heading;
}

double turnAngle(double from, double to)
{
  	double turnAngle = stdAngle(to) - stdAngle(from);
   	if( turnAngle > PI ) turnAngle = turnAngle - 2*PI;
    else if( turnAngle < -PI ) turnAngle = turnAngle + 2*PI;
    return turnAngle;
}

double bearing(double heading, double base)
{
   	double bearing = stdAngle(heading) - stdAngle(base);
   	if( bearing > PI ) bearing = bearing - 2*PI;
   	else if( bearing < -PI ) bearing = bearing + 2*PI;
   	return bearing;
}

double absBearing(double heading1, double heading2)
{
    return fabs(bearing(heading1,heading2));
}