/*
 *   DIS/x : An implementation of the IEEE 1278.1 protocol
 *
 *   Copyright (C) 1997, Riley Rainey (rrainey@ix.netcom.com)
 *
 *   This library is free software; you can redistribute it and/or
 *   modify it under the terms of either:
 *
 *   a) the GNU Library General Public License as published by the Free
 *   Software Foundation; either version 2 of the License, or (at your
 *   option) any later version.  A description of the terms and conditions
 *   of the GLPL may be found in the "COPYING.LIB" file.
 *
 *   b) the "Artistic License" which comes with this Kit.  Information
 *   about this license may be found in the "Artistic" file.
 *
 *   This library 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
 *   Library General Public License or the Artistic License for more details.
 *
 *   You should have received a copy of the GNU Library General Public
 *   License along with this library; if not, write to the Free
 *   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *   Information describing how to contact the author can be found in the
 *   README file.
 */

/*
 * IMPLEMENTATION NOTES.
 * 
 * "The Surveying Handbook", edited by Brinker and Minnick contains a decent
 * discussion of the technical issues required to understand what's going on in
 * this code.
 */

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <ctype.h>
/*
 * Linker options collected by make-makefile script.
 * LINKER_OPTIONS -lm
 */
#include <math.h>

#include "../../util/error.h"
#include "../../util/units.h"

#define earth_IMPORT
#include "earth.h"
#undef earth_IMPORT


double earth_normalizeLatitude(double lat)
{
	if( ! isfinite(lat) )
		return lat;
	/* Force range [-PI,+PI] by adding/removing multiples of 2*PI: */
	if(lat > M_PI)
		lat = lat - 2*M_PI*(floor(lat / (2*M_PI)) + 1);
	if(lat < -M_PI)
		lat = lat + 2*M_PI*(floor(-lat / (2*M_PI)) + 1);
	/* Normalize to the [-PI/2,+PI/2] range: */
	if (lat > M_PI_2)
		lat = M_PI_2 - lat;
	else if (lat < -M_PI_2)
		lat = -M_PI - lat;
	return lat;
}


double earth_normalizeLongitude(double lon)
{
	if( ! isfinite(lon) )
		return lon;
	/* Force range [-PI,+PI] by adding/removing multiples of 2*PI: */
	if(lon > M_PI)
		lon = lon - 2*M_PI*(floor(lon / (2*M_PI)) + 1);
	if(lon < -M_PI)
		lon = lon + 2*M_PI*(floor(-lon / (2*M_PI)) + 1);
	/* Normalize to the ]-PI,+PI] range: */
	if( lon > M_PI )
		lon -= 2*M_PI;
	else if( lon <= - M_PI )
		lon += 2*M_PI;
	return lon;
}


void earth_updateLatLon(earth_LatLonAlt * p,
	double cos_course, double sin_course, double d_meters)
{
	double    n1, n2, m1;
	double    sin_lat, sin_lat_sqr, tan_lat, sin_course_sqr;
	double    delta_latitude, delta_longitude, d_sqr, cos_lat;
	double    B, C, /* D, */ E, h, sin_newlat;

/*  Increase our height to the height above the reference ellipsoid */

	double    wgs84_a = earth_MAJOR + p->z;

	sin_lat = sin(p->latitude);
	sin_lat_sqr = sin_lat * sin_lat;
	cos_lat = cos(p->latitude);
	tan_lat = sin_lat / cos_lat;
	sin_course_sqr = sin_course * sin_course;
	d_sqr = d_meters * d_meters;

	n1 = wgs84_a / sqrt(1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr);
	m1 = (wgs84_a * (1.0 - earth_ECCENTRICITY_SQR)) /
		pow(1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr, 1.5);

	B = 1.0 / m1;

	h = d_meters * B * cos_course;

	C = tan_lat / (2.0 * m1 * n1);

#ifdef notdef
	D = (3.0 * earth_ECCENTRICITY_SQR * sin_lat * cos_lat) /
		(2.0 * (1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr));
#endif

	E = (1.0 + 3.0 * tan_lat * tan_lat) *
		(1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr) / (6.0 * wgs84_a * wgs84_a);

	delta_latitude = d_meters * B * cos_course -
		d_sqr * C * sin_course_sqr -
		h * d_sqr * E * sin_course_sqr;

	p->latitude = earth_normalizeLatitude(p->latitude + delta_latitude);

	sin_newlat = sin(p->latitude);

	n2 = wgs84_a / sqrt(1.0 - earth_ECCENTRICITY_SQR * sin_newlat * sin_newlat);

	delta_longitude = (d_meters * sin_course) / (n2 * cos(p->latitude));

	p->longitude = earth_normalizeLongitude(p->longitude + delta_longitude);
}


void earth_updateLatLonEx(earth_LatLonAlt * p,
	double cos_course, double sin_course, double d_meters,
	double * delta_course_rad )
{
	double    n1, n2, m1;
	double    sin_lat, sin_lat_sqr, tan_lat, sin_course_sqr;
	double    delta_latitude, delta_longitude, d_sqr, cos_lat;
	double    B, C, /* D, */ E, h, sin_newlat;
	double    old_latitude, phi_m, sin_phi_m, cos_phi_m;

/* arc-seconds per rad */
	const double rho = 206264.8062470964;

/*  Increase our height to the height above the reference ellipsoid */

	double    wgs84_a = earth_MAJOR + p->z;

	sin_lat = sin(p->latitude);
	sin_lat_sqr = sin_lat * sin_lat;
	cos_lat = cos(p->latitude);
	tan_lat = sin_lat / cos_lat;
	sin_course_sqr = sin_course * sin_course;
	d_sqr = d_meters * d_meters;

	n1 = wgs84_a / sqrt(1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr);
	m1 = (wgs84_a * (1.0 - earth_ECCENTRICITY_SQR)) /
		pow(1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr, 1.5);

	B = 1.0 / m1;

	h = d_meters * B * cos_course;

	C = tan_lat / (2.0 * m1 * n1);

#ifdef notdef
	D = (3.0 * earth_ECCENTRICITY_SQR * sin_lat * cos_lat) /
		(2.0 * (1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr));
#endif

	E = (1.0 + 3.0 * tan_lat * tan_lat) *
		(1.0 - earth_ECCENTRICITY_SQR * sin_lat_sqr) / (6.0 * wgs84_a * wgs84_a);

	delta_latitude = d_meters * B * cos_course -
		d_sqr * C * sin_course_sqr -
		h * d_sqr * E * sin_course_sqr;

	old_latitude = p->latitude;

	p->latitude = earth_normalizeLatitude(p->latitude + delta_latitude);

	phi_m = old_latitude + delta_latitude / 2.0;
	sin_phi_m = sin(phi_m);
	cos_phi_m = cos(phi_m);

	sin_newlat = sin(p->latitude);

	n2 = wgs84_a / sqrt(1.0 - earth_ECCENTRICITY_SQR * sin_newlat * sin_newlat);

	delta_longitude = (d_meters * sin_course) / (n2 * cos(p->latitude));

	*delta_course_rad = delta_longitude * sin_phi_m / cos(delta_latitude / 2.0) +
		delta_longitude * (sin_phi_m * cos_phi_m * cos_phi_m) / rho;

	p->longitude = earth_normalizeLongitude(p->longitude + delta_longitude);
}

char * earth_latitudeToString(char *s, int s_capacity, double la, earth_LatLonDisplayFormat  mode)
{
	int       s100, d, m;
	double    dla, dmin;
	double    round_dm = 1.0 / (600.0 * 2.0);
	char      ns;
	
	if( ! (isfinite(la) && -M_PI_2 <= la && la <= M_PI_2) ){
		// De-normalized or INF or NAN.
		snprintf(s, s_capacity, "%g", la);
		return s;
	}
	
	// Beware: pay attention to printf() format descriptors ".1f" as a
	// rounding might take place giving unexpected results, for example
	// 59.95 would be rendered as "60.0" which is not a valid number of primes nor
	// seconds! Better using int numbers representing the wanted precision and
	// manage rounding ourself.

	round_dm = 0.0;

	switch (mode) {

	case earth_LLM_DMS:
		ns = (la >= 0.0) ? 'N' : 'S';
		s100 = units_RADtoDEG(fabs(la)) * 360000 + 0.5; // hundreds of second
		if( s100 > 360000 * 90 ){
			// Rounding fooled us...
			s100 = 360000 * 90;
		}
		d = s100 / 360000;   s100 -= 360000 * d;
		m = s100 / 6000;    s100 -= 6000 * m;
		snprintf(s, s_capacity, "%02d-%02d-%02d.%02d%c", d, m, s100 / 100, s100 % 100, ns);
		break;

	case earth_LLM_DM:
		ns = (la >= 0.0) ? 'N' : 'S';
		dla = units_RADtoDEG(fabs(la)) + round_dm;
		d = (int) dla;
		dmin = (dla - (double) d) * 60.0;
		snprintf(s, s_capacity, "%d %.1f %c", d, dmin, ns);
		break;

	case earth_LLM_D:
		ns = (la >= 0.0) ? 'N' : 'S';
		dla = units_RADtoDEG(fabs(la));
		snprintf(s, s_capacity, "%.1f %c", dla, ns);
		break;

	case earth_LLM_SIGNED_D:
		snprintf(s, s_capacity, "%.1f", units_RADtoDEG(la));
		break;
		
	default:
		error_internal("invalid format descriptor: %d", mode);
	}

	return s;
}

char * earth_longitudeToString(char *s, int s_capacity, double lo, earth_LatLonDisplayFormat  mode)
{
	int       s100, d, m;
	double    dlo, dmin;
	double    round_dm = 1.0 / (600.0 * 2.0);
	char      ew;
	
	if( ! (isfinite(lo) && -M_PI <= lo && lo <= M_PI) ){
		// De-normalized or INF or NAN.
		snprintf(s, s_capacity, "%g", lo);
		return s;
	}

	round_dm = 0.0;

	switch (mode) {

	case earth_LLM_DMS:
		ew = (lo >= 0.0) ? 'E' : 'W';
		s100 = units_RADtoDEG(fabs(lo)) * 360000 + 0.5; // hundreds of second
		if( s100 > 360000 * 180 ){
			// Rounding fooled us...
			s100 = 360000 * 180;
		}
		d = s100 / 360000;   s100 -= 360000 * d;
		m = s100 / 6000;    s100 -= 6000 * m;
		sprintf(s, "%03d-%02d-%02d.%02d%c", d, m, s100 / 100, s100 % 100, ew);
		break;

	case earth_LLM_DM:
		ew = (lo >= 0.0) ? 'E' : 'W';
		dlo = units_RADtoDEG(fabs(lo)) + round_dm;
		d = (int) dlo;
		dmin = (dlo - (double) d) * 60.0;
		snprintf(s, s_capacity, "%d %.1f %c", d, dmin, ew);
		break;

	case earth_LLM_D:
		ew = (lo >= 0.0) ? 'E' : 'W';
		dlo = units_RADtoDEG(fabs(lo));
		snprintf(s, s_capacity, "%.1f %c", dlo, ew);
		break;

	case earth_LLM_SIGNED_D:
		snprintf(s, s_capacity, "%.1f", units_RADtoDEG(lo));
		break;
		
	default:
		error_internal("invalid format descriptor: %d", mode);
	}

	return s;

}


void earth_XYZToString(char *s, int s_capacity, VPoint *xyz)
{
	snprintf(s, s_capacity, "%.0f m, %.0f m, %.0f m",
		xyz->x, xyz->y, xyz->z);
}


void earth_LatLonAltToString(char * s, int s_capacity, earth_LatLonAlt *w,
	earth_LatLonDisplayFormat  mode)
{
	char lat[99], lon[99];
	earth_latitudeToString(lat, sizeof(lat), w->latitude, mode);
	earth_longitudeToString(lon, sizeof(lon), w->longitude, mode);
	snprintf(s, s_capacity, "%s %s %.0f m", lat, lon, w->z);
}


void earth_XYZToLatLonAlt(VPoint * loc, earth_LatLonAlt * p)
{
	double    a_sqr = earth_MAJOR * earth_MAJOR, b_sqr = earth_MINOR * earth_MINOR;
	double    w, x, x_sqr, z, delta_x, cos_x;
	double    f, f_prime, w0, z0;

	w = sqrt(loc->x * loc->x + loc->y * loc->y);
	z = loc->z;

/*
 *  x is the sine of the parametric latitude.  Use the sine of the geocentric
 *  latitude as the initial guess.
 */

	if (w == 0.0 && z == 0.0) {
		p->latitude = 0.0;
		p->longitude = 0.0;
		p->z = 0.0;
		return;
	}

	x = z / sqrt(w * w + z * z);

/*
 *  Compute x with accuracy that will yield a lat/lon accuracy of
 *  about 0.0001 arc-seconds (~ 0.10 foot).
 */

	for (delta_x = 1.0; fabs(delta_x) > 4.8E-10;) {

		x_sqr = x * x;

		cos_x = sqrt(1.0 - x_sqr);

		f = 2.0 * (earth_MAJOR * x * w - a_sqr * x * cos_x - earth_MINOR * cos_x * z +
				   b_sqr * cos_x * x);

		f_prime = 2.0 * (a_sqr + 2.0 * (a_sqr * x_sqr) - earth_MAJOR * w * x_sqr +
						 b_sqr - 2.0 * b_sqr * x_sqr + earth_MINOR * x * z);

		delta_x = f / f_prime;
		x -= delta_x;
	}

	z0 = earth_MINOR * x;
	w0 = earth_MAJOR * sqrt(1.0 - x * x);

	p->z = sqrt((z - z0) * (z - z0) + (w - w0) * (w - w0));
	p->latitude = atan(z0 / (w0 * (1.0 - earth_ECCENTRICITY_SQR)));
	p->longitude = atan2(loc->y, loc->x);
}


void earth_LatLonAltToXYZ(earth_LatLonAlt * w, VPoint * p)
{
	double    N, N1;
	double    cos_latitude, sin_latitude;

	sin_latitude = sin(w->latitude);
	cos_latitude = cos(w->latitude);

/*
 *  N is the length of the normal line segment from the surface to the
 *  spin axis.
 */

	N = earth_MAJOR / sqrt(1.0 - (earth_ECCENTRICITY_SQR * sin_latitude * sin_latitude));

/*
 *  N1 lengthens the normal line to account for height above the surface
 */

	N1 = N + w->z;

	p->x = N1 * cos_latitude * cos(w->longitude);
	p->y = N1 * cos_latitude * sin(w->longitude);
	p->z = (((earth_MINOR * earth_MINOR) / (earth_MAJOR * earth_MAJOR)) * N + w->z) * sin_latitude;
}

/*
 * Symbols scanner for geographical coordinates.
 */

#define STATE_INITIAL	0
#define STATE_WORD	1
#define STATE_INTEGER	2
#define STATE_FLOAT	3

typedef enum {
	EndOfFile,
	TOKEN_FLOAT,
	TOKEN_LONG,
	TOKEN_DASH,
	TOKEN_NORTH,
	TOKEN_SOUTH,
	TOKEN_EAST,
	TOKEN_WEST
} token_id;

typedef union {
	double    double_value;
	long      long_value;
} lex_val;

static lex_val lex_value;

struct lex_record {
	char     *s;
	FILE     *f;
	int       lookahead_valid;
	int       lookahead;
	int       stack_top;
	lex_val   value_stack[16];
};

static int
input(struct lex_record *p)
{
	int       val;

	if (p->lookahead_valid) {
		p->lookahead_valid = 0;
		val = p->lookahead;
	}
	else if (p->s) {
		val = *(p->s)++;
	}
	else {
		val = fgetc(p->f);
	}
	return val;
}

#define push_value(p, type, val) \
	p->value_stack[p->stack_top++].type = val

#define pop_value(p, type) (p->value_stack[--p->stack_top].type)

#define unput(p, c)	{ p->lookahead = c; p->lookahead_valid = 1; }

#define InitializeLexRecord(p)	{ p->lookahead_valid = 0; }

static char token[256];
static int token_length = 0;

static    token_id
NextTokenx(struct lex_record *p)
{
	register int c, state = STATE_INITIAL;

	token_length = 0;

	while ((c = input(p)) != EOF) {

		switch (state) {

		case STATE_INITIAL:

			if (isspace(c)) {
				continue;
			}
			else if (isdigit(c)) {
				token[token_length++] = c;
				state = STATE_INTEGER;
			}
			else if (c == '.') {
				token[token_length++] = c;
				state = STATE_FLOAT;
			}
			else {
				token[0] = c;
				token[1] = '\0';
#ifdef DEBUG
				printf("other %s\n", token);
#endif
				switch (c) {
				case '-':
					return TOKEN_DASH;
				case 'n':
				case 'N':
					return TOKEN_NORTH;
				case 'e':
				case 'E':
					return TOKEN_EAST;
				case 's':
				case 'S':
					return TOKEN_SOUTH;
				case 'w':
				case 'W':
					return TOKEN_WEST;
/*
 *  invalid character
 */
				default:
					return EndOfFile;
				}
			}
			break;

		case STATE_INTEGER:
		case STATE_FLOAT:
			if (isspace(c) ||
				c == '-' ||
				toupper(c) == 'N' ||
				toupper(c) == 'S' ||
				toupper(c) == 'W' ||
				toupper(c) == 'E') {
				token[token_length] = '\0';
				unput(p, c);
				if (state == STATE_INTEGER) {
					lex_value.long_value = atoi(token);
					return TOKEN_LONG;
				}
				else {
					lex_value.double_value = atof(token);
					return TOKEN_FLOAT;
				}
			}
			else {
				if (c == '.') {
					state = STATE_FLOAT;
				}
				token[token_length++] = c;
			}
			break;

		default:
			token[token_length++] = c;
			break;
		}
	}

	return EndOfFile;
}

static    token_id
NextToken(struct lex_record *p)
{
	token_id  t;

	t = NextTokenx(p);

#ifdef DEBUG
	printf("token %s\n", token);
#endif
	return t;
}

static int
ParseLatitude(struct lex_record *p)
{
	double    x = 0.0;
	double    divider = 1.0;
	int       int_valid = 1;
	token_id  t;

	t = NextToken(p);
	for (;;) {
		switch (t) {
		case TOKEN_NORTH:
			lex_value.double_value = x;
			return 0;

		case TOKEN_SOUTH:
			lex_value.double_value = -x;
			return 0;

		case TOKEN_LONG:
			if (int_valid) {
				x += lex_value.long_value / divider;
				divider *= 60.0;
				t = NextToken(p);
				if (t == TOKEN_DASH) {
					t = NextToken(p);
				}
			}
			else {
				return -1;
			}
			break;

		case TOKEN_FLOAT:
			int_valid = 0;
			x += lex_value.double_value / divider;
			divider *= 60.0;
			t = NextToken(p);
			if (t == TOKEN_DASH) {
				t = NextToken(p);
			}
			break;
		default:
			return -1;
		}
	}
}

static int
ParseLongitude(struct lex_record *p)
{
	double    x = 0.0;
	double    divider = 1.0;
	int       t, int_valid = 1;

	t = NextToken(p);
	for (;;) {
		switch (t) {
		case TOKEN_EAST:
			lex_value.double_value = x;
			return 0;

		case TOKEN_WEST:
			lex_value.double_value = -x;
			return 0;

		case TOKEN_LONG:
			if (int_valid) {
				x += lex_value.long_value / divider;
				divider *= 60.0;
				t = NextToken(p);
				if (t == TOKEN_DASH) {
					t = NextToken(p);
				}
			}
			else {
				return -1;
			}
			break;

		case TOKEN_FLOAT:
			int_valid = 0;
			x += lex_value.double_value / divider;
			divider *= 60.0;
			t = NextToken(p);
			if (t == TOKEN_DASH) {
				t = NextToken(p);
			}
			break;

		default:
			return -1;
		}
	}
}


int earth_parseLatitude(char *s, double *latitude_rad)
{
	struct lex_record p;
	p.s = s;
	p.lookahead_valid = 0;
	if (ParseLatitude(&p) != 0)
		return 0;
	*latitude_rad = units_DEGtoRAD(lex_value.double_value);
	return *p.s == 0;
}


int earth_parseLongitude(char *s, double *longitude_rad)
{
	struct lex_record p;
	p.s = s;
	p.lookahead_valid = 0;
	if (ParseLongitude(&p) != 0)
		return 0;
	*longitude_rad = units_DEGtoRAD(lex_value.double_value);
	return *p.s == 0;
}


char     *
earth_parseLatLon(char *s, earth_LatLonAlt * w)
{
	struct lex_record p;

	p.s = s;
	p.lookahead_valid = 0;

	if (ParseLatitude(&p) != 0) {
		return 0;
	}
	w->latitude = units_DEGtoRAD(lex_value.double_value);

	if (ParseLongitude(&p) != 0) {
		return 0;
	}
	w->longitude = units_DEGtoRAD(lex_value.double_value);
	w->z = 0.0;
	return p.s;
}


void earth_generateWorldToLocalMatrix(earth_LatLonAlt *w, VMatrix *XYZtoNED)
{
	VPoint gc, p;
	VIdentMatrix(XYZtoNED);
	VRotate(XYZtoNED, ZRotation, -w->longitude);
	VRotate(XYZtoNED, YRotation, w->latitude);
	VRotate(XYZtoNED, YRotation, units_DEGtoRAD(90.0));
	earth_LatLonAltToXYZ(w, &gc);
	VTransform(&gc, XYZtoNED, &p);
	XYZtoNED->m[0][3] = -p.x;
	XYZtoNED->m[1][3] = -p.y;
	XYZtoNED->m[2][3] = -p.z;
}
