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


/* TODO complete the implementation */

void
libtellurian_end_point_radians(double latitude1, double longitude1, double azimuth1, double distance,
                               double *latitude2_out, double *longitude2_out, double *azimuth2_out)
{
	/*
	 * Vincenty's formula for the "direct problem"
	 */
	double t, sigma, cos_2sigma_m, sin_sigma, cos_sigma;
	double x, x0, y, C, L, v, lambda;

	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 sin_u1 = sin(u1);
	double cos_u1 = cos(u1);
	double sin_alpha1 = sin(azimuth1);
	double cos_alpha1 = cos(azimuth1);

	double sigma1 = atan2(tan(u1), cos_alpha1);

	double sin_alpha = cos_u1 * sin_alpha1;
	double cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0);

	double uu = cos2_alpha * fma(a / b, a / b, -1.0);

	double A = fma(fma(fma(fma(-175.0, uu, 320.0), uu, -768.0), uu, 4096.0), uu / 16384.0, 1.0);
	double B = fma(fma(fma(-47.0, uu, 74.0), uu, -128.0), uu, 256.0) * (uu / 1024.0);

	double sigma_0 = distance / (b * A);

	sigma = sigma_0;

	/* { */

	cos_2sigma_m = cos(fma(2.0, sigma1, sigma));
	sin_sigma = sin(sigma);
	cos_sigma = cos(sigma);

	v = cos_sigma * fma(2 * cos_2sigma_m, cos_2sigma_m, -1.0);
	t = fma(2.0 * cos_2sigma_m, 2.0 * cos_2sigma_m, -3.0);
	t *= fma(2.0 * sin_sigma, 2.0 * sin_sigma, -3.0);
	t = fma(B * cos_2sigma_m / -6.0, t, v);
	t = fma(0.25 * B, t, cos_2sigma_m);
	sigma = fma(B * sin_sigma, t, sigma_0);

	/* } repeat until sigma converges */

	if (latitude2_out || azimuth2_out)
		x0 = fma(cos_sigma * cos_alpha1, cos_u1, -sin_u1 * sin_alpha);

	if (latitude2_out) {
		y = fma(sin_sigma * cos_alpha1, cos_u1, sin_u1 * cos_sigma);
		x = sqrt(fma(sin_alpha, sin_alpha, x0 * x0)) * c;
		*latitude2_out = atan2(y, x);
	}

	if (azimuth2_out)
		*azimuth2_out = atan2(sin_alpha, x0);

	if (longitude2_out) {
		C = fma(fma(-0.75, cos2_alpha, 1.0), f, 1.0) * cos2_alpha * f / 4.0;
		y = sin_sigma * sin_alpha1;
		x = fma(sin_sigma * cos_alpha1, sin_u1, cos_u1 * cos_sigma);
		lambda = atan2(y, x);
		t = fma(C * sin_sigma, fma(C, v, cos_2sigma_m), sigma);
		L = fma(fma(C, f, -f) * sin_alpha, t, lambda);
		*longitude2_out = longitude1 + L;
	}
}