/* See LICENSE file for copyright and license details. */
#include "common.h"
#ifndef TEST
/* 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
*/
}
#else
TODO_TEST
#endif