/*

ARIA header files for use with ARNL 1.7.1

Copyright(C) 2004, 2005 ActivMedia Robotics, LLC. 
Copyright(C) 2006, 2007, 2008, 2009 MobileRobots Inc.
All rights reserved.

This copy of Aria was relicensed for use with Arnl and the Arnl
license by MobileRobots Inc.  If you wish to download a seperate
distribution of Aria licensed under the GPL or a commercial license go to
http://www.mobilerobots.com/SOFTWARE/aria.html or contact MobileRobots
Inc, at robots@mobilerobots.com or MobileRobots Inc,
10 Columbia Drive, Amherst, NH 03031; 800-639-9481

MobileRobots Inc hereby grants to other individuals or
organizations permission to use this software with Arnl and in
compliance with the Arnl license.  This software may not be
distributed to others except by MobileRobots Inc.

MobileRobots Inc does not make any representations about the
suitability of this software for any purpose.  It is provided "as is"
without express or implied warranty.

*/
#ifndef ARGPSCOORDS_H
#define ARGPSCOORDS_H

#include "ariaTypedefs.h"



class Ar3DPoint;
class ArLLACoords;
class ArECEFCoords;
class ArENUCoords;
class ArWGS84;

/** Base class for points in 3 dimensional cartesian space. */
class Ar3DPoint 
{
  public:

  Ar3DPoint(void) : myX(0), myY(0), myZ(0) {}
  Ar3DPoint(double x, double y, double z) : myX(x), myY(y), myZ(z) {}
  /// Destructor.
  ~Ar3DPoint() {}
  /// Add
  Ar3DPoint operator+(Ar3DPoint c)
  {
    Ar3DPoint sum(myX + c.myX, 
		  myY + c.myY,
		  myZ + c.myZ);
    
    return sum;
  }
  /// Diff
  Ar3DPoint operator-(Ar3DPoint c)
  {
    Ar3DPoint dif(myX - c.myX, 
		  myY - c.myY,
		  myZ - c.myZ);
    return dif;
  }
  /// Diff
  Ar3DPoint operator*(double c)
  {
    Ar3DPoint pro(myX*c, myY*c, myZ*c);
    return pro;
  }
  /// Dot product
  double dot(Ar3DPoint c)
  {
    double dotP(myX * c.myX + myY * c.myY + myZ * c.myZ);
    return dotP;
  }
  /// Cross product
  Ar3DPoint cross(Ar3DPoint c)
  {
    Ar3DPoint crossP(myY * c.myZ - myZ * c.myY, 
		     myZ * c.myX - myX * c.myZ, 
		     myX * c.myY - myY * c.myX);
    return crossP;
  }
  /// Print.
  /** @swignote Use 'printPoint' instead */
  AREXPORT void print(const char* head=NULL);

  double getX() const {return myX;}
  double getY() const {return myY;}
  double getZ() const {return myZ;}

protected:

  double myX;
  double myY;
  double myZ;

};

#ifdef WIN32
// Need to export some variables on Windows because they are used in inline methods (which is good), but they can't be exported if const.
#define ARGPSCOORDS_CONSTANT 
#else
#define ARGPSCOORDS_CONSTANT const
#endif


/*!
 * All the constants defined by the World Geodetic System 1984.
 */
class ArWGS84
{
  public:
  ArWGS84(void) {}
  
  static double getE()     {return mye;}
  static double getA()     {return mya;}
  static double getB()     {return myb;}
  static double getEP()    {return myep;}
  static double get1byf()  {return my1byf;}
  static double getOmega() {return myOmega;}
  static double getGM()    {return myGM;}
  

private:
  AREXPORT static ARGPSCOORDS_CONSTANT double mya;     // meters
  AREXPORT static ARGPSCOORDS_CONSTANT double myb;     // meters
  AREXPORT static ARGPSCOORDS_CONSTANT double myep; 
  AREXPORT static ARGPSCOORDS_CONSTANT double myc;     // m/sec
  AREXPORT static ARGPSCOORDS_CONSTANT double mye;
  AREXPORT static ARGPSCOORDS_CONSTANT double my1byf;
  AREXPORT static ARGPSCOORDS_CONSTANT double myOmega; // rad/sec
  AREXPORT static ARGPSCOORDS_CONSTANT double myGM;    // m^3/sec^2
  AREXPORT static ARGPSCOORDS_CONSTANT double myg;     // m/sec^2. Ave g.
  AREXPORT static ARGPSCOORDS_CONSTANT double myM;     // kg. Mass of earth.
};


/*!
 * Earth Centered Earth Fixed Coordinates.
 */
class ArECEFCoords : public Ar3DPoint
{
  public:
  ArECEFCoords(double x, double y, double z) : Ar3DPoint(x, y, z) {}
  AREXPORT ArLLACoords ECEF2LLA(void);
  AREXPORT ArENUCoords ECEF2ENU(ArECEFCoords ref);
};
/*!
 * Latitude, Longitude and Altitude Coordinates.
 */
class ArLLACoords : public Ar3DPoint
{
  public:
  ArLLACoords(void) : Ar3DPoint(0, 0, 0) {}
  ArLLACoords(double x, double y, double z) : Ar3DPoint(x, y, z) {}
  AREXPORT ArECEFCoords LLA2ECEF(void);
  double getLatitude(void) const {return getX();}
  double getLongitude(void) const {return getY();}
  double getAltitude(void) const {return getZ();}
};
/*!
 * East North Up coordinates.
 */
class ArENUCoords : public Ar3DPoint
{
  public:
  ArENUCoords(double x, double y, double z) : Ar3DPoint(x, y, z) {}
  AREXPORT ArECEFCoords ENU2ECEF(ArLLACoords ref);
  double getEast(void) const {return getX();}
  double getNorth(void) const {return getY();}
  double getUp(void) const {return getZ();}
};

/*!
 * Coordinates based on a map with origin in LLA coords with conversion
 * methods from LLA to ENU and from ENU to LLA coordinates.
 */
class ArMapGPSCoords : public ArENUCoords
{
  public:
  ArMapGPSCoords(ArLLACoords org) : ArENUCoords(0.0, 0.0, 0.0) 
  {
    myOriginSet = true;
    myOriginLLA = new ArLLACoords(org);
    myOriginECEF = new ArECEFCoords(myOriginLLA->LLA2ECEF());
  }
  AREXPORT bool convertMap2LLACoords(const double ea, const double no, const double up,
			    double& lat, double& lon, double& alt) const;
  AREXPORT bool convertLLA2MapCoords(const double lat, const double lon, const double alt,
			    double& ea, double& no, double& up) const;
  bool convertLLA2MapCoords(const ArLLACoords& lla, double& ea, double& no, double& up)
{
    return convertLLA2MapCoords(lla.getLatitude(), lla.getLongitude(), lla.getAltitude(), ea, no, up);
  }
     
  ArECEFCoords* myOriginECEF;
  ArLLACoords* myOriginLLA;
  bool myOriginSet;
};




#endif // ARGPSCOORDS_H