/* Copyright 2002 Daniel Egnor.  See LICENSE file.
 *
 * Basic spherical geometry primitives. 
 * Formulas from: http://williams.best.vwh.net/avform.htm 
 * This file predates the Google contest, but I wrote it all. */

#include "geodesy.h"

#include <math.h>

static const double epsilon = 1e-30;

static double square(double x) { return x*x; }
static double to_radians(double x) { return x*(M_PI/180.0); }
static double to_degrees(double x) { return x*(180.0/M_PI); }
static double to_kilometers(double x) { return to_degrees(x)*(60.0*1.852); }
static double from_kilometers(double x) { return to_radians(x/(60.0*1.852)); }

double geo_distance(double lat1,double lon1,double lat2,double lon2) {
        lat1 = to_radians(lat1);
        lon1 = to_radians(lon1);
        lat2 = to_radians(lat2);
        lon2 = to_radians(lon2);
        return to_kilometers(2.0*asin(sqrt(
                  square(sin(0.5*(lat1 - lat2)))
                + cos(lat1)*cos(lat2)*square(sin(0.5*(lon2 - lon1))))));
}

double geo_heading(double lat1,double lon1,double lat2,double lon2) {
        double ret;

        lat1 = to_radians(lat1);
        lon1 = to_radians(lon1);
        lat2 = to_radians(lat2);
        lon2 = to_radians(lon2);

        /* what color was the bear? */
        if (cos(lat1) < epsilon) return (lat1 > 0.0) ? 180.0 : 0.0;

        ret = to_degrees(atan2(
                sin(lon2 - lon1)*cos(lat2),
                cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(lon2 - lon1)));
        while (ret < 0.0) ret += 360.0;
        while (ret >= 360.0) ret -= 360.0;
        return ret;
}

double geo_latitude(double lat1,double heading,double distance) {
        lat1 = to_radians(lat1);
        heading = to_radians(heading);
        distance = from_kilometers(distance);
        return to_degrees(asin(
                  sin(lat1)*cos(distance)
                + cos(lat1)*sin(distance)*cos(heading)));
}

double geo_longitude(
        double lat1,double lon1,double lat2,
        double heading,double distance)
{
        lat1 = to_radians(lat1);
        lon1 = to_radians(lon1);
        lat2 = to_radians(lat2);
        heading = to_radians(heading);
        distance = from_kilometers(distance);

        lon1 += atan2(
                  sin(heading)*sin(distance)*cos(lat1),
                  cos(distance) - sin(lat1)*sin(lat2));

        if (lon1 < -M_PI) lon1 += 2*M_PI;
        else if (lon1 >= M_PI) lon1 -= 2*M_PI;
        return to_degrees(lon1);
}

double geo_mercator_easting(double longitude) {
	return to_radians(longitude) / M_PI;
}

double geo_mercator_longitude(double easting) {
	return to_degrees(easting * M_PI);
}

double geo_mercator_northing(double latitude) {
	if (latitude >= 0)
		return log(tan(M_PI/4 + to_radians(latitude)/2)) / M_PI;
	else
		return - log(tan(M_PI/4 - to_radians(latitude)/2)) / M_PI;
}

double geo_mercator_latitude(double northing) {
	if (northing >= 0)
		return to_degrees(2*(atan(exp(northing * M_PI)) - M_PI/4));
	else
		return to_degrees(2*(M_PI/4 - atan(exp(- northing * M_PI))));
}