/* See LICENSE file for copyright and license details. */
#include "common.h"
#ifndef TEST
#include <stdio.h> // TODO remove


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;
	double lambda_prev;
	int max_interations = 20;

	lambda = L;

	do {
		lambda_prev = lambda;

		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;
		if (!isfinite(sin_alpha)) {
			fprintf(stderr, "BAD\n");
			if (distance_out) {
				/* Dot product of vectors is 0 if coincidental, but π if antipodal */
				/* TODO it may be fast to see if latitude1+latitude2≋0 and longitude2-longitude1≋π */
				t = sin(latitude1) * sin(latitude2);
				t = fma(cos(latitude1) * cos(latitude2), cos(longitude2 - longitude1), t);
				if (t > 0.0) {
					fprintf(stderr, "  COINCIDENTAL\n");
					*distance_out = 0;
				} else {
					fprintf(stderr, "  ANTIPODAL\n");
					*distance_out = LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE;
				}
			}
			if (azimuth1_out)
				*azimuth1_out = nan("");
			if (azimuth2_out)
				*azimuth2_out = nan("");
			return;
		}
		cos2_alpha = fma(-sin_alpha, sin_alpha, 1.0);

		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);

	} while (lambda != lambda_prev && --max_interations); /* repeat until lambda converges */
	fprintf(stderr, "%lg => %lg -> %lg : %lg (%i)\n", L, lambda_prev, lambda, lambda - lambda_prev, max_interations);

	if (distance_out) {
		uu = cos2_alpha * fma(c, c, -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);
		*azimuth2_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


# define PE LIBTELLURIAN_EQUATORIAL_CIRCUMFERENCE
# define PM LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE

static int
approx(double a, double e)
{
        return fabs((a / e) - 1.0) <= 1.0e-8;
}

int
main(void)
{
	double s, a1, a2;

#define RESET ((void)(s = 111, a1 = 222, a2 = 333))

	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, &a1, &a2);
	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, &a1, NULL);
	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, NULL, &a2);
	libtellurian_vincenty_inverse__(1, 2, 3, 4, &s, NULL, NULL);
	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, &a1, &a2);
	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, &a1, NULL);
	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, NULL, &a2);
	libtellurian_vincenty_inverse__(1, 2, 3, 4, NULL, NULL, NULL);

	RESET;
	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, &a1, &a2);
	ASSERT(s == 0);
	ASSERT(isnan(a1));
	ASSERT(isnan(a2));

	RESET;
	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, &a1, &a2);
	ASSERT(isnan(a1));
	ASSERT(isnan(a2));

	RESET;
	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, NULL, &a2);
	ASSERT(s == 0);
	ASSERT(isnan(a2));

	RESET;
	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, NULL, &a2);
	ASSERT(isnan(a2));

	RESET;
	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, &a1, NULL);
	ASSERT(s == 0);
	ASSERT(isnan(a1));

	RESET;
	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, &a1, NULL);
	ASSERT(isnan(a1));

	RESET;
	libtellurian_vincenty_inverse__(0, 0, 0, 0, &s, NULL, NULL);
	ASSERT(s == 0);

	libtellurian_vincenty_inverse__(0, 0, 0, 0, NULL, NULL, NULL);

	fprintf(stderr, "--------\n");
	RESET;
	libtellurian_vincenty_inverse__(0, 0, D45, 0, &s, &a1, &a2);
	ASSERT(approx(s, PM / 4));
	ASSERT(isnan(a1));
	ASSERT(isnan(a2));

	return 0;
}


#endif