/***************************************************************************
    gmf_geometry.cpp	- methods for geometrical operations - see header
                             -------------------
    begin                : Fri Sep 26th 2003
    copyright            : (C) 2003 by Andrew O'Neill,HiRes Grad Student
    email                : oneill@phys.columbia.edu
 ***************************************************************************/
 /* 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */


#include "gmf_geometry.h"

using namespace std;

//////////////////////////////////////////////////////////////////////////////
//   COORDINATE CONVERSIONS
//////////////////////////////////////////////////////////////////////////////

void EllipToCartesian (const double& Phi, const double& Lambda, 
                       const double& H, Vector3D& XYZ)
{
	// Taken from 
	// http://www.spenvis.oma.be/spenvis/help/background/coortran/coortran.html

	double A = WGS84_A, B = WGS84_B;  // semi major / minor axes of earth
	double N = A*A / sqrt( A*A*cos(Phi)*cos(Phi) + B*B*sin(Phi)*sin(Phi) );
	double X = (N + H)*cos(Phi)*cos(Lambda);
	double Y = (N + H)*cos(Phi)*sin(Lambda);
	double Z = ( (B*B*N)/(A*A) + H)*sin(Phi);
	XYZ.setXYZ(X, Y, Z);  // set the Vector3D components

}


// Go from Cartesian to elliptical coordinates for a point XYZ
void CartesianToEllip (const Vector3D& XYZ, double& Phi, double& Lambda, 
                       double& H)
{
	// reverse of the above transformations, can only be done iteratively
	// http://titan.etf.bg.ac.yu/csidc/docs/gps_transformation.php

	// longitude is easy  tan (lambda) = y / x, then make sure we are in the
	// right quadrant  // YUCKY BUG!
	Lambda = atan2 (XYZ.Y(), XYZ.X());
	
//	dout(INFO) << "CTE: Lambda = " << Lambda << endl;
	// radius of parallel    x^2 + y^2	
	double Rho = sqrt(XYZ.X()*XYZ.X() + XYZ.Y()*XYZ.Y());
	double A = WGS84_A, B = WGS84_B;  // semi major / minor axes of earth
	double E2 = (A*A - B*B)/(B*B);      // eccentricity of earth (WGS84)
	
	// first calc of Phi   =  atan (z/(rho*(1-e^2))
	double Phi_0 = atan2 (XYZ.Z(), (Rho*(1 - E2))); 
	int Iterate = 1;
	double N = 0.0;
	while (Iterate)
	{ 
		Iterate++;
		// Calculate N(Phi)   (see EllipToCartesian)
		N = A*A / sqrt( A*A*cos(Phi_0)*cos(Phi_0) + B*B*sin(Phi_0)*sin(Phi_0) );
		
		// Calculate better value for height
		H = (Rho/cos(Phi_0)) - N;

		// Use this to get a better answer for Phi
		Phi = atan2 ( XYZ.Z(), (Rho * (1 - (E2*N)/(N + H))) );
		
//		dout(LINFO) << "Iterating Phi: " << Phi << " Phi0: "<< Phi_0 << endl;
		if (fabs(Phi - Phi_0) <= EllipIterationAccuracy)  // stop iterating?
		{
			Iterate = 0;
		}
		else if (Iterate > 100)
		{
			dout(ERR) << "Reached 100 iterations without converging in "
			          << "CartesianToEllip()... exiting iteration" << endl;
			Iterate = 0;			         
		
		}
//		Phi_0 = Phi;  // and iterate again from our best guess
	}  // end of while loop	
//	dout(INFO) << "CTE: Phi = " << Phi << endl;
		
}

// Topocentric Cartesian (East, North, Upwards) with respect to some origin
// to Geocentric cartesian (XYZ) 
// Note: Eta and Xi are local gravitational deflections at the origin (i.e. 
// Earth's lumpiness makes "up" not point radially, I think).
void TopoCartToCartesian (const double& East, const double& North, 
                          const double& Up, const Vector3D& Origin,
                          const double& Eta, const double& Xi, 
                          Vector3D& XYZ)
{
	// get lat and long of origin
	double Phi = 0.0, Lambda = 0.0, H = 0.0;
	CartesianToEllip (Origin, Phi, Lambda, H);
	
	// get corrections to vertical defelctions
	// as done in HiRes geolib.  See http://www.ngs.noaa.gov/GEOID/DEFLEC96/
	// and dst2k/src/geolib/geolib_enu2xyz.c
	Phi = asin (sin(Phi)/cos(Eta)) + Xi;
	Lambda = Lambda + asin(sin(Eta)/cos(Phi));
	dout(INFO) << "   Corrected Phi = " << Phi << " Lambda: " << Lambda << endl;
	// Now rotate  (this is the next function)
	ENUToXYZ (East, North, Up, Phi, Lambda, XYZ);
	
	// now translate to geocentric origin (from topocentric origin)
	
	XYZ += Origin;
	return;
}

// At a given Longitude/latitude with respect to the XYZ origin,
// we need to rotate East, North, Up to XYZ
void ENUToXYZ (const double& East, const double& North,
                const double& Up,
                const double& Phi, const double& Lambda,
                Vector3D& XYZ)
{
	double X = 0.0, Y = 0.0, Z = 0.0;
	// USE SOUTH for convenience
	double South = - North;

	// Rotate Phi - Pi/2.0
	double X1 = South*cos(Phi - Pi/2.0) - Up*sin(Phi-Pi/2.0);
	double Y1 = East;
	double Z1 = Up*cos(Phi - Pi/2.0) + South*sin(Phi - Pi/2.0);

	// ROTATE BY -Lambda
	X = X1*cos(-Lambda) + Y1*sin(-Lambda);
	Y = Y1*cos(-Lambda) - X1*sin(-Lambda);
	Z = Z1;

	//dout(INFO) << "   E2X: XYZ = " << X << "  " << Y << "  " << Z << endl;

	XYZ.setXYZ(X, Y, Z);
	return;
}

// XYZ to Azimuth, Zenith, Range
void CartesianToTopoPolar (const Vector3D& XYZ, const Vector3D& Origin,
                           double& Zenith, double& Azimuth, double& Range)
{
	// TRANSLATE ORIGIN
	Vector3D Difference = XYZ - Origin;
	
	// ROTATE BY LATITUDE LONGITUDE
	//   - get lat, long
	double Phi = 0.0, Lambda = 0.0, Height = 0.0;
	CartesianToEllip(Origin, Phi, Lambda, Height);
	//   - do rotation
	double East = 0.0, North = 0.0, Up = 0.0;
	XYZToENU (Difference, Phi, Lambda, East, North, Up);
	
	// CALCULATE AZIMUTH, ZENITH, RANGE
	Range = sqrt(East*East + North*North + Up*Up);
	Azimuth = atan2(North, East);
	Zenith = acos(Up/Range);
}

void CartesianToTopoCart(const Vector3D& XYZ, const Vector3D& Origin,
                         double& East, double& North, double& Up)
{
	// TRANSLATE ORIGIN
	Vector3D Difference = XYZ - Origin;
	
	// ROTATE BY LATITUDE LONGITUDE
	//   - get lat, long
	double Phi = 0.0, Lambda = 0.0, Height = 0.0; // of detector
	CartesianToEllip(Origin, Phi, Lambda, Height);
	
	// do rotation using XYZToENU
	XYZToENU(Difference, Phi, Lambda, East, North, Up);  // should do it!
}

// rotate from XYZ frame to ENU topocentric frame at Lambda. Phi
void XYZToENU (const Vector3D& XYZ, 
               const double& Phi, const double& Lambda,
               double& East, double& North, double& Up)
{
	// ROTATE BY LAMBDA (LONGITUDE)
	double X1 = XYZ.X()*cos(Lambda) + XYZ.Y()*sin(Lambda);
	double Y1 = XYZ.Y()*cos(Lambda) - XYZ.X()*sin(Lambda);
	double Z1 = XYZ.Z();

	// ROTATE BY PHI (LATITUDE)
	North = -X1*cos(Pi/2.0 - Phi) + Z1*sin(Pi/2.0-Phi);
	East = Y1;
	Up = Z1*cos(Pi/2.0 - Phi) + X1*sin(Pi/2.0 - Phi);
}

// Azimuth, Zenith, Range with respect to Origin to geocentric XYZ
void TopoPolarToCartesian (const double& Zenith, const double& Azimuth, 
                           const double& Range, const Vector3D& Origin,
                           const double& Eta, const double& Xi,
                           Vector3D& XYZ)
{
	double E = Range*sin(Zenith)*cos(Azimuth);
	double N = Range*sin(Zenith)*sin(Azimuth);
	double U = Range*cos(Zenith);

	TopoCartToCartesian (E, N, U, Origin, Eta, Xi, XYZ);
}


///////////////////////////////////////////////////////////////////////////
//                 OUTPUT
///////////////////////////////////////////////////////////////////////////