/** \file
 *
 *  Contains the Location class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 */

#include "Location.h"
#include "io/OutStream.h"
#include "utils/AuvMath.h"
#include "utils/RingBuffer.h"

/*RingBufferVoid* Location::MallocRing_;

Location::StaticDestructor Location::StaticDestructor_;

Location::StaticDestructor::~StaticDestructor()
{
    delete MallocRing_;
    MallocRing_ = NULL;
}
*/
/// Default constructor
Location::Location()
    : //DataValue( 2 * sizeof( float ), LOCATION ),
      lat_( value_[ 0 ] ),
      lon_( value_[ 1 ] )
{
    value_[ 0 ] = value_[ 1 ] = __builtin_nan( "" );
}

/// Multi value unit constructior
Location::Location( const Unit& unit, const double& lat, const double& lon, bool scaleValues )
    : //DataValue( unit, 2 * sizeof( float ), LOCATION ),
      lat_( value_[ 0 ] ),
      lon_( value_[ 1 ] )
{
    value_[ 0 ] = scaleValues ? unit.getSI( lat ) : lat;
    value_[ 1 ] = scaleValues ? unit.getSI( lon ) : lon;
}

/// Copy Constructor
Location::Location( const Location& value )
    : //DataValue( value.getUnit(), 2 * sizeof( float ), LOCATION ),
      lat_( value_[ 0 ] ),
      lon_( value_[ 1 ] )
{
    value_[ 0 ] = value.lat_;
    value_[ 1 ] = value.lon_;
}
/*
/// Constructor from binary
Location::Location( const Unit& unit, const void *binary )
        : DataValue( unit, 2 * sizeof( float ), LOCATION ),
        lat_( value_[ 0 ] ),
        lon_( value_[ 1 ] )
{
    fromBinary( binary );
}
*/
const double Location::getLat( const Unit& unit ) const
{
    return unit.getScaled( lat_ );
}

const double Location::getLon( const Unit& unit ) const
{
    return unit.getScaled( lon_ );
}

void Location::setLat( const Unit& unit, const double& lat )
{
    lat_ = unit.getSI( lat );
}

void Location::setLon( const Unit& unit, const double& lon )
{
    lon_ = unit.getSI( lon );
}
/*
/// Override new
void* Location::operator new( size_t size )
{
    if ( NULL == dynamic_cast<RingBufferVoid*>( MallocRing_ ) )
    {
        MallocRing_ = new RingBufferVoid( true );
    }
    void* newMem = MallocRing_->pop();
    if ( NULL == newMem )
    {
        newMem = malloc( size );
    }
    return newMem;
}

/// Override delete
void Location::operator delete( void* p )
{
    if ( NULL != MallocRing_ )
    {
        MallocRing_->push( p );
    }
    else
    {
        free( p );
    }
}
*/
/// Return a pointer to a new copy
Location* Location::copy() const
{
    return new Location( *this );
}
/*
/// Subtraction of another DataValue to yield another DataValue
void Location::minus( const DataValue* rhs, DataValue* assignTo ) const
{
    if ( rhs->getBinaryType() == LOCATION && assignTo->getBinaryType() == LOCATION )
    {
        const Location* rhsLocation(( const Location* ) rhs );
        Location* assignToLocation(( Location* ) assignTo );
        assignToLocation->lat_ = lat_ -rhsLocation->lat_;
        assignToLocation->lon_ = lon_ -rhsLocation->lon_;
    }
}

/// Multiplication by a double to yield another DataValue
void Location::times( const double rhs, DataValue* assignTo ) const
{
    if ( assignTo->getBinaryType() == LOCATION )
    {
        Location* assignToLocation(( Location* ) assignTo );
        assignToLocation->lat_ = lat_ * rhs;
        assignToLocation->lon_ = lon_ * rhs;
    }
}

/// Return the absolute value of this difference from another DataValue
double Location::absDiff( const DataValue* rhs ) const
{
    if ( rhs->getBinaryType() == LOCATION )
    {
        const Location* rhsLocation(( const Location* ) rhs );
        return getDistance( *rhsLocation );
    }
    return __builtin_nan( "" );
}
*/
/// Copy n-dimensional value to double
bool Location::copyTo( const Unit& unit, double& assignTo, unsigned int dim ) const
{
    if( dim == 0 )
    {
        assignTo = getLat( unit );
        return true;
    }
    else if( dim == 1 )
    {
        assignTo = getLon( unit );
        return true;
    }
    return false;
}

/*
/// Comparison with another DataValue
int Location::compare( const DataValue& compareTo ) const
{
    if ( compareTo.getBinaryType() == LOCATION )
    {
        return ( int ) AuvMath::Sign( AuvMath::NanZero( lat_ - ( *( Location * ) &compareTo ).lat_ ) +
                                      AuvMath::NanZero( lon_ - ( *( Location * ) &compareTo ).lon_ ) );
    }
    return -1;
}

/// Equality test with another DataValue
bool Location::equals( const DataValue& compareTo ) const
{
    if ( compareTo.getBinaryType() == LOCATION )
    {
        return ( *( Location * ) &compareTo ).lat_ == lat_ &&
               ( *( Location * ) &compareTo ).lon_ == lon_;
    }
    return false;
}

/// set DataValue value
bool Location::setFrom( const DataValue& value )
{
    if ( value.getBinaryType() == LOCATION )
    {
        if ( ! value.equals( *this ) )
        {
            const Location* location = dynamic_cast<const Location*>( &value );
            if ( NULL != location )
            {
                bool floatsEqual = ( float )( location->lat_ ) == ( float )lat_ &&
                                   ( float )( location->lon_ ) == ( float )lon_;
                lat_ = location->lat_;
                lon_ = location->lon_;
                return !floatsEqual;
            }
        }
    }
    return false;
};
*/
// as a Str
Str Location::toString( const Unit& unit ) const
{
    return "[" + Str( unit.getScaled( value_[0] ) ) + ","
           + Str( unit.getScaled( value_[1] ) ) + "]";
}
/*
/// Send the data value to the indicated stream.
/// Return the number of bytes written.
/// For now, assume little-endian.
unsigned int Location::toStream( OutStream& outStream ) const
{
    //return outStream.write(( const char* ) & value_, typeSize_ ).gcount();
    float temp = ( float )value_[0];
    size_t written = outStream.write(( const char* ) & temp, typeSize_ / 2 ).bytesWritten();
    temp = ( float )value_[1];
    return written + outStream.write(( const char* ) &temp, typeSize_ / 2 ).bytesWritten();
}

/// Load the data from the a binary representation in the buffer
/// Typically just a memcopy of the variable, though endianness
/// may come into it.
void Location::fromBinary( const void *buffer )
{
    //memcpy(( void * ) & value_, ( const void * ) buffer, typeSize_ );
    float temp;
    memcpy(( void * ) &temp, ( const void * ) buffer, typeSize_ / 2 );
    value_[0] = temp;
    memcpy(( void * ) &temp, ( const unsigned char * ) buffer + typeSize_ / 2, typeSize_ / 2 );
    value_[1] = temp;
}
*/
Location& Location::operator=( const Location & rhs )
{
    lat_ = rhs.lat_;
    lon_ = rhs.lon_;
    return *this;
}

Location& Location::operator+=( const Location & rhs )
{
    lat_ += rhs.lat_;
    lon_ += rhs.lon_;
    return *this;
}

Location& Location::operator-=( const Location & rhs )
{
    lat_ -= rhs.lat_;
    lon_ -= rhs.lon_;
    return *this;
}

Location& Location::operator*=( const double mult )
{
    lat_ *= mult;
    lon_ *= mult;
    return *this;
}

bool Location::isNan() const
{
    return isnan( lat_ ) || isnan( lon_ );
}

const bool Location::operator==( const Location& rhs ) const
{
    return ( lat_ == rhs.lat_ && lon_ == rhs.lon_ )
           || ( isnan( lat_ ) && isnan( rhs.lat_ ) && isnan( lon_ ) && isnan( rhs.lon_ ) );
}

const bool Location::operator!=( const Location& rhs ) const
{
    return !( *this == rhs );
}

const Location Location::operator+( const Location& rhs ) const
{
    return Location( *this ) += rhs;
}

const Location Location::operator-( const Location& rhs ) const
{
    return Location( *this ) -= rhs;
}

const Location Location::operator*( const double mult ) const
{
    return Location( *this ) *= mult;
}

double Location::GetBearing( const double& lat0, const double& lon0, const double& lat1, const double& lon1 )
{
    double y = sin( lon1 - lon0 ) * cos( lat1 );
    double x = cos( lat0 ) * sin( lat1 ) - sin( lat0 ) * cos( lat1 ) * cos( lon1 - lon0 );
    return atan2( y, x );
}

double Location::getBearing( const double& lat1, const double& lon1 ) const
{
    return GetBearing( lat_, lon_, lat1, lon1 );
}

double Location::getBearing( const Location & location1 ) const
{
    return GetBearing( lat_, lon_, location1.lat_, location1.lon_ );
}

const double Location::EARTH_RADIUS = 6371000.0; // meters
const double Location::EARTH_RADIUS_INVERTED = 1.0 / 6371000.0; // meters

void Location::CalcVector( const double& lat0, const double& lon0, const double& lat1, const double& lon1, double& toEast, double& toNorth )
{
    toEast = EARTH_RADIUS * sin( lon1 - lon0 ) * cos( lat1 );
    toNorth = EARTH_RADIUS * ( cos( lat0 ) * sin( lat1 ) - sin( lat0 ) * cos( lat1 ) * cos( lon1 - lon0 ) );
}

void Location::calcVector( const Location & location1, double& toEast, double& toNorth ) const
{
    CalcVector( lat_, lon_, location1.lat_, location1.lon_, toEast, toNorth );
}

// Calculates the vector (in meters, northings & eastings) between this location and another.
Point2D Location::calcVector( const Location& location1 ) const
{
    double toNorth, toEast;
    this->calcVector( location1, toEast, toNorth );
    return Point2D( toNorth, toEast );
}

double Location::GetDistance( const double& lat0, const double& lon0, const double& lat1, const double& lon1 )
{
    return 2.*EARTH_RADIUS * asin( sqrt( HavSin( lat1 - lat0 ) + cos( lat0 ) * cos( lat1 ) * HavSin( lon1 - lon0 ) ) );
}

// Gives the distance (in meters) between two locations (in radians).
// Derived from Spherical Law of Cosines, no inverse trig.
// Less accurate at long distances (0.25% error at 4000km), but very accurate for short distances
double Location::GetDistanceFast( const double& lat0, const double& lon0, const double& lat1, const double& lon1 )
{
    return EARTH_RADIUS * sqrt( AuvMath::Square( lat1 - lat0 ) + cos( lat0 ) * cos( lat1 ) * AuvMath::Square( lon1 - lon0 ) );
}

double Location::getDistance( const double& lat1, const double& lon1 ) const
{
    return GetDistance( lat_, lon_, lat1, lon1 );
}

double Location::getDistance( const Location & location1 ) const
{
    return GetDistance( lat_, lon_, location1.lat_, location1.lon_ );
}

double Location::getDistanceFast( const Location & location1 ) const
{
    return GetDistanceFast( lat_, lon_, location1.lat_, location1.lon_ );
}

void Location::AtBearing( const float bearing, const float distance, double& lat, double& lon )
{
    float r = distance * EARTH_RADIUS_INVERTED;
    double newLat = asin( sin( lat ) * cos( r ) +
                          cos( lat ) * sin( r ) * cos( bearing ) );
    lon += atan2( sin( bearing ) * sin( r ) * cos( lat ),
                  cos( r ) - sin( lat ) * sin( newLat ) );
    lat = newLat;
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */
/* Vincenty Direct Solution of Geodesics on the Ellipsoid (c) Chris Veness 2005-2011              */
/*                                                                                                */
/* from: Vincenty direct formula - T Vincenty, "Direct and Inverse Solutions of Geodesics on the  */
/*       Ellipsoid with application of nested equations", Survey Review, vol XXII no 176, 1975    */
/*       http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf                                             */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -  */

/**
 * Calculates destination point given start point lat/long, bearing & distance,
 * using Vincenty inverse formula for ellipsoids
 *
 * Adapted from: http://www.movable-type.co.uk/scripts/latlong-vincenty-direct.html
 *
 */
void Location::AtBearingEllipsoid( const float brng, const float dist, double& lat1, double& lon1 )
{
    const double a = 6378137, b = 6356752.3142,  f = 1 / 298.257223563;  // WGS-84 ellipsiod
    double s = dist;
    double alpha1 = brng;
    double sinAlpha1 = sin( alpha1 );
    double cosAlpha1 = cos( alpha1 );

    double tanU1 = ( 1 - f ) * tan( lat1 );
    double cosU1 = 1 / sqrt( ( 1 + tanU1 * tanU1 ) ), sinU1 = tanU1 * cosU1;
    double sigma1 = atan2( tanU1, cosAlpha1 );
    double sinAlpha = cosU1 * sinAlpha1;
    double cosSqAlpha = 1 - sinAlpha * sinAlpha;
    double uSq = cosSqAlpha * ( a * a - b * b ) / ( b * b );
    double A = 1 + uSq / 16384 * ( 4096 + uSq * ( -768 + uSq * ( 320 - 175 * uSq ) ) );
    double B = uSq / 1024 * ( 256 + uSq * ( -128 + uSq * ( 74 - 47 * uSq ) ) );

    double sigma = s / ( b * A ), sigmaP = 2 * M_PI;
    double cos2SigmaM = cos( 2 * sigma1 + sigma );
    double sinSigma = sin( sigma );
    double cosSigma = cos( sigma );
    while( fabs( sigma - sigmaP ) > 1e-12 )
    {
        cos2SigmaM = cos( 2 * sigma1 + sigma );
        sinSigma = sin( sigma );
        cosSigma = cos( sigma );
        double deltaSigma = B * sinSigma * ( cos2SigmaM + B / 4 * ( cosSigma * ( -1 + 2 * cos2SigmaM * cos2SigmaM ) -
                                             B / 6 * cos2SigmaM * ( -3 + 4 * sinSigma * sinSigma ) * ( -3 + 4 * cos2SigmaM * cos2SigmaM ) ) );
        sigmaP = sigma;
        sigma = s / ( b * A ) + deltaSigma;
    }

    double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
    double lat2 = atan2( sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                         ( 1 - f ) * sqrt( sinAlpha * sinAlpha + tmp * tmp ) );
    double lambda = atan2( sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1 );
    double C = f / 16 * cosSqAlpha * ( 4 + f * ( 4 - 3 * cosSqAlpha ) );
    double L = lambda - ( 1 - C ) * f * sinAlpha *
               ( sigma + C * sinSigma * ( cos2SigmaM + C * cosSigma * ( -1 + 2 * cos2SigmaM * cos2SigmaM ) ) );
    double lon2 = AuvMath::ModPi( lon1 + L + 3 * M_PI );  // normalise to -180...+180

    //double revAz = atan2(sinAlpha, -tmp);  // final bearing, if required

    lat1 = lat2;
    lon1 = lon2;

}

void Location::atBearing( const double bearing, const double distance )
{
    AtBearing( bearing, distance, lat_, lon_ );
}

// Calculate a new latitude due to an absolute change in northings
double Location::NorthingsDelta( const double startLat, const float northingsDelta )
{
    double r = northingsDelta * EARTH_RADIUS_INVERTED;
    return asin( sin( startLat ) * cos( r ) + cos( startLat ) * sin( r ) );
}

// Calculate a new longitude due to an absolute change in eastings
double Location::EastingsDelta( const double startLat, const double startLon, const float eastingsDelta )
{
    double r = eastingsDelta * EARTH_RADIUS_INVERTED;
    return startLon + atan2( sin( r ) * cos( startLat ),
                             cos( r ) - sin( startLat ) * sin( startLat ) );
}

// Adjust latitude & longitude for an absolute change in northings and eastings
void Location::Delta( double& lat, double& lon, double northings, double eastings )
{
    AtBearing( atan2( eastings, northings ), sqrt( northings * northings + eastings * eastings ), lat, lon );
}

double Location::DistanceFromBaseline( const Location location, const Location pointOnLine1, const Location pointOnLine2 )
{
    // Let's say location's coordinate is 0,0 --> find out the other coordinates
    Point2D vector1 = location.calcVector( pointOnLine1 );
    Point2D vector2 = location.calcVector( pointOnLine2 );
    double vdist = ( vector1 - vector2 ).getMagnitude();
    return abs( ( vector2.getY() - vector1.getY() ) * vector1.getX() - vector1.getY() * ( vector2.getX() - vector1.getX() ) ) / vdist;
}

// Caculate two locations from intesecting circles around two center locations and radii
// Return values:
//   intersect1, intersect2: locations where the circles intersect, or if they don't, where they are the closest
//   intersecting: true if the two circles intersects, false if the two circles do not intersects
bool Location::IntersectingCircles( const Location& c1, const double& r1, const Location& c2, const double& r2,
                                    Location& intersect1, Location& intersect2 )
{
    // [Matlab] function [P1, P2, intersect] = circleIntersections(C1, r1, C2, r2)
    // Convert to eastings, northings relative to c1
    double e1 = 0, n1 = 0, e2, n2;
    c1.calcVector( c2, e2, n2 );
    // Distance between circles centers
    double d = sqrt( ( e1 - e2 ) * ( e1 - e2 ) + ( n1 - n2 ) * ( n1 - n2 ) );
    bool intersecting = r1 + r2 >= d && d >= abs( r1 - r2 );

    if( intersecting )
    {
        // Two circles intersects or tangent
        // Area according to Heron's formula
        //----------------------------------
        double a1 = d + r1 + r2;
        double a2 = d + r1 - r2;
        double a3 = d - r1 + r2;
        double a4 = -d + r1 + r2;
        double area = sqrt( a1 * a2 * a3 * a4 ) / 4;

        // Calculating x axis intersection values
        //---------------------------------------
        double val1 = ( e1 + e2 ) / 2 + ( e2 - e1 ) * ( r1 * r1 - r2 * r2 ) / ( 2 * d * d );
        double val2 = 2 * ( n1 - n2 ) * area / ( d * d );
        double x1 = val1 + val2;
        double x2 = val1 - val2;

        // Calculating y axis intersection values
        //---------------------------------------
        val1 = ( n1 + n2 ) / 2 + ( n2 - n1 ) * ( r1 * r1 - r2 * r2 ) / ( 2 * d * d );
        val2 = 2 * ( e1 - e2 ) * area / ( d * d );
        double y1 = val1 - val2;
        double y2 = val1 + val2;

        // Intersection pointsare (x1, y1) and (x2, y2)
        // Because for every x we have two values of y, and the same thing for y,
        // we have to verify that the intersection points as chose are on the
        // circle otherwise we have to swap between the points
        double test = abs( ( x1 - e1 ) * ( x1 - e1 ) + ( y1 - n1 ) * ( y1 - n1 ) - r1 * r1 );
        if( test > 0.001 )
        {
            // point is not on the circle, swap between y1 and y2
            // the value of 0.0000001 is arbitrary chose, smaller values are also OK
            // do not use the value 0 because of computer rounding problems
            double tmp = y1;
            y1 = y2;
            y2 = tmp;
        }
        intersect1 = c1;
        intersect1.delta( y1, x1 );
        intersect2 = c1;
        intersect2.delta( y2, x2 );
        intersecting = true;
    }
    else
    {
        // circles are not intersecting each other
        // Return point between C1 and C2
        double r1x = r1 + ( d - r1 - r2 ) / 2;
        double x = e1 + ( e2 - e1 ) * r1x / d;
        double y = n1 + ( n2 - n1 ) * r1x / d;
        intersect1 = c1;
        intersect1.delta( y, x );
        intersect2 = intersect1;
    }
    return intersecting;
}

double Location::HavSin( double angle )
{
    return sin( angle / 2. ) * sin( angle / 2. );
}