/*
 *  geo-dist
 *
 *  Copyright (C) 2006 Nils Faerber <nils.faerber@kernelconcepts.de>
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */


/*
 * Calculate distances on Earth surface given geogrphic locations
 */

#include <stdio.h>
#include <math.h>

#include "geo-dist.h"

// #define DEBUG


/*
 * Earth models
 * Remember, Earth is not a simple ball ;)
 */
struct ellipse_s {
	char *name;
	double a;
	double invf;
};

static struct ellipse_s ellipse[] = {
	{
		"WGS84",
		6378.137 / 1.852,
		298.257223563
	}, {
		"NAD27",
		6378.2064 / 1.852,
		294.9786982138
	}, {
		"International",
		6378.388 / 1.852,
		297.0
	}, {
		"Krasovsky",
		6378.245 / 1.852,
		298.3
	}, {
		"Bessel",
		6377.397155 / 1.852,
		299.1528
	}, {
		"WGS72",
		6378.135 / 1.852,
		298.26
	}, {
		"WGS66",
		6378.145 / 1.852,
		298.25
	}, {
		"FAI sphere",
		6371.0 / 1.852,
		1000000000.
	}
};


/*
 * math helper
 */
#if 0
double atan2(double y, double x)
{
double out=0.;

	if (x < 0) {
		out = atan(y/x) + M_PI;
	} else
	if ((x > 0) && (y >= 0)) {
		out = atan(y/x);
	} else
	if ((x > 0) && (y < 0)) {
		out = atan(y/x) + 2. * M_PI;
	} else
	if ((x == 0) && (y > 0)) {
		out = M_PI / 2.;
	} else
	if ((x == 0) && (y < 0)) {
		out = 3. * M_PI / 2.;
	} else
	if ((x == 0) && (y == 0)) {
		out= 0.;
	}

return out;
}
#endif

/*
 * glat1, glat2: geodetic latitude in radians, N positive
 * glon1, glon2: geodetic longitude in radians E positive
 * Coordinates in radians means
 * 50°50.32N ^= 50 degree, 50 minutes and 32 seconds
 *           ^= 50 + (50 / 60) + (32 / 3600)
 *           ^= 50.84222
 *
 * returns: distance in nm (nautical miles), factor of 1.852 gives km
 */
double dist_ell(double glat1, double glon1, double glat2, double glon2, EllipsoidType e_model)
{
double a = ellipse[e_model].a; // 6378.137/1.852
double invf = ellipse[e_model].invf; // 298.257223563
double f = 1. / invf;
double r, tu1, tu2, cu1, su1, cu2, s1, b1, f1;
double x, sx, cx, sy=0, cy=0, y=0, sa, c2a=0, cz=0, e=0, c, d;
double EPS = 0.00000000005;
double s;
double l_iter=1;
double MAXITER=100;

#ifdef DEBUG
	printf("  calc dist from %.04lf %.04lf to %.04lf %.04lf\n", glat1, glon1, glat2, glon2);
#endif
	glat1 = (M_PI / 180.) * glat1;
	glon1 = (M_PI / 180.) * -1. * glon1;
	glat2 = (M_PI / 180.) * glat2;
	glon2 = (M_PI / 180.) * -1. * glon2;

#ifdef DEBUG
	printf("Using ellipse model %s (%.04lf %.04lf)\n", ellipse[e_model].name, a, invf);
	printf("  calc dist from %.04lf %.04lf to %.04lf %.04lf\n", glat1, glon1, glat2, glon2);
#endif

	if ((glat1+glat2==0.) && (fabs(glon1-glon2)==M_PI)) {
#ifdef DEBUG
		printf("Warning: Course and distance between antipodal points is undefined\n");
#endif
		glat1=glat1+0.00001; // allow algorithm to complete
	}

	if (glat1==glat2 && (glon1==glon2 || fabs(fabs(glon1-glon2)-2*M_PI) < EPS)) {
#ifdef DEBUG
		printf("Points 1 and 2 are identical- course undefined\n");
#endif
		return 0;
	}

	r = 1. - f;
	tu1 = r * tan (glat1);
	tu2 = r * tan (glat2);
	cu1 = 1. / sqrt (1. + tu1 * tu1);
	su1 = cu1 * tu1;
	cu2 = 1. / sqrt (1. + tu2 * tu2);
	s1 = cu1 * cu2;
	b1 = s1 * tu2;
	f1 = b1 * tu1;
	x = glon2 - glon1;
	d = x + 1.; // force one pass

	while ((fabs(d - x) > EPS) && (l_iter < MAXITER)) {
		l_iter = l_iter + 1;
		sx = sin (x);
		cx = cos (x);
		tu1 = cu2 * sx;
		tu2 = b1 - su1 * cu2 * cx;
		sy = sqrt (tu1 * tu1 + tu2 * tu2);
		cy = s1 * cx + f1;
		y = atan2 (sy, cy);
		sa = s1 * sx / sy;
		c2a = 1. - sa * sa;
		cz = f1 + f1;
		if (c2a > 0.)
			cz = cy - cz / c2a;
		e = cz * cz * 2. - 1.;
		c = ((-3. * c2a + 4.) * f + 4.) * c2a * f / 16.;
		d = x;
		x = ((e * cy * c + cz) * sy * c + y) * sa;
		x = (1. - c) * x * f + glon2 - glon1;
	}

	x = sqrt ((1. / (r * r) - 1) * c2a + 1.);
	x += 1.;
	x = (x - 2.) / x;
	c = 1. - x;
	c = (x * x / 4. + 1.) / c;
	d = (0.375 * x * x - 1.) * x;
	x = e * cy;
	s = ((((sy*sy*4.-3.)*(1.-e-e)*cz*d/6.-x)*d/4.+cz)*sy*d+y)*c*a*r;
#ifdef DEBUG
	if (fabs (l_iter - MAXITER) < EPS) {
		printf ("Algorithm did not converge\n");
	} else
		fprintf(stderr,"%s = %.4f\n", __FUNCTION__, s);
#endif

return s;
}