//
// Open Service Platform
// Copyright (c) 2012 Samsung Electronics Co., Ltd.
//
// Licensed under the Apache License, Version 2.0 (the License);
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

/**
 * @file	FLoc_EllipsoidModel.h
 * @brief	This is the implementation file for the _EllipsoidModel class.
 *
 * This header file contains implementation of the _EllipsoidModel class.
 */

#include <math.h>
#include <FBaseDouble.h>
#include "FLoc_EllipsoidModel.h"
#include "FLoc_MathUtils.h"

using namespace Tizen::Base;

namespace Tizen { namespace Locations
{

const double _EllipsoidModel::SEMI_MAJOR_AXIS = 6378137.0;         //in meter
const double _EllipsoidModel::SEMI_MINOR_AXIS = 6356752.3142;      // in meter
const double _EllipsoidModel::FLATTENING = 298.257223563;
const double _EllipsoidModel::EARTH_RADIUS = _EllipsoidModel::SEMI_MAJOR_AXIS;

float
_EllipsoidModel::GetDistance(double lat1, double lon1, double lat2, double lon2)
{
	return SolveInverseProblem(lat1, lon1, lat2, lon2, RT_DISTANCE);
}

float
_EllipsoidModel::GetAzimuth(double lat1, double lon1, double lat2, double lon2)
{
	return SolveInverseProblem(lat1, lon1, lat2, lon2, RT_FORWARD_AZIMUTH);
}

float
_EllipsoidModel::SolveInverseProblem(double lat1, double lon1, double lat2, double lon2, ResultType type)
{
	// References: Direct and inverse solutions of geodesics on the ellipsoid with application of nested equations
	// The section 4, Inverse Formula, is used.
	// Instead of the equations (3), (4), and (6), the simplified ones (3a), (4a), and (6a) are adopted.

	const int ITERATION_MAX = 10;
	const double DELTA_TOLERANCE = 1.0e-12;
	const double a = EARTH_RADIUS; // WGS-84 major axis (6378137.0)
	const double b = SEMI_MINOR_AXIS; // WGS-84 semi-minor axis
	const double f = 1.0 / FLATTENING; // inverse flattening

	double L = (_MathUtils::DEG2RAD* lon2) -(_MathUtils::DEG2RAD* lon1);     // difference in longitude, positive east.
	double U1 = atan((1.0 - f) * tan(_MathUtils::DEG2RAD * lat1)); // reduced latitude
	double U2 = atan((1.0 - f) * tan(_MathUtils::DEG2RAD * lat2)); // reduced latitude

	double cosU1 = cos(U1);
	double cosU2 = cos(U2);
	double sinU1 = sin(U1);
	double sinU2 = sin(U2);
	double cosU1cosU2 = cosU1 * cosU2;
	double sinU1sinU2 = sinU1 * sinU2;

	double sigma = 0.0; // angular distance P1, P2 on the sphere
	double lambda = L; // difference in longitude on an auxiliary sphere. use L for first approximation.
	double A = 0.0, B = 0.0, C = 0.0, sinSigma = 0.0, cosSigma = 0.0, cos2SM = 0.0;
	int i = 0;

	while (i < ITERATION_MAX)
	{
		double sinAlpha = 0.0, cosSqAlpha = 0.0, uSq = 0.0, delta = 0.0;
		double lambdaOld = lambda;

		double cosLambda = cos(lambda);
		double sinLambda = sin(lambda);

		double t1 = cosU2 * sinLambda;
		double t2 = cosU1 * sinU2 - sinU1 * cosU2 * cosLambda;
		sinSigma = sqrt(t1 * t1 + t2 * t2); // (14)
		cosSigma = sinU1sinU2 + cosU1cosU2 * cosLambda; // (15)

		sigma = atan2(sinSigma, cosSigma); // (16)

		sinAlpha = Double::Compare(sinSigma, 0.0) == 0 ? 0.0 : cosU1cosU2 * sinLambda / sinSigma; // (17)
		cosSqAlpha = 1.0 - (sinAlpha * sinAlpha);
		cos2SM = Double::Compare(cosSqAlpha, 0.0) == 0 ? 0.0 : cosSigma - (2.0 * sinU1sinU2 / cosSqAlpha); // (18)

		uSq = cosSqAlpha * ((a * a - b * b) / (b * b)); // u^2 = cos(alpha)^2 * (a^2 = b^2) / b^2

		A = 1.0 + (uSq / 256.0) * (64.0 + uSq * (-12.0 - (5.0 * uSq))); // (3a)
		B = (uSq / 512.0) * (128.0 + uSq * (-64.0 + (37.0 * uSq))); // (4a)
		C = (f / 16.0) * cosSqAlpha * (4.0 + f * (4.0 - 3.0 * cosSqAlpha)); // (10)

		lambda = L + ((1.0 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SM + (C * cosSigma * (-1.0 + 2.0 * cos2SM * cos2SM))))); // from (11)

		delta = (lambda - lambdaOld) / lambda;
		if (fabs(delta) < DELTA_TOLERANCE)
		{
			break;
		}
		++i;
	}

	float ret = 0.0F;
	if (RT_DISTANCE == type)
	{
		double deltaSigma = B * sinSigma * (cos2SM + (B / 4.0) * (cosSigma * (-1.0 + 2.0 * cos2SM * cos2SM))); // (6a)

		ret = b * A * (sigma - deltaSigma);

	}
	else if (RT_FORWARD_AZIMUTH == type)
	{
		double cosLambda = cos(lambda);
		double sinLambda = sin(lambda);

		ret = atan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda);  // (20)

		if (ret < 0.0F)
		{
			ret += _MathUtils::PI2;
		}
		ret *= _MathUtils::RAD2DEG;
	}

	return ret;
}

} } // Tizen::Locations