/***************************************************************************
 *            aon_vector.h - 3d vector class
 *
 *  Begun:     Fri May 6 2004
 *  Copyright  2004  Andrew C. O'Neill
 *  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 Library 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.
 */
 
#ifndef AON_VECTOR_H
#define AON_VECTOR_H



#include "aon_debug.h"
#include "aon_physconstants.h"
#include <cmath>

using namespace std;

// These are the semi-major axis, flatenning and eccentricity of the 
// ellipsoid model of the earth.
const double WGS84_A = 6378137.0;
const double WGS84_B = 6356752.3;


const double EllipIterationAccuracy = 1e-6;	// In radians
															// The accuracy to which we
															// will calculate the latitude

/* =========================================================================
*  Class: Vector3D
*	Should probably inherit from abstract vector class but MEH! I can't see the
*	point at the moment.
*  FIXME:   should move the geometry transformations to this framework at some
*           point

* ========================================================================== */
class Vector3D
{
public:
	// CONSTRUCTORS
	Vector3D(const double x = 0.0, const double y = 0.0, const double z = 0.0);
	Vector3D(const Vector3D & B);
	inline Vector3D& operator=(const Vector3D& B);
	
	~Vector3D();
	
	// SET FUNCTIONS
	void setX(const double& x) {itsX = x; }
	void setY(const double& y) {itsY = y; }
	void setZ(const double& z) {itsZ = z; }

	// SET IN DIFFERENT COORDINATE SYSTEMS
	// "GEOCENTRIC XYZ" - as above
	inline void setXYZ(const double& x, const double& y, const double& z);
	
 	// "GEOCENTRIC SPHERICAL" - as above but polar
	inline void setRThetaPhi(const double& R, const double& Theta, 
	                         const double& Phi);
	// "LOCAL CARTESIAN" - East, North, Up (ENU)
	inline void setENU(const double& East, const double& North, 
                      const double& Up, const double& Latitude, 
                      const double& Longitude);
	// "ELLIPTICAL" - Latitude, longitude, Height above ellipsoid
	inline void setEllipsoidal(const double& Latitude, const double& Longitude,
                              const double& Height);
	// "TOPOCENTRIC CARTESIAN" East, North, Up wrt to some origin (detector)
	inline void setTopocentricCartesian(const double& East, const double& North,
	                                    const double& Up, const Vector3D Origin,
	                                    const double& Eta, const double& Xi);
	// "TOPOCENTRIC POLAR" Range, Azimuth, Zenith wrt some origin (detector)
	inline void setTopocentricPolar(const double& Zenith, const double& Azimuth,
                                   const double& Range, const Vector3D& Origin,
                                   const double& Eta, const double& Xi);
	
	// ACCESSORS
	double X() const {return itsX; }
	double Y() const {return itsY; }
 	double Z() const {return itsZ; }
  	
   double Theta() const;
   double Phi() const;
	inline double R() const;
  	inline double R2() const; // R^2
 	
 	// "LOCAL CARTESIAN" ENU accessors -> see overloaded local versions below too
 	double East(const double& Longitude, const double& Latitude) const;
 	double North(const double& Longitude, const double& Latitude) const;
 	double Up(const double& Longitude, const double& Latitude) const;
 	
 	// "ELLIPTICAL" accessors
 	double Lat() const;  // Latitude
 	double Long() const; // Longitude
 	double H() const;    // Height above ellipsoid
 	
 	// "TOPOCENTRIC CARTESIAN" accessors
 	double East(const Vector3D& Origin) const;
 	double North(const Vector3D& Origin) const; 	
 	double Up(const Vector3D& Origin) const;
 	
 	// "TOPOCENTRIC POLAR" accessors
 	double Range(const Vector3D& Origin) const;
	double Azimuth(const Vector3D& Origin) const;
 	double Zenith(const Vector3D& Origin) const;
 	


 	// OTHER USEFUL OPERATIONS
 	double Transverse(const Vector3D& B) const; 
   Vector3D Unit() const;
   	
   // NOT IMPLEMENTED YET
   /*
   bool operator== (const Vector3D& V);
   bool operator!= (const Vector3D& V);
   Vector3D Orthogonal();
   inline Vector3D Cross(const Vector3D& B);
   
   */
   
   // VECTOR ARITHMETIC
   inline Vector3D& operator+= (const Vector3D& B);
   inline Vector3D& operator-= (const Vector3D& B);
   inline Vector3D& operator*= (const double& Scale);
   inline double Dot(const Vector3D& B) const;
   
   
    	
private:
	double itsX;
	double itsY;
	double itsZ;
};

// MORE OPERATORS
Vector3D operator+ (const Vector3D& A, const Vector3D& B);
Vector3D operator- (const Vector3D& A, const Vector3D& B);
Vector3D operator* (const double& s, const Vector3D& A);
Vector3D operator* (const Vector3D& A, const double& s);

// OUTPUT OVERLOADING
ostream& operator<< (ostream& OutStream, const Vector3D& OutVec);

// INLINE FUNCTIONS
inline Vector3D::Vector3D& Vector3D::operator=(const Vector3D& B)
{
	itsX = B.itsX;
	itsY = B.itsY;
	itsZ = B.itsZ;
	return *this;
}


/*=========================================================================
*                        SET FUNCTIONS
==========================================================================*/

inline void Vector3D::setXYZ(const double& x, const double& y, const double& z)
{
	itsX = x;
	itsY = y;
	itsZ = z;
}

inline void Vector3D::setRThetaPhi(const double& R, const double& Theta, 
                         const double& Phi)
{
	if (R < 0)
	{
		dout(ERR) << "in setRThetaPhi R is negative - check your code" << endl;
	}
	itsX = R*sin(Theta)*cos(Phi);
	itsY = R*sin(Theta)*sin(Phi);
	itsZ = R*cos(Theta);	
}

inline void Vector3D::setENU(const double& East, const double& North,
                             const double& Up, const double& Latitude, 
                             const double& Longitude)
{
	double South = - North;

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

	// ROTATE BY -Lambda
	itsX = X1*cos(-Longitude) + Y1*sin(-Longitude);
	itsY = Y1*cos(-Longitude) - X1*sin(-Longitude);
	itsZ = Z1;

}

inline void Vector3D::setEllipsoidal(const double& Latitude, 
                                     const double& Longitude,
                                     const double& Height)
{
	// 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(Latitude)*cos(Latitude) + 
 	                       B*B*sin(Latitude)*sin(Latitude) );
	itsX = (N + Height)*cos(Latitude)*cos(Longitude);
	itsY = (N + Height)*cos(Latitude)*sin(Longitude);
	itsZ = ( B*B*N/(A*A) + Height)*sin(Latitude);

}

inline void Vector3D::setTopocentricCartesian(const double& East, const double& North,
	                                    const double& Up, const Vector3D Origin,
	                                    const double& Eta, const double& Xi)
{
	// get lat and long of origin
	double Phi = Origin.Lat(); 
 	double Lambda = Origin.Long(); 
  
	// 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;
	double South = - North;

	// Rotate by 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
	itsX = X1*cos(-Lambda) + Y1*sin(-Lambda);
	itsY = Y1*cos(-Lambda) - X1*sin(-Lambda);
	itsZ = Z1;
	
	// now translate to geocentric origin (from topocentric origin)
	*this += Origin;
	/*itsX += Origin.X();
	itsY += Origin.Y();
	itsZ += Origin.Z(); */
}

inline void Vector3D::setTopocentricPolar(const double& Zenith, 
                                const double& Azimuth, const double& Range, 
                                const Vector3D& Origin,
                                const double& Eta, const double& Xi)
{
	// FIXME - this might not be the mose efficient but I'm not using this much
	// just use the logic from setTopocentricCartesian
 	double E = Range*sin(Zenith)*cos(Azimuth);
	double N = Range*sin(Zenith)*sin(Azimuth);
	double U = Range*cos(Zenith);

	//dout(INFO) << "East: " << E << "  North: " << N  << "  Up: " << U << endl;
	setTopocentricCartesian(E, N, U, Origin, Eta, Xi);
}


/*=========================================================================
*                        ACCESSORS
==========================================================================*/


inline double Vector3D::R() const
{
	return sqrt(R2());
}

inline double Vector3D::R2() const
{
	return itsX*itsX + itsY*itsY + itsZ*itsZ;
}

                              

/*===========================================================================
*                    OTHER USEFUL FUNCTIONS
*==========================================================================*/

inline Vector3D& Vector3D::operator+= (const Vector3D& B)
{
	itsX += B.itsX;
	itsY += B.itsY;
	itsZ += B.itsZ;
	return *this;
}

inline Vector3D& Vector3D::operator-= (const Vector3D& B)
{
	itsX -= B.itsX;
	itsY -= B.itsY;
	itsZ -= B.itsZ;
	return *this;
}

inline Vector3D& Vector3D::operator*= (const double& Scale)
{
	itsX *= Scale;
	itsY *= Scale;
	itsZ *= Scale;
	return *this;
}

inline double Vector3D::Dot(const Vector3D& B) const
{
	return (itsX*(B.itsX) + itsY*(B.itsY) + itsZ*(B.itsZ));
}


#endif