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


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

	sigma = sigma_0;

	do {
		sigma_prev = sigma;

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

	} while (fabs(sigma - sigma_prev) > 1e-14 && --max_interations);

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


#else


# define PM LIBTELLURIAN_MERIDIONAL_CIRCUMFERENCE
# define PE LIBTELLURIAN_EQUATORIAL_CIRCUMFERENCE

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

static double
normal_angle(double u)
{
	u = fmod(u + 4.0 * M_PI, 2.0 * M_PI);
	if (u > M_PI)
		u -= 2.0 * M_PI;
	return u;
}

static void
end_point(double a, double b, double c, double d,
          double *lat_out, double *lon_out, double *az_out)
{
	libtellurian_end_point_radians(a, b, c, d, lat_out, lon_out, az_out);
	if (lat_out) {
		*lat_out = normal_angle(*lat_out);
		if (*lat_out > M_PI / 2) {
			*lat_out = M_PI - *lat_out;
			if (*lon_out)
				*lon_out += M_PI;
		} else if (*lat_out < -M_PI / 2) {
			*lat_out = -M_PI - *lat_out;
			if (*lon_out)
				*lon_out += M_PI;
		}
	}
	if (lon_out)
		*lon_out = normal_angle(*lon_out);
	if (az_out)
		*az_out = normal_angle(*az_out);
}

int
main(void)
{
	double lat, lon, az;

	end_point(0, 0, 0, 0, &lat, &lon, &az);
	ASSERT(lat == 0);
	ASSERT(lon == 0);
	ASSERT(az == 0);

	end_point(0, 0, 3, 0, &lat, &lon, &az);
	ASSERT(lat == 0);
	ASSERT(lon == 0);
	ASSERT(az == 3);

	end_point(0, 0, 6, 0, &lat, &lon, &az);
	ASSERT(lat == 0);
	ASSERT(lon == 0);
	ASSERT(az == normal_angle(6));

	end_point(0, 0, 0, PM / 4, &lat, &lon, &az);
	ASSERT(approx(lat, D90));
	ASSERT(approx(lon, 0));

	end_point(0, 0, 0, PM / 4, NULL, &lon, &az);
	ASSERT(approx(lon, 0));

	end_point(0, 0, 0, PM / 2, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, D180));
	ASSERT(approx(az, D180));

	end_point(0, 0, 0, PM / 2, &lat, NULL, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(az, D180));

	end_point(0, 0, 0, PM / 2, &lat, &lon, NULL);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, D180));

	end_point(0, 0, 0, PM / 2, &lat, NULL, NULL);
	ASSERT(approx(lat, 0));

	end_point(0, 0, 0, PM / 2, NULL, NULL, NULL);

	end_point(0, 0, 0, PM / 2, NULL, NULL, &az);
	ASSERT(approx(az, D180));

	end_point(0, 0, 0, PM / 2, NULL, &lon, NULL);
	ASSERT(approx(lon, D180));

	end_point(0, 0, 0, PM, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, 0));
	ASSERT(approx(az, 0));

	end_point(0, 0, 0, 2 * PM, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, 0));
	ASSERT(approx(az, 0));

	end_point(0, 0, 0, 3 * PM / 2, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, D180));
	ASSERT(approx(az, D180));

	end_point(0, 0, D90, PE, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, 0));
	ASSERT(approx(az, D90));

	end_point(0, 0, D90, PE / 8, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, D180 / 4));
	ASSERT(approx(az, D90));

	end_point(0, 0, D90, PE / 4, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, D90));
	ASSERT(approx(az, D90));

	end_point(0, 0, D90, PE / 2, &lat, &lon, &az);
	ASSERT(approx(lat, 0));
	ASSERT(approx(lon, D180));
	ASSERT(approx(az, D90));

	return 0;
}


#endif