#include <math.h>
#include <stdlib.h>

struct pair_coord {
	double lat1;
	double lon1;
	double lat2;
	double lon2;
};

double fai_sphere(double lat1r, double lon1r, double lat2r, double lon2r) {
	const double FAI_SPHERE_RADIUS = 6371000.0;

	double p1 = sin((lat1r - lat2r) / 2.0);
	double p2 = sin((lon1r - lon2r) / 2.0);

	double ad = 2.0 * asin(sqrt(p1 * p1 + cos(lat1r) * cos(lat2r) * p2 * p2));

	return ad * FAI_SPHERE_RADIUS;
}


double wgs84_meeus(double lat1r, double lon1r, double lat2r, double lon2r) {
	const double SEMI_MAJOR_AXIS = 6378137.0;
	const double FLATTENING = 1.0 / 298.257223563;

	double f = (lat1r + lat2r) / 2.0;
	double g = (lat1r - lat2r) / 2.0;
	double l = (lon1r - lon2r) / 2.0;

	double sing = sin(g); sing *= sing;
	double cosl = cos(l); cosl *= cosl;
	double cosf = cos(f); cosf *= cosf;
	double sinl = sin(l); sinl *= sinl;
	double sinf = sin(f); sinf *= sinf;
	double cosg = cos(g); cosg *= cosg;

	double s = sing * cosl + cosf * sinl;
	double c = cosg * cosl + sinf * sinl;
	double w = atan2(sqrt(s), sqrt(c));
	double r = sqrt((s * c)) / w;
	double h1 = (3.0 * r - 1.0) / (2.0 * c);
	double h2 = (3.0 * r + 1.0) / (2.0 * s);
	double d = 2.0 * w * SEMI_MAJOR_AXIS;

	return (d * (1.0 + FLATTENING * h1 * sinf * cosg - FLATTENING * h2 * cosf * sing));
}


double wgs84_andoyer(double lat1r, double lon1r, double lat2r, double lon2r) {
	const double SEMI_MAJOR_AXIS = 6378137.0;
	const double FLATTENING = 1.0 / 298.257223563;
	const double c0 = 0.0;
	const double c1 = 1.0;

	double dlon = lon2r - lon1r;
	double cos_dlon = cos(dlon);
	double sin_lat1 = sin(lat1r);
	double cos_lat1 = cos(lat1r);
	double sin_lat2 = sin(lat2r);
	double cos_lat2 = cos(lat2r);

	double cos_d = sin_lat1 * sin_lat2 + cos_lat1 * cos_lat2 * cos_dlon;

	if (cos_d < -c1) cos_d = -c1;
	else if (cos_d > c1) cos_d = c1;

	double d = acos(cos_d);
	double sin_d = sin(d);

	double diffsinlat = sin_lat1 - sin_lat2;
	double sumsinlat = sin_lat1 + sin_lat2;
	double k = diffsinlat * diffsinlat;
	double l = sumsinlat * sumsinlat;
	double three_sin_d = 3.0 * sin_d;

	double one_minus_cos_d = c1 - cos_d;
	double one_plus_cos_d = c1 + cos_d;

	double h = (one_minus_cos_d == c0) ? c0 : (d + three_sin_d) / one_minus_cos_d;
	double g = (one_plus_cos_d == c0) ? c0 : (d - three_sin_d) / one_plus_cos_d;

	double dd = -(FLATTENING / 4.0) * (h * k + g * l);

	return SEMI_MAJOR_AXIS * (d + dd);
}

double wgs84_vincenty(double lat1r, double lon1r, double lat2r, double lon2r)
{
	const double a = 6378137.0, b = 6356752.314245, f = 1.0 / 298.257223563;

	double L = (lon2r - lon1r);
	double U1 = atan((1.0 - f) * tan(lat1r));
	double U2 = atan((1.0 - f) * tan(lat2r));

	double sinU1 = sin(U1), cosU1 = cos(U1);
	double sinU2 = sin(U2), cosU2 = cos(U2);
	double cosSqAlpha;
	double sinSigma;
	double cos2SigmaM;
	double cosSigma;
	double sigma;

	double lambda = L, lambdaP;
	int iterLimit = 100;
	do
	{
		double sinLambda = sin(lambda), cosLambda = cos(lambda);
		sinSigma = sqrt((cosU2 * sinLambda)
			* (cosU2 * sinLambda)
			+ (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
			* (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
			);
		if (sinSigma == 0)
		{
			return 0;
		}

		cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
		sigma = atan2(sinSigma, cosSigma);
		double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
		cosSqAlpha = 1 - sinAlpha * sinAlpha;
		cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;

		double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
		lambdaP = lambda;
		lambda = L + (1 - C) * f * sinAlpha
			* (sigma + C * sinSigma
			* (cos2SigmaM + C * cosSigma
			* (-1 + 2 * cos2SigmaM * cos2SigmaM)));


	} while (abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

	if (iterLimit == 0)
	{
		return 0;
	}

	double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
	double A = 1 + uSq / 16384
		* (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
	double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
	double deltaSigma
		= B * sinSigma
		* (cos2SigmaM + B / 4
		* (cosSigma
		* (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM
		* (-3 + 4 * sinSigma * sinSigma)
		* (-3 + 4 * cos2SigmaM * cos2SigmaM)));

	return b * A * (sigma - deltaSigma);
}

int main(void)
{
	const long TESTCOUNT = 1000;
	const double RANDMAX = (double)RAND_MAX;
	const double DEG2RAD = M_PI / 180.0;

	pair_coord* mem = (pair_coord*)malloc(TESTCOUNT * sizeof(pair_coord));

	srand(2);

	for (long i = 0; i < TESTCOUNT; i++) {
		double baseLat = (double)rand() * 60.0 / RANDMAX;
		pair_coord* x = &mem[i];
		x->lat1 = (baseLat + (double)rand() * 3.0 / RANDMAX - 1.5) * DEG2RAD;
		x->lon1 = ((double)rand() * 3.0 / RANDMAX - 1.5) * DEG2RAD;
		x->lat2 = (baseLat + (double)rand() * 3.0 / RANDMAX - 1.5) * DEG2RAD;
		x->lon2 = ((double)rand() * 3.0 / RANDMAX - 1.5) * DEG2RAD;
	}

	while(true) {

		// set the output pin to HIGH state

		double s = 0;
		for (long i = 0; i < TESTCOUNT; i++) {
			pair_coord* x = &mem[i];
			double d = fai_sphere(x->lat1, x->lon1, x->lat2, x->lon2);
			s += d;
		}

		// set the output pin to LOW state

		s = 0;
		for (long i = 0; i < TESTCOUNT; i++) {
			pair_coord* x = &mem[i];
			double d = wgs84_andoyer(x->lat1, x->lon1, x->lat2, x->lon2);
			s += d;
		}

	}

	return 0;
}