/*
 * Test code for the 'earth' module.
 * Exits with status code 0 on success, 1 on failure.
 */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "../../../src/dis/dis/earth.h"
#include "../../../src/util/units.h"
#include "../../../src/util/memory.h"


static int err = 0;

#ifdef xxxxxxxxxxxxxxxxxxxxxx
void earth_XYZToLatLonAlt(VPoint * loc, earth_LatLonAlt * p)
{
	double a2 = earth_MAJOR * earth_MAJOR;
	double e4 = earth_ECCENTRICITY_SQR * earth_ECCENTRICITY_SQR;
	double zeta = (1 - earth_ECCENTRICITY_SQR) * loc->z / a2;
	double p2 = loc->x * loc->x + loc->y * loc->y;
	double p = sqrt(p);
	double rho = (p2 / a2 + zeta - e4) / 6;
	double s = e4 * zeta * p2 / (4 * a2);
	double t = pow(rho*rho*rho + s + sqrt(s*(s+2*rho*rho*rho)));
	double u = rho + t + rho*rho/t;
	double v = sqrt(u*u + e4*zeta);
	double w = earth_ECCENTRICITY_SQR*(u+v-zeta)/(2*v);
	double k = 1 + earth_ECCENTRICITY_SQR*(sqrt(u+v+w*w) + w) / (u+v);
	p->latitude = atan2(k*loc->z, w);
	p->longitude = atan2(loc->y, loc->x);
	p->z = 1;
}
#endif


/**
 * Check numerical conversions of Cartesian to geographic coordinates back and forth.
 */
static void coordsConversionsTest(int line)
{
	double latitude_deg, longitude_deg;
	double z = 1000.0;
	for(latitude_deg = -80; latitude_deg <= 80; latitude_deg += 10){
		for(longitude_deg = -180; longitude_deg <= 180; longitude_deg += 10){
			
			// Converts geographic to Cartesian:
			earth_LatLonAlt geographic;
			geographic.latitude = units_DEGtoRAD(latitude_deg);
			geographic.longitude = units_DEGtoRAD(longitude_deg);
			geographic.z = z;
			VPoint cartesian;
			earth_LatLonAltToXYZ(&geographic, &cartesian);
			
			// Convert back to geographic:
			earth_LatLonAlt geographic2;
			earth_XYZToLatLonAlt(&cartesian, &geographic2);
			
			// Evaluate error on the resulting geographic coordinates:
			double err_latitude_deg = units_RADtoDEG(geographic2.latitude - geographic.latitude);
			double err_longitude_deg = units_RADtoDEG(geographic2.longitude - geographic.longitude);
			double err_z = geographic2.z - geographic.z;
			if( fabs(err_latitude_deg) > 1e-6 // about 0.1 m
			|| fabs(err_longitude_deg) > 1e-6 // about 0.1 m
			|| fabs(err_z) > 0.1){
				err++;
				printf("in line %d, %g,%g,%g --> %g,%g,%g (err: %g,%g,%g)\n",
					line,
					units_RADtoDEG(geographic.latitude),
					units_RADtoDEG(geographic.longitude),
					geographic.z,
					units_RADtoDEG(geographic2.latitude),
					units_RADtoDEG(geographic2.longitude),
					geographic2.z,
					err_latitude_deg,
					err_longitude_deg,
					err_z
				);
			}
		}
	}
}


static double sqr(double x)
{
	return x * x;
}


static double distance(earth_LatLonAlt *a, earth_LatLonAlt *b)
{
	VPoint p, q;
	earth_LatLonAltToXYZ(a, &p);
	earth_LatLonAltToXYZ(b, &q);
	return sqrt( sqr(p.x - q.x) + sqr(p.y - q.y) + sqr(p.z - q.z) );
}


/**
 * Testing Cartesian to geographic string conversion forth and back.
 * Testing earth_updateLatLon().
 */
static void test_conversions(int line, VPoint *xyz,
	char *exp_xyz, char *exp_geo)
{
	char xyz_s[99];
	VPoint xyz2;       char xyz2_s[99];
	earth_LatLonAlt geo;  char geo_s[99];
	earth_LatLonAlt geo2; char geo2_s[99];
	double step, delta;
	
	/* Convert Cartesian to string and check: */
	earth_XYZToString(xyz_s, sizeof(xyz_s), xyz);
	if( strcmp(xyz_s, exp_xyz) != 0 ){
		err++;
		printf("in line %d formatted Cartesian:\n\tgot: %s\n\texp: %s\n",
			line, xyz_s, exp_xyz);
	}
	
	/* Convert Cartesian to geographic and check: */
	earth_XYZToLatLonAlt(xyz, &geo);
	earth_LatLonAltToString(geo_s, sizeof(geo_s), &geo, earth_LLM_D);
	if( strcmp(geo_s, exp_geo) != 0 ){
		err++;
		printf("in line %d converting to geographic:\n\tgot: %s\n\texp: %s\n",
			line, geo_s, exp_geo);
		printf("lon=%g\n", geo.longitude);
	}
	
	/* Convert back geographic to Cartesian and check: */
	earth_LatLonAltToXYZ(&geo, &xyz2);
	earth_XYZToString(xyz2_s, sizeof(xyz2_s), &xyz2);
	if( strcmp(xyz2_s, exp_xyz) != 0 ){
		err++;
		printf("in line %d converting back to Cartesian:\n\tgot: %s\n\texp: %s\n",
			line, xyz2_s, exp_xyz);
	}
	
	/*
	 * Move E, N, S, W, returning to the same point.
	 * FIXME: Skip this test near the poles because the behavior of
	 * earth_updateLatLon() is undefined there and the result are completely
	 * random numbers.
	 */
	if( fabs(geo.latitude) < units_DEGtoRAD(89) ){
		geo2 = geo;
		step = units_NMtoMETERS(120);
		earth_updateLatLon(&geo2, 0.0, 1.0, step);
		earth_updateLatLon(&geo2, 1.0, 0.0, step);
		earth_updateLatLon(&geo2, -1.0, 0.0, step);
		earth_updateLatLon(&geo2, 0.0, -1.0, step);
		delta = distance(&geo2, &geo);
		if( delta > 10.0 /* m */ ){
			earth_LatLonAltToString(geo2_s, sizeof(geo2_s), &geo2, earth_LLM_D);
			err++;
			printf("in line %d after a closed loop:\n\tgot: %s\n\texp: %s\n\tdst: %g m\n",
				line, geo2_s, geo_s, delta);
		}
	}
}


int main()
{
	coordsConversionsTest(__LINE__);
	
	VPoint xyz;
	
	//parser_test();
	
	/* North pole, sea level: */
	xyz.x = 0.0;  xyz.y = 0.0;  xyz.z = earth_MINOR;
	test_conversions(__LINE__, &xyz, "0 m, 0 m, 6356752 m", "90.0 N 0.0 E 0 m");
	
	/* South pole, sea level: */
	xyz.x = 0.0;  xyz.y = 0.0;  xyz.z = - earth_MINOR;
	test_conversions(__LINE__, &xyz, "0 m, 0 m, -6356752 m", "90.0 S 0.0 E 0 m");
	
	/* Greenwich crossing equator: */
	xyz.x = earth_MAJOR;  xyz.y = 0.0;  xyz.z = 0.0;
	test_conversions(__LINE__, &xyz, "6378137 m, 0 m, 0 m", "0.0 N 0.0 E 0 m");
	
	/* 90E meridian crossing equator: */
	xyz.x = 0.0;  xyz.y = earth_MAJOR;  xyz.z = 0.0;
	test_conversions(__LINE__, &xyz, "0 m, 6378137 m, 0 m", "0.0 N 90.0 E 0 m");
	
	/* 180E meridian crossing equator: */
	xyz.x = - earth_MAJOR;  xyz.y = 0.0;  xyz.z = 0.0;
	test_conversions(__LINE__, &xyz, "-6378137 m, 0 m, 0 m", "0.0 N 180.0 E 0 m");
	
	/* 90W meridian crossing equator: */
	xyz.x = 0.0;  xyz.y = - earth_MAJOR;  xyz.z = 0.0;
	test_conversions(__LINE__, &xyz, "0 m, -6378137 m, 0 m", "0.0 N 90.0 W 0 m");
	
	err += memory_report();
	return err == 0? 0 : 1;
}