/***************************************************************************
 *            aon_vector.cpp - 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.
 */
 
 
#include "aon_vector.h"

// CONSTRUCTORS ETC

Vector3D::Vector3D(const double x, const double y, const double z)
	: itsX(x), itsY(y), itsZ(z)
{
}

Vector3D::Vector3D(const Vector3D& B)
{
	itsX = B.itsX;
	itsY = B.itsY;
	itsZ = B.itsZ;
}

Vector3D::~Vector3D()
{
}


// VECTOR ARITMETIC

Vector3D operator+ (const Vector3D& A, const Vector3D& B)
{
	
	return Vector3D (A.X() + B.X(), A.Y() + B.Y(), A.Z()+B.Z());
}


Vector3D operator- (const Vector3D& A, const Vector3D& B)
{
	return Vector3D (A.X() - B.X(), A.Y() - B.Y(), A.Z()-B.Z());
}

Vector3D operator* (const double& s, const Vector3D& A)
{
	return Vector3D (s*A.X(), s*A.Y(), s*A.Z());
}

Vector3D operator* (const Vector3D& A, const double& s)
{
	return Vector3D (s*A.X(), s*A.Y(), s*A.Z());
}

                                                              
// Overload "<<" to output a 3D Vector 

ostream& operator<< (ostream& OutStream, const Vector3D& OutVec)
{
   OutStream << "(" << OutVec.X() << ", " << OutVec.Y() << ", "
             << OutVec.Z() << ")" ;
   return OutStream;
}


////////////////////////////////////////////////////////////////////////////
// NON-INLINED FUNCTIONS (INLINING CAUSED PROBLEMS WITH GCC 3.3.2 on FC1)
// and when I say "problems" I mean seg-faults in the compiler!
////////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////
//            AZIMUTH, ZENITH, RANGE
////////////////////////////////////////////////////////////////////////////
double Vector3D::Azimuth(const Vector3D& Origin) const
{
   double localEast = East(Origin);
   double localNorth = North(Origin);
   //dout(INFO) << "East: " << localEast << "  North: " << localNorth << endl;

   return atan2(localNorth, localEast);
}

double Vector3D::Zenith(const Vector3D& Origin) const
{
   double localEast = East(Origin);
   double localNorth = North(Origin);
   double localUp = Up(Origin);

   // cos (Zenith) = up/range
   return acos( localUp/
                (sqrt(localEast*localEast + localNorth*localNorth +
                      localUp*localUp)) );
}


double Vector3D::Range(const Vector3D& Origin) const
{
   double localEast = East(Origin);
   double localNorth = North(Origin);
   double localUp = Up(Origin);

   return sqrt(localEast*localEast + localNorth*localNorth + localUp*localUp);   
}

////////////////////////////////////////////////////////////////////////////
//            EAST, NORTH, UP "TOPOCENTRIC CARTESIAN" accessors
////////////////////////////////////////////////////////////////////////////
double Vector3D::East(const Vector3D& Origin) const
{
   // TRANSLATE ORIGIN to detector
   Vector3D Difference = *this - Origin;

   // ROTATE by longitude and latitude
   return Difference.East(Origin.Long(), Origin.Lat());

}

double Vector3D::North(const Vector3D& Origin) const
{
   // TRANSLATE ORIGIN to detector
   Vector3D Difference = *this - Origin;

   // ROTATE by longitude and latitude
   return Difference.North(Origin.Long(), Origin.Lat());
}

double Vector3D::Up(const Vector3D& Origin) const
{
   // TRANSLATE ORIGIN to detector
   Vector3D Difference = *this - Origin;

   // ROTATE by longitude and latitude
   return Difference.Up(Origin.Long(), Origin.Lat());
}


double Vector3D::East(const double& Longitude,
                             const double& Latitude) const
{
   // ROTATE BY LONGITUDE
   return itsY*cos(Longitude) - itsX*sin(Longitude);
}

double Vector3D::North(const double& Longitude,
                              const double& Latitude) const
{
   // ROTATE BY LONGITUDE
   double X1 = itsX*cos(Longitude) + itsY*sin(Longitude);

   // ROTATE BY LATITUDE
   return -X1*cos(Pi/2.0 - Latitude) + itsZ*sin(Pi/2.0-Latitude);
}


double Vector3D::Up(const double& Longitude, const double& Latitude) const
{
   // ROTATE BY LONGITUDE
   double X1 = itsX*cos(Longitude) + itsY*sin(Longitude);

   // ROTATE BY LATITUDE   
   return itsZ*cos(Pi/2.0 - Latitude) + X1*sin(Pi/2.0 - Latitude);
}





////////////////////////////////////////////////////////////////////////////
//        THETA, PHI 
////////////////////////////////////////////////////////////////////////////
double Vector3D::Theta() const
{
   if(itsX == 0.0 && itsY == 0.0 && itsZ == 0.0)
   {
      return 0.0;
   }
   else
   {
      double Rho = sqrt(itsX*itsX + itsY*itsY);
      return atan2(Rho, itsZ);
   }
}


double Vector3D::Phi() const
{
   if (itsX == 0.0 && itsY == 0.0)
   {
      return 0.0;
   }
   else
   {
      return atan2(itsY, itsX);
   }
}


////////////////////////////////////////////////////////////////////////////
//                    LATITUDE, LONGITUDE, HEIGHT
////////////////////////////////////////////////////////////////////////////

// Latitude
double Vector3D::Lat() const
{
   // reverse of the Ellipsoid 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  !was a bug!  use atan2 to avoid that
   // double Lambda = Long();

   // dout(INFO) << "CTE: Lambda = " << Lambda << endl;
   // radius of parallel    x^2 + y^2  
   double Rho = sqrt(itsX*itsX + itsY*itsY);
   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 N = 0.0, H = 0.0, Phi = 0.0;
   double Phi_0 = atan2 (itsZ, (Rho*(1 - E2)));
   int Iterate = 1;
   while (Iterate)
   {
      Iterate++;
      // Calculate N(Phi) (see reference above)
      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 ( itsZ, (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 > 1000)
      {
         dout(ERR) << "Reached 100 iterations without converging in "
                   << "Vector3D::Phi()... exiting iteration" << endl;
         Iterate = 0;
      }
      // iterate again starting with our new best guess
      Phi_0 = Phi;
   }
   return Phi;
}



// Longitude
double Vector3D::Long() const
{
   // longitude is easy  tan (lambda) = y / x, then make sure we are in the
   // right quadrant  !was a bug!  use atan2 to avoid that

   return atan2 (itsY, itsX);
}



// Height above ellipsoid
double Vector3D::H() const
{
   // http://titan.etf.bg.ac.yu/csidc/docs/gps_transformation.php

   // NOT BEING MOST EFFICIENT IF WE NEED H AND LATITUDE TOGETHER -
   // BUT I DON'T DO THAT SO I DON'T CARE!
   double Rho = sqrt (itsX*itsX + itsY*itsY);
   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)


   double Phi = Lat();
   // Calculate N(Phi) (see reference above)
   double N = A*A / sqrt( A*A*cos(Phi)*cos(Phi) + B*B*sin(Phi)*sin(Phi) );
   // Calculate better value for height
   return (Rho/cos(Phi)) - N;

}



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

double Vector3D::Transverse(const Vector3D& B) const
{
   // transverse component of this with respect to the vector B
   // simple - A_transverse^2 = A^2 - [A*cos(Theta_AB)]^2
   //                       = A^2 - [(A.B) / B ]
   // FIXME - I think this function is causing a bizarre segfault sometimes
   double BSqrd = B.R2();
   if(BSqrd == 0.0)
   {
      dout(WARN) << "Taking transverse of null vector" << endl;
      return 0.0; // undefined and silly to take the transverse of a null vector
   }
   else
   {
      double AdotB = Dot(B);
      return  sqrt (R2() - (AdotB*AdotB/BSqrd));
   }
}




Vector3D Vector3D::Unit() const
{
   double Mag = R();
   Vector3D Unit(itsX/Mag, itsY/Mag, itsZ/Mag);
   return Unit;
}