//------------------------------------------------------------------------------
// <copyright file="LqrSMACGeodetic.cpp" company="LiquidRobotics">
//
// Copyright (c) 2014 Liquid Robotics, Inc.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// This software is associated with a release.  Liquid Robotics makes no
// representation that the design, interfaces or component distribution will
// remain unchanged in future releases.
//
// </copyright>
//
// <summary>
//   Implementation of Geodetic functions
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------

#include "LqrSMACGeodetic.h"
#include <cmath>
#include <iomanip>
#include <sstream>
#include <string>
#include <stdexcept>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;

static const double earthRadius = 6371.0071809;
static const double pi = 3.14159265359;
static const double twopi = 2*pi;
static const double threeSixty = 360.0;
static const double degToRad = twopi/threeSixty;
static const double radToDeg = threeSixty/twopi;

namespace LiquidRobotics {
namespace SMACFramework {

// Returns distance (km) and bearing (deg) between two positions (from and to), with
// members named "latitude" and "longitude" in degrees
Geodetic::BearingAndDistance Geodetic::CalculateBearingAndDistance(const Location& from, const Location& to)
{
	if (!from.valid || !to.valid)
		throw invalid_argument("CalculateBearingAndDistance: one or more invalid parameters");

	const BearingAndDistance result(CalculateInitialBearing(from,to), earthRadius*CalculateCentralAngleRad(from,to));
	return result;
}

// Calculates the central angle between two lat/lon positions (from and to have latitude
// and longitude members in degrees) -- returns degrees
double Geodetic::CalculateCentralAngle(const Location& from, const Location& to)
{
	if (!from.valid || !to.valid)
		throw invalid_argument("CalculateCentralAngle: one or more invalid parameters");

	const double result = RadianstoDegrees(LawOfCosinesCARad(from.lat, from.lon, to.lat, to.lon));
	return result;
}

// Calculates the central angle between two lat/lon positions (from and to have latitude
// and longitude members in degrees) -- returns radians
double Geodetic::CalculateCentralAngleRad(const Location& from, const Location& to)
{
	if (!from.valid || !to.valid)
		throw invalid_argument("CalculateCentralAngleRad: one or more invalid parameters");

	const double result = LawOfCosinesCARad(from.lat, from.lon, to.lat, to.lon);
	return result;
}

// Calculates the initial bearing between two lat/lon positions (from and to have latitude
// and longitude members in degrees)
double Geodetic::CalculateInitialBearing(const Location& from, const Location& to)
{
	if (!from.valid || !to.valid)
		throw invalid_argument("CalculateInitialBearing: one or more invalid parameters");

	const double result = CalculateInitialBearing(from.lat, from.lon, to.lat, to.lon);
	return result;
}

// Calculates the initial bearing between two lat/lon positions (from and to have latitude
// and longitude members in degrees)
double Geodetic::CalculateInitialBearing(double flat, double flon, double tlat, double tlon)
{
	flat = DegreesToRadians(flat);
	flon = DegreesToRadians(flon);
	tlat = DegreesToRadians(tlat);
	tlon = DegreesToRadians(tlon);

	//const double dLat = tlat-flat;
	const double dLon = tlon-flon;
	const double y = sin(dLon)*cos(tlat);
	const double x = cos(flat)*sin(tlat)-sin(flat)*cos(tlat)*cos(dLon);
	const double result = RadianstoDegrees(atan2(y,x));
	return result;
}

// Central angle calculation using haversine formula (inputs in degrees, result in radians)
double Geodetic::HaversineCARad(double flat, double flon, double tlat, double tlon)
{
	flat = DegreesToRadians(flat);
	flon = DegreesToRadians(flon);
	tlat = DegreesToRadians(tlat);
	tlon = DegreesToRadians(tlon);

	const double dLat = tlat-flat;
	const double dLon = tlon-flon;
	const double sinDLatOver2 = sin(dLat/2);
	const double sinDLonOver2 = sin(dLon/2);
	const double a = sinDLatOver2*sinDLatOver2 + sinDLonOver2*sinDLonOver2*cos(flat)*cos(tlat);
	const double result = 2*atan2(sqrt(a), sqrt(1-a));
	return result;

}

// Central angle calculation using a modified law of cosines (relies on a large mantissa, inputs in degrees, result in radians)
double Geodetic::LawOfCosinesCARad(double flat, double flon, double tlat, double tlon)
{
	flat = DegreesToRadians(flat);
	flon = DegreesToRadians(flon);
	tlat = DegreesToRadians(tlat);
	tlon = DegreesToRadians(tlon);

	const double deltaLon = tlon-flon;
	const double sinFlat = sin(flat);
	const double sinTlat = sin(tlat);
	const double cosFlat = cos(flat);
	const double cosTlat = cos(tlat);
	const double cosDeltaLon = cos(deltaLon);
	const double result = acos(sinFlat*sinTlat + cosFlat*cosTlat*cosDeltaLon);
	return result;
}

Geodetic::Location Geodetic::OffsetLocation(const Location& loc, const BearingAndDistance& offset)
{
	if (abs(offset.distance) < 0.000001)
		return loc;

	BearingAndDistance bd = offset;

	if (bd.distance < 0)
	{
		bd.distance = -bd.distance;
		bd.bearing += DegreesToRadians(bd.bearing+180);
	}

	const double flat = DegreesToRadians(loc.lat);
	const double flon = DegreesToRadians(loc.lon);
	const double sinFlat = sin(flat);
	const double cosFlat = cos(flat);
	const double dOverR = bd.distance/earthRadius;
	const double sinDoverR = sin(dOverR);
	const double cosDoverR = cos(dOverR);
	const double tlat = asin(sinFlat*cosDoverR+cosFlat*sinDoverR*cos(bd.bearing));
	const double yParam = sin(bd.bearing)*sinDoverR*cosFlat;
	const double xParam = cosDoverR-sinFlat*sin(tlat);
	const double tlon = flon+atan2(yParam,xParam);

	Location result(RadianstoDegrees(tlat,LATITUDE), RadianstoDegrees(tlon,LONGITUDE));
	return result;
}

// Converts a number of degrees to radians in the range of -pi -> pi
double Geodetic::DegreesToRadians(double deg)
{
	double intPart;
	double rads = twopi*modf(deg/threeSixty, &intPart);

	if (rads < -pi)
		rads += twopi;
	else if (rads > pi)
		rads -= twopi;

	return rads;
}

// Converts a number of radians into degrees in the range of 0 -> 359.999...
double Geodetic::RadianstoDegrees(double rad)
{
	double intPart;
	double degs = threeSixty*modf(rad/twopi, &intPart);

	if (degs < 0)
		degs += threeSixty;

	return degs;
}

double Geodetic::RadianstoDegrees(double rad, HemisphereOrientation orientation)
{
	double intPart;
	double degs = threeSixty*modf(rad/twopi, &intPart);

	switch(orientation)
	{
	case LATITUDE:
		if (degs > 180)
			degs -= 360;
		else if (degs < -180)
			degs += 360;
		break;

	case LONGITUDE:
		if (degs > 180)
			degs -= 360;
		else if (degs < -180)
			degs += 360;
		break;

	default:
	{
		stringstream ss;
		ss << "Invalid argument (" << orientation << ") given for orientation: expected LATITUDE or LONGITUDE";
		throw invalid_argument(ss.str());
	}
	}

	return degs;
}

double Geodetic::StringToAngle(const string& str, HemisphereOrientation orientation)
{
	try
	{
		const double result = boost::lexical_cast<double>(str);
		return result;
	}
	catch(...)
	{
	}

	double degrees, minutes, seconds;
	Hemisphere hemi;
	StringToDMS(str, degrees, minutes, seconds, hemi, orientation);
	const double result = DMSToAngle(degrees, minutes, seconds, hemi);

	switch(orientation)
	{
	case LATITUDE:

		if (abs(result) > 90)
		{
			stringstream ss;
			ss << "An angle of " << result << " is out of range for latitude";
			throw invalid_argument(ss.str());
		}

		break;

	case LONGITUDE:

		if (abs(result) > 180)
		{
			stringstream ss;
			ss << "An angle of " << result << " is out of range for longitude";
			throw invalid_argument(ss.str());
		}

		break
		;
	default:
	{
		stringstream ss;
		ss << "Unexpected argument (" << orientation << ") given for orientation: expected LATITUDE or LONGITUDE";
		throw invalid_argument(ss.str());
	}
	}

	return result;
}

string Geodetic::AngleToString(double angle, bool useDMS, HemisphereOrientation orientation)
{
	stringstream stm;

	if (useDMS)
	{
		double degrees, minutes, seconds;

		AngleToDMS(abs(angle), degrees, minutes, seconds);
		stm << degrees << " " << minutes << "\'" << setprecision(8) << seconds << "\"";

		switch(orientation)
		{
		case LATITUDE:

			if (angle < 0)
				stm << "S";
			else
				stm << "N";

			break;

		case LONGITUDE:

			if (angle < 0)
				stm << "W";
			else
				stm << "E";

			break;

		default:
		{
			stringstream ss;
			ss << "Unexpected argument (" << orientation << ") given for orientation: expected LATITUDE or LONGITUDE";
			throw invalid_argument(ss.str());
		}

		}
	}
	else
		stm << setprecision(10) << angle;

	return stm.str();
}

void Geodetic::AngleToDMS(double angle, double& degrees, double& minutes, double& seconds)
{
	seconds = 60*modf(60*modf(angle,&degrees),&minutes);
}

double Geodetic::DMSToAngle(double degrees, double minutes, double seconds, Hemisphere hemi)
{
	double sign;
	double overflow;

	switch(hemi)
	{
	case NORTH:	sign = 1;	overflow = 90;	break;
	case SOUTH:	sign = -1;	overflow = 90;	break;
	case EAST:	sign = 1;	overflow = 180;	break;
	case WEST:	sign = -1;	overflow = 180;	break;
	default:
	{
		stringstream ss;
		ss << "Unexpected argument (" << hemi << ") for hemisphere: expected NORTH, SOUTH, EAST or WEST";
		throw invalid_argument(ss.str());
	}
	}

	const double result = sign*(degrees + minutes/60.0 + seconds/3600.0);

	if ((result < -overflow) || (result > overflow))
	{
		stringstream ss;
		ss << "Resulting angle (" << result << ") is out of range for hemisphere (";

		switch(hemi)
		{
		case NORTH:	ss << "NORTH";	break;
		case SOUTH:	ss << "SOUTH";	break;
		case EAST:	ss << "EAST";	break;
		case WEST:	ss << "WEST";	break;
		}

		ss << ")";
		throw runtime_error(ss.str());
	}

	return result;
}

void Geodetic::StringToDMS(const string& str, double& degrees, double& minutes, double& seconds, Hemisphere& hemi, HemisphereOrientation orientation)
{
	const size_t degreesEnd = str.find_first_of(' ');
	const size_t minutesEnd = (degreesEnd == string::npos) ? string::npos : str.find_first_of("'", degreesEnd);
	const size_t secondsEnd = (minutesEnd == string::npos) ? string::npos : str.find_first_of("\"", minutesEnd);

	if ((secondsEnd == string::npos) || ((1+secondsEnd) == str.length()))
		throw invalid_argument("Invalid format, expected hours minutes'seconds\" N/S/E/W");

	string remainder = str.substr(secondsEnd+1);
	boost::trim(remainder);

	switch(orientation)
	{
	case LATITUDE:
	{
		if (boost::iequals(remainder, "N"))
			hemi = NORTH;
		else if (boost::iequals(remainder, "S"))
			hemi = SOUTH;
		else
		{
			stringstream stm;
			stm << "Unexpected hemisphere (" << remainder << ") expected for latitude: expected N or S";
			throw invalid_argument(stm.str());
		}

		break;
	}

	case LONGITUDE:
	{
		if (boost::iequals(remainder, "E"))
			hemi = EAST;
		else if (boost::iequals(remainder, "W"))
			hemi = WEST;
		else
		{
			stringstream stm;
			stm << "Unexpected hemisphere (" << remainder << ") expected for latitude: expected E or W";
			throw invalid_argument(stm.str());
		}

		break;
	}

	default:
	{
		stringstream stm;
		stm << "Unexpected argument (" << orientation << ") given for orientation: must be LATITUDE or LONGITUDE";
		throw invalid_argument(stm.str());
	}
	}

	string paramName;
	string paramValue;

	try
	{
		paramName = "degrees";
		paramValue = str.substr(0,degreesEnd);
		boost::trim(paramValue);
		degrees = boost::lexical_cast<double>(paramValue);

		paramName = "minutes";
		paramValue = str.substr(degreesEnd+1,minutesEnd-degreesEnd-1);
		boost::trim(paramValue);
		minutes = boost::lexical_cast<double>(paramValue);

		paramName = "seconds";
		paramValue = str.substr(minutesEnd+1,secondsEnd-minutesEnd-1);
		boost::trim(paramValue);
		seconds = boost::lexical_cast<double>(paramValue);
	}
	catch(...)
	{
		degrees = minutes = seconds = 0.0;

		stringstream stm;
		stm << "Unable to convert " << paramName << " (" << paramValue << ") to double";
		throw invalid_argument(stm.str());
	}

	if ((degrees < 0) || (minutes < 0) || (seconds < 0))
		throw invalid_argument("Negative numbers cannot be specified for component values");
}

// ==============================================

Geodetic::BearingAndDistance::BearingAndDistance()
: bearing(0.0)
, distance(0.0)
{
}

Geodetic::BearingAndDistance::BearingAndDistance(double _bearing, double _distance)
: bearing(_bearing)
, distance(_distance)
{
}

string Geodetic::BearingAndDistance::ToString() const
{
	stringstream stm;
	stm << "bearing=" << setprecision(10) << bearing
		<< " distance=" << setprecision(10) << distance;
	return stm.str();
}

Geodetic::Location::Location()
: lat(0.0)
, lon(0.0)
, valid(false)
{
}

Geodetic::Location::Location(double _lat, double _lon)
: lat(_lat)
, lon(_lon)
, valid(true)
{
}

Geodetic::BearingAndDistance Geodetic::Location::CalculateBearingAndDistanceFrom(const Location& from) const
{
	BearingAndDistance result = CalculateBearingAndDistance(from, *this);
	return result;
}

Geodetic::BearingAndDistance Geodetic::Location::CalculateBearingAndDistanceTo(const Location& to) const
{
	BearingAndDistance result = CalculateBearingAndDistance(*this, to);
	return result;
}

void Geodetic::Location::MoveLocation(const BearingAndDistance& offset)
{
	Location temp = *this;
	*this = OffsetLocation(temp, offset);
}

string Geodetic::Location::ToString(bool toDMS) const
{
	stringstream stm;

	if (valid)
		stm << "(lat=" << AngleToString(lat,toDMS,LATITUDE)
			<< ",lon=" << AngleToString(lon,toDMS,LONGITUDE)
			<< ")";
	else
		stm << "INVALID";

	return stm.str();
}


}
}
