/* See LICENSE file for copyright and license details. */
#include "common.h"


/* TODO complete the implementation */

void
libtellurian_vincenty_inverse__(double latitude1, double longitude1, double latitude2, double longitude2,
                                double *distance_out, double *azimuth1_out, double *azimuth2_out)
{
	/*
	 * Vincenty's formula for the "inverse problem"
	 */

	double lambda, t, uu, A, B, sigma_minus_delta_sigma;
	double x, y, cos_lambda, sin_lambda, sin_sigma, cos_sigma, sigma;
	double sin_alpha, cos2_alpha, cos_2sigma_m, C, cos2_2sigma_m;

	double a = LIBTELLURIAN_EQUATORIAL_RADIUS;
	double b = LIBTELLURIAN_POLAR_RADIUS;
	double c = b / a;
	double f = 1.0 - c;

	double u1 = atan(c * tan(latitude1));
	double u2 = atan(c * tan(latitude2));

	double cos_u1 = cos(u1), sin_u1 = sin(u1);
	double cos_u2 = cos(u2), sin_u2 = sin(u2);

	double cos_u1_sin_u2 = cos_u1 * sin_u2;
	double sin_u1_cos_u2 = sin_u1 * cos_u2;
	double cos_u1_cos_u2 = cos_u1 * cos_u2;
	double sin_u1_sin_u2 = sin_u1 * sin_u2;

	double L = longitude2 - longitude1;

	lambda = L;

	/* { */

	cos_lambda = cos(lambda);
	sin_lambda = sin(lambda);

	y = cos_u2 * sin_lambda;
	x = fma(-sin_u1_cos_u2, cos_lambda, cos_u1_sin_u2);

	sin_sigma = sqrt(fma(y, y, x * x));
	cos_sigma = fma(cos_u1_cos_u2, cos_lambda, sin_u1_sin_u2);
	sigma = atan2(sin_sigma, cos_sigma);

	sin_alpha = (cos_u1_cos_u2 * sin_lambda) / sin_sigma;
	cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0);
	/*
	 * If sin σ = 0 the value of sin α is indeterminate.
	 * It represents an end point coincident with, or
	 * diametrically opposed to, the start point.
	 */

	cos_2sigma_m = fma(-2.0 / cos2_alpha, sin_u1_sin_u2, cos_sigma);
	C = 0.25 * f * cos2_alpha * fma(f, fma(-0.75, cos2_alpha, 1.0), 1.0);

	cos2_2sigma_m = cos_2sigma_m * cos_2sigma_m;
	t = fma(2.0, cos2_2sigma_m, -1.0);
	t = fma(C * cos_sigma, t, cos_2sigma_m);
	t = fma(C * sin_sigma, t, sigma);
	lambda = fma(fma(C, f, -f) * sin_alpha, t, L);

	/* } repeat until lambda converges */

	if (distance_out) {
		uu = cos2_alpha * fma(a / b, a / b, -1.0);
		A = fma(fma(fma(fma(-175.0, uu, 320.0), uu, -768.0), uu, 4096.0), uu / 16384.0, 1.0);
		B = fma(fma(fma(-47.0, uu, 74.0), uu, -128.0), uu, 256.0) * (uu / 1024.0);

		t = fma(4.0, cos2_2sigma_m, -3.0);
		t *= fma(2.0 * sin_sigma, 2.0 * sin_sigma, -3.0);
		t *= cos_2sigma_m * B / -6.0;
		t = fma(cos_sigma, fma(2.0, cos2_2sigma_m, -1.0), t);
		t = fma(t, B / 4.0, cos_2sigma_m);
		sigma_minus_delta_sigma = fma(t, -B * sin_sigma, sigma);

		*distance_out = b * A * sigma_minus_delta_sigma;
	}

	if (azimuth1_out)
		*azimuth1_out = atan2(y, x);

	if (azimuth2_out) {
		y = cos_u1 * sin_lambda;
		x = fma(cos_u1_sin_u2, cos_lambda, -sin_u1_cos_u2);
		*azimuth1_out = atan2(y, x);
	}

	/*
	 * Between two nearly antipodal points, the iterative formula may fail to converge;
	 * this will occur when the first guess at λ as computed by the equation above is
	 * greater than π in absolute value.
	 */

	/*
	 * https://en.wikipedia.org/wiki/Vincenty's_formulae#Vincenty's_modification
	 * https://en.wikipedia.org/wiki/Vincenty's_formulae#Nearly_antipodal_points
	 */
}