
#ifndef FOUNDATION__LAT_LON_HEIGHT_H__INCLUDED__
#define FOUNDATION__LAT_LON_HEIGHT_H__INCLUDED__

#include <deque>
#include <foundation/SharedPtr.h>
#include <foundation/Math/Point.hpp>
#include <foundation/Math/Vec.hpp>
#include <foundation/Math/QuadCoord.hpp>

namespace Tn { 
    
namespace math {

double const EARTH_RADIUS_METERS = 6371010.0;
    
struct LatLonHeight
{
    /* longitude = x
     * latitude = y
     * elevation = z
     */

    static const uint32_t LONGITUDE = 0;
    static const uint32_t LATITUDE  = 1;
    static const uint32_t HEIGHT    = 2;

    //Create invalid LatLonHeight object
    LatLonHeight();
    LatLonHeight(double latitude, double longitude, double heightMeters = 0);
    
    void SetLatitude(double latitude);
    void SetLongitude(double longitude);
    void SetHeightMeters(double heightMeters);
    
    double Latitude() const;
    double Longitude() const;
    double HeightMeters() const;
    
    bool   Valid() const; // values outside permissable range
    void   Wrap(); // fix invalidity

    double operator[](uint32_t) const;

    friend bool operator==(LatLonHeight const &, LatLonHeight const &);
    
private:

    double m_positionLonLatHeight[3];
};

bool operator!=(LatLonHeight const &, LatLonHeight const &);


Vec3d latLonToNormal(LatLonHeight const &);
LatLonHeight normalToLatLon(Vec3d const &);
LatLonHeight slerp(LatLonHeight const &, LatLonHeight const &, double t);

/*  This is not the true heading. This is only the Mercator projection
 *  heading. It's really only for use getting the heading of the
 *  vehicle while along the route. */

double getMercatorHeading(LatLonHeight const & from, LatLonHeight const & to);

/*! Haversine function.
 */
double haversine(double);
double inverseHaversine(double);

/*! Find great arc distance on a unit circle given lon/lat points.
 */
double sphericalDistance(LatLonHeight const & from, LatLonHeight const & to);


/*! Route and RoutePos is for moving along a route at accurate
 *  distances (assuming a spherical earth, etc.).
 */


typedef std::deque<LatLonHeight> Route;
typedef boost::shared_ptr<Route> RoutePtr;

double segmentLength(RoutePtr const &,  uint32_t);

/* Calculates new index and t. */

std::pair<uint32_t, double> move(RoutePtr const & route,
                                 uint32_t segment, 
                                 double t,
                                 double meters);


struct RoutePos {

    RoutePos();
    RoutePos(RoutePtr const &);
    RoutePos(RoutePtr const &, uint32_t index, double t);

    LatLonHeight getLatLon() const;
    double getHeading() const;

    RoutePtr const & getRoute() const;
    uint32_t getIndex() const;
    double getT() const;

    LatLonHeight getBegin() const;
    LatLonHeight getEnd() const;

    bool valid() const;
    bool pastEnd() const;

    double distanceToIndex(uint32_t) const;

private:

    RoutePtr route;
    uint32_t index;
    double t;
};


RoutePos operator+(RoutePos const & pos, double meters);
RoutePos operator-(RoutePos const & pos, double meters);


} //end namespace math
    
} //end namespace Tn

#endif // FOUNDATION__LAT_LON_HEIGHT_H__INCLUDED__

