#include "pigeoposition.h"

const double PIGeoPosition::one_cm_tolerance = 0.01; // One centimeter tolerance.
const double PIGeoPosition::one_mm_tolerance = 0.001; // One milimeter tolerance.
const double PIGeoPosition::one_um_tolerance = 0.000001; // One micron tolerance.
double PIGeoPosition::position_tolerance = PIGeoPosition::one_mm_tolerance; // Default tolerance in meters.


PIGeoPosition::PIGeoPosition() {
	initialize(PIMathVectorT3d());
}


PIGeoPosition::PIGeoPosition(double a, double b, double c, PIGeoPosition::CoordinateSystem s, PIEllipsoidModel ell) {
	PIMathVectorT3d v;
	v[0] = a;
	v[1] = b;
	v[2] = c;
	initialize(v, s, ell);
}


PIGeoPosition::PIGeoPosition(PIMathVectorT3d v, PIGeoPosition::CoordinateSystem s, PIEllipsoidModel ell) {
	initialize(v, s, ell);
}


PIGeoPosition &PIGeoPosition::transformTo(PIGeoPosition::CoordinateSystem sys) {
	if(sys == Unknown || sys == s) return *this;
	PIGeoPosition tmp(*this);
	switch(s) {
		case Unknown:
		return *this;
		case Geodetic:
			switch(sys) {
				case Unknown: case Geodetic: return *this;
				case Geocentric:
					convertGeodeticToGeocentric(*this, tmp, el);
					tmp.s = Geocentric;
				break;
				case Cartesian:
					convertGeodeticToCartesian(*this, tmp, el);
					tmp.s = Cartesian;
				break;
				case Spherical:
					convertGeodeticToGeocentric(*this, tmp, el);
					tmp[0] = 90 - tmp[0];   // geocen -> sph
					tmp.s = Spherical;
				break;
			}
		break;
		case Geocentric:
			switch(sys) {
				case Unknown: case Geocentric: return *this;
				case Geodetic:
					convertGeocentricToGeodetic(*this, tmp, el);
					tmp.s = Geodetic;
				break;
				case Cartesian:
					convertGeocentricToCartesian(*this, tmp);
					tmp.s = Cartesian;
				break;
				case Spherical:
					tmp[0] = 90 - tmp[0];   // geocen -> sph
					tmp.s = Spherical;
				break;
			}
		break;
		case Cartesian:
			switch(sys) {
				case Unknown: case Cartesian: return *this;
				case Geodetic:
					convertCartesianToGeodetic(*this, tmp, el);
					tmp.s = Geodetic;
				break;
				case Geocentric:
					convertCartesianToGeocentric(*this, tmp);
					tmp.s = Geocentric;
				break;
				case Spherical:
					convertCartesianToSpherical(*this, tmp);
					tmp.s = Spherical;
				break;
			}
		break;
		case Spherical:
			switch(sys) {
				case Unknown: case Spherical: return *this;
				case Geodetic:
					(*this)[0] = 90 - (*this)[0];   // sph -> geocen
					convertGeocentricToGeodetic(*this, tmp, el);
					tmp.s = Geodetic;
				break;
				case Geocentric:
					tmp[0] = 90 - tmp[0];   // sph -> geocen
					tmp.s = Geocentric;
				break;
				case Cartesian:
					convertSphericalToCartesian(*this, tmp);
					tmp.s = Cartesian;
				break;
			}
		break;
	}
	*this = tmp;
	return *this;
}

double PIGeoPosition::x() const {
	if(s == Cartesian) return (*this)[0];
	PIGeoPosition t(*this);
	t.transformTo(Cartesian);
	return t[0];
}


double PIGeoPosition::y() const {
	if(s == Cartesian) return (*this)[1];
	PIGeoPosition t(*this);
	t.transformTo(Cartesian);
	return t[1];
}


double PIGeoPosition::z() const {
	if(s == Cartesian) return (*this)[2];
	PIGeoPosition t(*this);
	t.transformTo(Cartesian);
	return t[2];
}


double PIGeoPosition::latitudeGeodetic() const {
	if(s == Geodetic) return (*this)[0];
	PIGeoPosition t(*this);
	t.transformTo(Geodetic);
	return t[0];
}


double PIGeoPosition::latitudeGeocentric() const {
	if(s == Geocentric) return (*this)[0];
	PIGeoPosition t(*this);
	t.transformTo(Geocentric);
	return t[0];
}


double PIGeoPosition::longitude() const {
	if(s != Cartesian) return (*this)[1];
	PIGeoPosition t(*this);
	t.transformTo(Spherical);
	return t[1];
}


double PIGeoPosition::theta() const {
	if(s == Spherical) return (*this)[0];
	PIGeoPosition t(*this);
	t.transformTo(Spherical);
	return t[0];
}


double PIGeoPosition::phi() const {
	return longitude();
}


double PIGeoPosition::radius() const {
	if(s == Spherical || s == Geocentric) return (*this)[2];
	PIGeoPosition t(*this);
	t.transformTo(Spherical);
	return t[2];
}


double PIGeoPosition::height() const {
	if(s == Geodetic) return (*this)[2];
	PIGeoPosition t(*this);
	t.transformTo(Geodetic);
	return t[2];
}


PIGeoPosition &PIGeoPosition::setGeodetic(double lat, double lon, double ht, PIEllipsoidModel ell) {
	if(lat > 90 || lat < -90) {
		piCout << "[PIGeoPosition]" << "Achtung! Invalid latitude in setGeodetic:" << lat;
		assert(lat <= 90 && lat >= -90);
	}
	(*this)[0] = lat;
	(*this)[1] = lon;
	if((*this)[1] < 0) (*this)[1] += 360 * (1 + (unsigned long)((*this)[1]/360));
	else if((*this)[1] >= 360) (*this)[1] -= 360 * (unsigned long)((*this)[1]/360);
	(*this)[2] = ht;
	el = ell;
	s = Geodetic;
	return *this;
}


PIGeoPosition &PIGeoPosition::setGeocentric(double lat, double lon, double rad) {
	if(lat > 90 || lat < -90) {
		piCout << "[PIGeoPosition]" << "Achtung! Invalid latitude in setGeocentric:" << lat;
		assert(lat <= 90 && lat >= -90);
	}
	if(rad < 0) {
		piCout << "[PIGeoPosition]" << "Achtung! Invalid radius in setGeocentric:" << rad;
		assert(rad >= 0);
	}
	(*this)[0] = lat;
	(*this)[1] = lon;
	(*this)[2] = rad;
	if((*this)[1] < 0) (*this)[1] += 360*(1+(unsigned long)((*this)[1]/360));
	else if((*this)[1] >= 360) (*this)[1] -= 360*(unsigned long)((*this)[1]/360);
	s = Geocentric;
	return *this;
}


PIGeoPosition &PIGeoPosition::setSpherical(double theta, double phi, double rad) {
	if(theta < 0 || theta > 180) {
		piCout << "[PIGeoPosition]" << "Achtung! Invalid theta in setSpherical:" << theta;
		assert(theta <= 180 && theta >= 0);
	}
	if(rad < 0) {
		piCout << "[PIGeoPosition]" << "Achtung! Invalid radius in setSpherical:" << rad;
		assert(rad >= 0);
	}
	(*this)[0] = theta;
	(*this)[1] = phi;
	(*this)[2] = rad;
	if((*this)[1] < 0) (*this)[1] += 360*(1+(unsigned long)((*this)[1]/360));
	else if((*this)[1] >= 360) (*this)[1] -= 360*(unsigned long)((*this)[1]/360);
	s = Spherical;
	return *this;
}


PIGeoPosition &PIGeoPosition::setECEF(double x, double y, double z) {
	(*this)[0] = x;
	(*this)[1] = y;
	(*this)[2] = z;
	s = Cartesian;
	return *this;
}


void PIGeoPosition::convertSphericalToCartesian(const PIMathVectorT3d &tpr, PIMathVectorT3d &xyz) {
	double st = sin(tpr[0] * deg2rad);
	xyz[0] = tpr[2] * st * cos(tpr[1] * deg2rad);
	xyz[1] = tpr[2] * st * sin(tpr[1] * deg2rad);
	xyz[2] = tpr[2] * cos(tpr[0] * deg2rad);
}


void PIGeoPosition::convertCartesianToSpherical(const PIMathVectorT3d &xyz, PIMathVectorT3d &tpr) {
	tpr[2] = xyz.length();
	if(tpr[2] <= PIGeoPosition::position_tolerance / 5) { // zero-length Cartesian vector
		tpr[0] = 90.0;
		tpr[1] = 0.0;
		return;
	}
	tpr[0] = acos(xyz[2] / tpr[2]) * rad2deg;
	if(sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1]) < PIGeoPosition::position_tolerance / 5) { // pole
		tpr[1] = 0.0;
		return;
	}
	tpr[1] = atan2(xyz[1],xyz[0]) * rad2deg;
	if(tpr[1] < 0.0) tpr[1] += 360.0;
}


void PIGeoPosition::convertCartesianToGeodetic(const PIMathVectorT3d &xyz, PIMathVectorT3d &llh, PIEllipsoidModel ell) {
	double p,slat,nn,htold,latold;
	p = sqrt(xyz[0] * xyz[0] + xyz[1] * xyz[1]);
	if(p < PIGeoPosition::position_tolerance / 5) { // pole or origin
		llh[0] = (xyz[2] > 0.0 ? 90.0: -90.0);
		llh[1] = 0.0; // lon undefined, really
		llh[2] = piAbsd(xyz[2]) - ell.a * sqrt(1.0-ell.eccSquared());
		return;
	}
	llh[0] = atan2(xyz[2], p*(1.0-ell.eccSquared()));
	llh[2] = 0;
	for(int i=0; i<5; i++) {
		slat = sin(llh[0]);
		nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
		htold = llh[2];
		llh[2] = p / cos(llh[0]) - nn;
		latold = llh[0];
		llh[0] = atan2(xyz[2], p*(1.0 - ell.eccSquared() * (nn / (nn + llh[2]))));
		if(piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
	}
	llh[1] = atan2(xyz[1], xyz[0]);
	if(llh[1] < 0.0) llh[1] += M_2PI;
	llh[0] *= rad2deg;
	llh[1] *= rad2deg;
}


void PIGeoPosition::convertGeodeticToCartesian(const PIMathVectorT3d &llh, PIMathVectorT3d &xyz, PIEllipsoidModel ell) {
	double slat = sin(llh[0] * deg2rad);
	double clat = cos(llh[0] * deg2rad);
	double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
	xyz[0] = (nn + llh[2]) * clat * cos(llh[1] * deg2rad);
	xyz[1] = (nn + llh[2]) * clat * sin(llh[1] * deg2rad);
	xyz[2] = (nn * (1.0 - ell.eccSquared()) + llh[2]) * slat;
}


void PIGeoPosition::convertCartesianToGeocentric(const PIMathVectorT3d &xyz, PIMathVectorT3d &llr) {
	convertCartesianToSpherical(xyz, llr);
	llr[0] = 90.0 - llr[0]; // convert theta to latitude
}


void PIGeoPosition::convertGeocentricToCartesian(const PIMathVectorT3d &llr, PIMathVectorT3d &xyz) {
	PIMathVectorT3d llh(llr);
	llh[0] = 90.0 - llh[0]; // convert latitude to theta
	convertSphericalToCartesian(llh, xyz);
}


void PIGeoPosition::convertGeocentricToGeodetic(const PIMathVectorT3d &llr, PIMathVectorT3d &llh, PIEllipsoidModel ell) {
	double cl, p, sl, slat, nn, htold, latold;
	llh[1] = llr[1]; // longitude is unchanged
	cl = sin((90.0 - llr[0]) * deg2rad);
	sl = cos((90.0 - llr[0]) * deg2rad);
	if(llr[2] <= PIGeoPosition::position_tolerance / 5) { // radius is below tolerance, hence assign zero-length, arbitrarily set latitude = longitude = 0
		llh[0] = llh[1] = 0.0;
		llh[2] = -ell.a;
		return;
	} else if(cl < 1.e-10) { // near pole ... note that 1mm/radius(Earth) = 1.5e-10
		if(llr[0] < 0.0) llh[0] = -90.0;
		else llh[0] =  90.0;
		llh[1] = 0.0;
		llh[2] = llr[2] - ell.a * sqrt(1.0 - ell.eccSquared());
		return;
	}
	llh[0] = atan2(sl, cl * (1.0 - ell.eccSquared()));
	p = cl * llr[2];
	llh[2] = 0.0;
	for(int i=0; i<5; i++) {
		slat = sin(llh[0]);
		nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
		htold = llh[2];
		llh[2] = p / cos(llh[0]) - nn;
		latold = llh[0];
		llh[0] = atan2(sl, cl * (1.0 - ell.eccSquared() * (nn / (nn + llh[2]))));
		if(piAbsd(llh[0] - latold) < 1.0e-9 && piAbsd(llh[2] - htold) < 1.0e-9 * ell.a) break;
	}
	llh[0] *= rad2deg;
}


void PIGeoPosition::convertGeodeticToGeocentric(const PIMathVectorT3d &llh, PIMathVectorT3d &llr, PIEllipsoidModel ell) {
	double slat = sin(llh[0] * deg2rad);
	double nn = ell.a / sqrt(1.0 - ell.eccSquared() * slat * slat);
	llr[1] = llh[1]; // longitude is unchanged
	llr[2] = sqrt((nn+llh[2])*(nn+llh[2]) + nn*ell.eccSquared()*(nn*ell.eccSquared()-2*(nn+llh[2]))*slat*slat); // radius
	if(llr[2] <= PIGeoPosition::position_tolerance/5) { // radius is below tolerance, hence assign zero-length
		llr[0] = llr[1] = llr[2] = 0; // arbitrarily set latitude = longitude = 0
		return;
	}
	if(1 - piAbsd(slat) < 1.e-10) { // at the pole
		if(slat < 0) llr[0] = -90.0;
		else llr[0] =  90.0;
		llr[1] = 0.0;
		return;
	}
	llr[0] = acos((nn * (1.0 - ell.eccSquared()) + llh[2]) * slat / llr[2]); // theta
	llr[0] *= rad2deg;
	llr[0] = 90.0 - llr[0];
}


double PIGeoPosition::radiusEarth(double geolat, PIEllipsoidModel ell) {
	double slat = sin(geolat * deg2rad);
	double e = (1.0 - ell.eccSquared());
	double f = (1.0 + (e * e - 1.0) * slat * slat) / (1.0 - ell.eccSquared() * slat * slat);
	return (ell.a * sqrt(f));
}


//PIGeoPosition &PIGeoPosition::operator=(const PIGeoPosition &v) {
//	*((PIMathVectorT3d*)(this)) = *((PIMathVectorT3d*)&v);
//	return *this;
//}


PIGeoPosition &PIGeoPosition::operator=(const PIMathVectorT3d &v) {
	*((PIMathVectorT3d*)(this)) = v;
	return *this;
}


PIGeoPosition &PIGeoPosition::operator-=(const PIGeoPosition &right) {
	PIGeoPosition r(right);
	CoordinateSystem saves = s;
	transformTo(Cartesian);
	r.transformTo(Cartesian);
	(*(PIMathVectorT3d*)(this)) -= r;
	transformTo(saves);
	return *this;
}


PIGeoPosition &PIGeoPosition::operator+=(const PIGeoPosition &right) {
	PIGeoPosition r(right);
	CoordinateSystem saves = s;
	transformTo(Cartesian);
	r.transformTo(Cartesian);
	(*(PIMathVectorT3d*)(this)) += r;
	transformTo(saves);
	return *this;
}


bool PIGeoPosition::operator==(const PIGeoPosition &right) const {
	if(el.a != right.el.a || el.eccSquared() != right.el.eccSquared()) return false;
	if(range(*this, right) < position_tolerance) return true;
	else return false;
}


void PIGeoPosition::initialize(PIMathVectorT3d v, PIGeoPosition::CoordinateSystem sys, PIEllipsoidModel ell) {
	double a(v[0]), b(v[1]), c(v[2]);
	if(sys == Geodetic || sys==Geocentric) {
		if(a > 90 || a < -90) {
			piCout << "[PIGeoPosition]" << "Achtung! Invalid latitude in constructor:" << a;
			assert(a <= 90 && a >= -90);
		}
		if(b < 0) b += 360*(1+(unsigned long)(b/360));
		else if(b >= 360) b -= 360*(unsigned long)(b/360);
	}
	if(sys==Geocentric || sys==Spherical) {
		if(c < 0) {
			piCout << "[PIGeoPosition]" << "Achtung! Invalid radius in constructor:" << c;
			assert(c >= 0);
		}
	}
	if(sys==Spherical) {
		if(a < 0 || a > 180) {
			piCout << "[PIGeoPosition]" << "Achtung! Invalid theta in constructor:" << a;
			assert(a >= 0 && a <= 180);
		}
		if(b < 0) b += 360*(1+(unsigned long)(b/360));
		else if(b >= 360) b -= 360*(unsigned long)(b/360);
	}
	(*this)[0] = a;
	(*this)[1] = b;
	(*this)[2] = c;
	el = ell;
	s = sys;
}


double PIGeoPosition::range(const PIGeoPosition &a, const PIGeoPosition &b) {
	PIGeoPosition l(a),r(b);
	l.transformTo(PIGeoPosition::Cartesian);
	r.transformTo(PIGeoPosition::Cartesian);
	return (l - r).length();
}


double PIGeoPosition::elevation(const PIGeoPosition &p) const {
	PIGeoPosition r(*this), s(p);
	r.transformTo(Cartesian);
	s.transformTo(Cartesian);
	return r.angleElevation(s);
}


double PIGeoPosition::elevationGeodetic(const PIGeoPosition &p) const {
	PIGeoPosition r(*this), s(p);
	double lat = r.latitudeGeodetic() * deg2rad;
	double lng = r.longitude() * deg2rad;
	double local_up;
	double cos_up;
	r.transformTo(Cartesian);
	s.transformTo(Cartesian);
	PIMathVectorT3d z = s - r;
	if (z.length() <= 1e-4) { // if the positions are within .1 millimeter
		piCout << "[PIGeoPosition]" << "Positions are within .1 millimeter" << z;
		assert(z.length() > 1e-4);
	}
	PIMathVectorT3d kv; // Compute k vector in local North-East-Up (NEU) system
	kv[0] = cos(lat) * cos(lng);
	kv[1] = cos(lat) * sin(lng);
	kv[2] = sin(lat);
	local_up = z.dot(kv); // Take advantage of dot method to get Up coordinate in local NEU system
	cos_up = local_up / z.length(); // Let's get cos(z), being z the angle with respect to local vertical (Up);
	return 90.0 - ((acos(cos_up)) * rad2deg);
}


double PIGeoPosition::azimuth(const PIGeoPosition &p) const {
	PIGeoPosition r(*this), s(p);
	r.transformTo(Cartesian);
	s.transformTo(Cartesian);
	double xy, xyz, cosl, sinl, sint, xn1, xn2, xn3, xe1, xe2;
	double z1, z2, z3, p1, p2, test, alpha;
	xy = r[0] * r[0] + r[1] * r[1];
	xyz = xy + r[2] * r[2];
	xy = sqrt(xy);
	xyz = sqrt(xyz);
	if (xy <= 1e-14 || xyz <=1e-14) {
		piCout << "[PIGeoPosition]" << "Divide by Zero Error";
		assert(xy > 1e-14 && xyz > 1e-14);
	}
	cosl = r[0] / xy;
	sinl = r[1] / xy;
	sint = r[2] / xyz;
	xn1 = -sint * cosl;
	xn2 = -sint * sinl;
	xn3 = xy / xyz;
	xe1 = -sinl;
	xe2 = cosl;
	z1 = s[0] - r[0];
	z2 = s[1] - r[1];
	z3 = s[2] - r[2];
	p1 = (xn1 * z1) + (xn2 * z2) + (xn3 * z3) ;
	p2 = (xe1 * z1) + (xe2 * z2) ;
	test = piAbsd(p1) + piAbsd(p2);
	if (test < 1.0e-16) {
		piCout << "[PIGeoPosition]" << "azAngle(), failed p1+p2 test" << test;
		assert(test >= 1.0e-16);
	}
	alpha = 90 - atan2(p1, p2) * rad2deg;
	if (alpha < 0) return alpha + 360;
	else  return alpha;
}


double PIGeoPosition::azimuthGeodetic(const PIGeoPosition &p) const {
	PIGeoPosition r(*this), s(p);
	double lat = r.latitudeGeodetic() * deg2rad;
	double lng = r.longitude() * deg2rad;
	r.transformTo(Cartesian);
	s.transformTo(Cartesian);
	PIMathVectorT3d z;
	z = s - r;
	if (z.length() <= 1e-4) { // if the positions are within .1 millimeter
		piCout << "[PIGeoPosition]" << "Positions are within 0.1 millimeter" << z.length();
		assert(z.length() > 1e-4);
	}
	PIMathVectorT3d iv; // Compute i vector in local North-East-Up (NEU) system
	iv[0] = -sin(lat) * cos(lng);
	iv[1] = -sin(lat) * sin(lng);
	iv[2] = cos(lat);
	PIMathVectorT3d jv; // Compute j vector in local North-East-Up (NEU) system
	jv[0] = -sin(lng);
	jv[1] = cos(lng);
	jv[2] = 0.0;
	double local_n = z.dot(iv) / z.length(); // Now, let's use dot product to get localN unitary vectors
	double local_e = z.dot(jv) / z.length(); // Now, let's use dot product to get localE unitary vector
	double test = piAbsd(local_n) + piAbsd(local_e); // Let's test if computing azimuth has any sense
	if (test < 1.0e-16) return 0.0; // Warning: If elevation is very close to 90 degrees, we will return azimuth = 0.0
	double alpha = atan2(local_e, local_n) * rad2deg;
	if (alpha < 0.0) return alpha + 360.0;
	else return alpha;
}

double PIGeoPosition::getCurvMeridian() const {
	double slat = sin(latitudeGeodetic() * deg2rad);
	double w = 1.0 / sqrt(1.0 - el.eccSquared() * slat * slat);
	return el.a * (1.0 - el.eccSquared()) * w * w * w;
}


double PIGeoPosition::getCurvPrimeVertical() const {
	double slat = sin(latitudeGeodetic() * deg2rad);
	return el.a / sqrt(1.0 - el.eccSquared() * slat * slat);
}