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

#include <stdlib.h>
#include "AuvMath.h"
#include <cstdio>
#include <stdint.h>

const double AuvMath::DEG_TO_RAD = atan( 1 ) / 45;
const double AuvMath::RAD_TO_DEG = 45 / atan( 1 );

/// Makes sure a variable falls within defined limits...
/// Either limit may be min or max.
/// If any input is nan, nan is returned
template <typename T>
T AuvMath::Limit( const T& value, const T& limit1, const T& limit2 )
{
    T min( limit1 ), max( limit2 );
    // This is a template-safe way to return a NaN
    if( value != value )
    {
        return value;
    }
    if( min != min )
    {
        return min;
    }
    if( max != max )
    {
        return max;
    }
    if( max < min )
    {
        min = limit2;
        max = limit1;
    }
    if( value > max )
    {
        return max;
    }
    if( value < min )
    {
        return min;
    }
    return value;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template signed char AuvMath::Limit<signed char>( const signed char&, const signed char&, const signed char& );
template unsigned char AuvMath::Limit< unsigned char>( const unsigned char&, const unsigned char&, const unsigned char& );
template int AuvMath::Limit<int>( const int&, const int&, const int& );
template unsigned int AuvMath::Limit<unsigned int>( const unsigned int&, const unsigned int&, const unsigned int& );
template float AuvMath::Limit<float>( const float&, const float&, const float& );
template double AuvMath::Limit<double>( const double&, const double&, const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

/// Tests whether a variable falls within defined limits...
/// Either limit may be min or max.
/// If any input is nan, nan is returned
template <typename T>
bool AuvMath::InLimit( const T& value, const T& limit1, const T& limit2 )
{
    T min( limit1 ), max( limit2 );
    // This is a template-safe way to return a NaN
    if( value != value )
    {
        return false;
    }
    if( min != min )
    {
        return false;
    }
    if( max != max )
    {
        return false;
    }
    if( max < min )
    {
        min = limit2;
        max = limit1;
    }
    if( value > max )
    {
        return false;
    }
    if( value < min )
    {
        return false;
    }
    return true;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template bool AuvMath::InLimit<signed char>( const signed char&, const signed char&, const signed char& );
template bool AuvMath::InLimit< unsigned char>( const unsigned char&, const unsigned char&, const unsigned char& );
template bool AuvMath::InLimit<int>( const int&, const int&, const int& );
template bool AuvMath::InLimit<unsigned int>( const unsigned int&, const unsigned int&, const unsigned int& );
template bool AuvMath::InLimit<float>( const float&, const float&, const float& );
template bool AuvMath::InLimit<double>( const double&, const double&, const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
T AuvMath::ModPi( const T& angle )
{
    return angle == angle ? -floor( angle * M_1_2PI + 0.5 ) * M_2PI + angle : angle;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::ModPi<float>( const float& );
template double AuvMath::ModPi<double>( const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

// Normalizes any number to an arbitrary range, but used here for wrapping angles when going below min or above max
template <typename T>
T AuvMath::NormalizeAngle( const T& value, const T& start, const T& end )
{
    const T width = end - start;
    T offsetValue = value - start;

    return value == value ? ( ( offsetValue - ( floor( offsetValue / width ) * width ) ) + start ) : value;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::NormalizeAngle<float>( const float&, const float& = 0.0, const float& = M_2PI );     // defaults to 0-360 degree range
template double AuvMath::NormalizeAngle<double>( const double&, const double& = 0.0, const double& = M_2PI ); // defaults to 0-360 degree range
/// \endcond // DOXYGEN_CANT_HANDLE_THIS


template <typename T>
T AuvMath::NanZero( const T& value )
{
    return value != value ? 0 : value;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::NanZero<float>( const float& );
template double AuvMath::NanZero<double>( const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

/// Has no effect, but included for use by template functions that accept uint8_t
template<>
uint8_t AuvMath::FlipEndian( uint8_t value )
{
    return value;
}

/// Has no effect, but included for use by template functions that accept int8_t
template<>
int8_t AuvMath::FlipEndian( int8_t value )
{
    return value;
}

template<>
uint16_t AuvMath::FlipEndian( uint16_t value )
{
    return ( value << 8 ) | ( value >> 8 );
}

template<>
int16_t AuvMath::FlipEndian( int16_t value )
{
    return ( value << 8 ) | ( ( value >> 8 ) & 0xFF );
}


template<>
uint32_t AuvMath::FlipEndian( uint32_t value )
{
    value = ( ( value << 8 ) & 0xFF00FF00 ) | ( ( value >> 8 ) & 0xFF00FF );
    return ( value << 16 ) | ( value >> 16 );
}

template<>
int32_t AuvMath::FlipEndian( int32_t value )
{
    value = ( ( value << 8 ) & 0xFF00FF00 ) | ( ( value >> 8 ) & 0xFF00FF );
    return ( value << 16 ) | ( ( value >> 16 ) & 0xFFFF );
}

template<>
uint64_t AuvMath::FlipEndian( uint64_t value )
{
    value = ( ( value << 8 ) & 0xFF00FF00FF00FF00ULL ) | ( ( value >> 8 ) & 0x00FF00FF00FF00FFULL );
    value = ( ( value << 16 ) & 0xFFFF0000FFFF0000ULL ) | ( ( value >> 16 ) & 0x0000FFFF0000FFFFULL );
    return ( value << 32 ) | ( value >> 32 );
}

template<>
int64_t AuvMath::FlipEndian( int64_t value )
{
    value = ( ( value << 8 ) & 0xFF00FF00FF00FF00ULL ) | ( ( value >> 8 ) & 0x00FF00FF00FF00FFULL );
    value = ( ( value << 16 ) & 0xFFFF0000FFFF0000ULL ) | ( ( value >> 16 ) & 0x0000FFFF0000FFFFULL );
    return ( value << 32 ) | ( ( value >> 32 ) & 0xFFFFFFFFULL );
}

template<>
float AuvMath::FlipEndian( float value )
{
    uint32_t ival = FlipEndian( *( uint32_t* )&value );
    return *( float* )&ival;
}

template<>
double AuvMath::FlipEndian( double value )
{
    uint64_t ival = FlipEndian( *( uint64_t* )&value );
    return *( double* )&ival;
}

/// Makes sure a variable is divisible by specified divisor...
/// If any input is nan, nan is returned
template <typename T>
T AuvMath::Round( const T& value, const T& divisor )
{
    // This is a template-safe way to return a NaN
    if( value != value )
    {
        return value;
    }
    if( divisor != divisor )
    {
        return divisor;
    }
    if( divisor == 0 )
    {
        return nan( "" );
    }
    return round( value / divisor ) * divisor ;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::Round<float>( const float&, const float& );
template double AuvMath::Round<double>( const double&, const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
T AuvMath::Sign( const T& x )
{
    return x != x ? x : ( x == 0 ? 0 : ( x < 0 ? -1 : ( x > 0 ? 1 : x ) ) );
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template signed char AuvMath::Sign<signed char>( const signed char& );
template int AuvMath::Sign<int>( const int& );
template float AuvMath::Sign<float>( const float& );
template double AuvMath::Sign<double>( const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
int AuvMath::FindLtEqIndex( const T* values, const T& value,
                            const unsigned int indexMin, const unsigned int indexMax )
{
    if( values[ indexMin ] > value || values[ indexMax ] < value )
    {
        return -1;
    }
    unsigned int indexMid = ( indexMin + indexMax ) >> 1;
    if( indexMid != indexMin )
    {
        T valueMid = values[ indexMid ];
        if( value < valueMid )
        {
            if( indexMid - indexMin > 1 )
            {
                return FindLtEqIndex( values, value, indexMin, indexMid );
            }
            return indexMin;
        }
        else if( value > valueMid )
        {
            if( indexMax - indexMid > 1 )
            {
                return FindLtEqIndex( values, value, indexMid, indexMax );
            }
            if( value == values[ indexMax ] )
            {
                return indexMax;
            }
        }
    }
    return indexMid;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template int AuvMath::FindLtEqIndex<float>( const float*, const float&, const unsigned int, const unsigned int );
template int AuvMath::FindLtEqIndex<double>( const double*, const double&, const unsigned int, const unsigned int );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
T AuvMath::Interpolate1D( const T& x, const T& data0, const T& data1,
                          const T& x0, const T& x1 )
{
    T xx = ( x1 - x0 );
    if( xx == 0 )
    {
        return nan( "" );
    }


    xx = ( x - x0 ) / xx;
    return data0 + ( data1 - data0 ) * xx;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::Interpolate1D<float>( const float&, const float&, const float&, const float&, const float& );
template double AuvMath::Interpolate1D<double>( const double&, const double&, const double&, const double&, const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
T AuvMath::Interpolate2D( const T& x, const T& y, T data[][ 2 ],
                          const T& x0, const T& x1, const T& y0, const T& y1 )
{
    T xx = ( x1 - x0 );
    if( xx == 0 )
    {
        return nan( "" );
    }
    xx = ( x - x0 ) / xx;
    T yy = ( y1 - y0 );
    if( yy == 0 )
    {
        return nan( "" );
    }
    yy = ( y - y0 ) / yy;
    return data[ 0 ][ 0 ] * ( 1 - xx ) * ( 1 - yy ) +
           data[ 1 ][ 0 ] * xx * ( 1 - yy ) +
           data[ 0 ][ 1 ] * ( 1 - xx ) * yy +
           data[ 1 ][ 1 ] * xx * yy;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::Interpolate2D<float>( const float&, const float&, float[][2], const float&, const float&, const float&, const float& );
template double AuvMath::Interpolate2D<double>( const double&, const double&, double[][2], const double&, const double&, const double&, const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
T AuvMath::Interpolate3D( const T& x, const T& y, const T& z,
                          T data[][ 2 ][ 2 ],
                          const T& x0, const T& x1,
                          const T& y0, const T& y1,
                          const T& z0, const T& z1 )
{
    T xx = ( x1 - x0 );
    if( xx == 0 )
    {
        return nan( "" );
    }
    xx = ( x - x0 ) / xx;
    T yy = ( y1 - y0 );
    if( yy == 0 )
    {
        return nan( "" );
    }
    yy = ( y - y0 ) / yy;
    T zz = ( z1 - z0 );
    if( zz == 0 )
    {
        return nan( "" );
    }
    zz = ( z - z0 ) / zz;
    return data[ 0 ][ 0 ][ 0 ] * ( 1 - xx ) * ( 1 - yy ) * ( 1 - zz ) +
           data[ 1 ][ 0 ][ 0 ] * xx * ( 1 - yy ) * ( 1 - zz ) +
           data[ 0 ][ 1 ][ 0 ] * ( 1 - xx ) * yy * ( 1 - zz ) +
           data[ 0 ][ 0 ][ 1 ] * ( 1 - xx ) * ( 1 - yy ) * zz +
           data[ 1 ][ 0 ][ 1 ] * xx * ( 1 - yy ) * zz +
           data[ 0 ][ 1 ][ 1 ] * ( 1 - xx ) * yy * zz +
           data[ 1 ][ 1 ][ 0 ] * xx * yy * ( 1 - zz ) +
           data[ 1 ][ 1 ][ 1 ] * xx * yy * zz;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::Interpolate3D<float>( const float&, const float&, const float&, float[][2][2], const float&, const float&, const float&, const float&, const float&, const float& );
template double AuvMath::Interpolate3D<double>( const double&, const double&, const double&, double[][2][2], const double&, const double&, const double&, const double&, const double&, const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
T AuvMath::Interpolate4D( const T& w, const T& x, const T& y,
                          const T& z, T data[][ 2 ][ 2 ][ 2 ],
                          const T& w0, const T& w1,
                          const T& x0, const T& x1,
                          const T& y0, const T& y1,
                          const T& z0, const T& z1 )
{
    T ww = ( w1 - w0 );
    if( ww == 0 )
    {
        return nan( "" );
    }
    ww = ( w - w0 ) / ww;
    T xx = ( x1 - x0 );
    if( xx == 0 )
    {
        return nan( "" );
    }
    xx = ( x - x0 ) / xx;
    T yy = ( y1 - y0 );
    if( yy == 0 )
    {
        return nan( "" );
    }
    yy = ( y - y0 ) / yy;
    T zz = ( z1 - z0 );
    if( zz == 0 )
    {
        return nan( "" );
    }
    zz = ( z - z0 ) / zz;
    return data[ 0 ][ 0 ][ 0 ][ 0 ] * ( 1 - ww ) * ( 1 - xx ) * ( 1 - yy ) * ( 1 - zz ) +
           data[ 0 ][ 0 ][ 0 ][ 1 ] * ( 1 - ww ) * ( 1 - xx ) * ( 1 - yy ) * zz +
           data[ 0 ][ 0 ][ 1 ][ 0 ] * ( 1 - ww ) * ( 1 - xx ) * yy * ( 1 - zz ) +
           data[ 0 ][ 0 ][ 1 ][ 1 ] * ( 1 - ww ) * ( 1 - xx ) * yy * zz +
           data[ 0 ][ 1 ][ 0 ][ 0 ] * ( 1 - ww ) * xx * ( 1 - yy ) * ( 1 - zz ) +
           data[ 0 ][ 1 ][ 0 ][ 1 ] * ( 1 - ww ) * xx * ( 1 - yy ) * zz +
           data[ 0 ][ 1 ][ 1 ][ 0 ] * ( 1 - ww ) * xx * yy * ( 1 - zz ) +
           data[ 0 ][ 1 ][ 1 ][ 1 ] * ( 1 - ww ) * xx * yy * zz +
           data[ 1 ][ 0 ][ 0 ][ 0 ] * ww * ( 1 - xx ) * ( 1 - yy ) * ( 1 - zz ) +
           data[ 1 ][ 0 ][ 0 ][ 1 ] * ww * ( 1 - xx ) * ( 1 - yy ) * zz +
           data[ 1 ][ 0 ][ 1 ][ 0 ] * ww * ( 1 - xx ) * yy * ( 1 - zz ) +
           data[ 1 ][ 0 ][ 1 ][ 1 ] * ww * ( 1 - xx ) * yy * zz +
           data[ 1 ][ 1 ][ 0 ][ 0 ] * ww * xx * ( 1 - yy ) * ( 1 - zz ) +
           data[ 1 ][ 1 ][ 0 ][ 1 ] * ww * xx * ( 1 - yy ) * zz +
           data[ 1 ][ 1 ][ 1 ][ 0 ] * ww * xx * yy * ( 1 - zz ) +
           data[ 1 ][ 1 ][ 1 ][ 1 ] * ww * xx * yy * zz;
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template float AuvMath::Interpolate4D<float>( const float&, const float&, const float&, const float&, float[][2][2][2], const float&, const float&, const float&, const float&, const float&, const float&, const float&, const float& );
template double AuvMath::Interpolate4D<double>( const double&, const double&, const double&, const double&, double[][2][2][2], const double&, const double&, const double&, const double&, const double&, const double&, const double&, const double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
void AuvMath::UnitVectorToAzimuthAndElevation( Point3D &uhat, T &azimuth, T &elevation )
{
    azimuth = ( uhat[0] == uhat[0] && uhat[1] == uhat[1] ) ? atan2( uhat[1], uhat[0] ) : nanf( "" );
    elevation = uhat[2] == uhat[2] && abs( uhat[2] ) <= 1 ? asin( -1.0 * uhat[2] ) : nanf( "" );
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template void AuvMath::UnitVectorToAzimuthAndElevation<float>( Point3D&, float&, float& );
template void AuvMath::UnitVectorToAzimuthAndElevation<double>( Point3D&, double&, double& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

template <typename T>
void AuvMath::AzimuthAndElevationToUnitVector( T &azimuth, T &elevation, Point3D &uhat )
{
    // NOTE: inputs should be in radians!
    double ca, ce, sa, se;
    if( azimuth == azimuth && elevation == elevation )
    {
        sa = sin( azimuth );
        ca = cos( azimuth );
        se = sin( elevation );
        ce = cos( elevation );
        uhat.setX( ca * ce );
        uhat.setY( sa * ce );
        uhat.setZ( -1.0 * se );
    }
    else
    {
        uhat = nanf( "" );
    }
}

/// \cond DOXYGEN_CANT_HANDLE_THIS
template void AuvMath::AzimuthAndElevationToUnitVector<float>( float&, float&, Point3D& );
template void AuvMath::AzimuthAndElevationToUnitVector<double>( double&, double&, Point3D& );
/// \endcond // DOXYGEN_CANT_HANDLE_THIS

double AuvMath::BulkModulus( double salinity, double temperature, double pressure )
{
    // use constant names exactly as in UNESCO 1983

    const double E0 =   19652.21;
    const double E1 =   148.4206;
    const double E2 =  -2.327105;
    const double E3 =   1.360477E-2;
    const double E4 =  -5.155288E-5;

    const double F0 =  54.6746;
    const double F1 =  -0.603459;
    const double F2 =   1.09987E-2;
    const double F3 =  -6.1670E-5;

    const double G0 =   7.944E-2;
    const double G1 =   1.6483E-2;
    const double G2 =  -5.3009E-4;

    const double H0 =   3.239908;
    const double H1 =   1.43713E-3;
    const double H2 =   1.16092E-4;
    const double H3 =  -5.77905E-7;

    const double I0 =   2.2838E-3;
    const double I1 =  -1.0981E-5;
    const double I2 =  -1.6078E-6;

    const double J0 =   1.91075E-4;        // ADDED LAST DIGIT 18 FEB 88 WWB

    const double K0 =   8.50935E-5;
    const double K1 =  -6.12293E-6;
    const double K2 =   5.2787E-8;

    const double M0 =  -9.9348E-7;
    const double M1 =   2.0816E-8;
    const double M2 =   9.1697E-10;

    double PRESS = pressure / 1e5;      // internally use pressure in bar
    double TEMP = temperature - 273.15; // internally use temperature in Celsius
    double KW   = E0 + ( E1 + ( E2 + ( E3 + E4 * TEMP ) * TEMP ) * TEMP ) * TEMP;       // eq 19

    double AW   = H0 + ( H1 + ( H2 + H3 * TEMP ) * TEMP ) * TEMP;
    double BW   = K0 + ( K1 + K2 * TEMP ) * TEMP;
    double A    = AW + ( I0 + ( I1 + I2 * TEMP ) * TEMP ) * salinity + J0 * salinity * sqrt( salinity ); // eq 17
    double B    = BW + ( M0 + ( M1 + M2 * TEMP ) * TEMP ) * salinity;        // eq 18
    double F    = ( F0 + ( F1 + ( F2 + F3 * TEMP ) * TEMP ) * TEMP ) * salinity;
    double G    = ( G0 + ( G1 + G2 * TEMP ) * TEMP ) * salinity * sqrt( salinity );

    double KST0 = KW + F + G;                                      // eq 16

    double bars = KST0 + ( A + B * PRESS ) * PRESS;               // eq 15
    return bars * 1e5;

}


int AuvMath::binaryToDecimal( int n )
{
    int num = n;
    int dec_value = 0;
    int base = 1;
    int temp = num;
    while( temp )
    {
        int last_digit = temp % 10;
        temp = temp / 10;

        dec_value += last_digit * base;

        base = base * 2;
    }
    return dec_value;
}

double AuvMath::Density( double salinity, double temperature, double pressure )
{
    const double A0 = 999.842594;
    const double A1 =   6.793952E-2;
    const double A2 =  -9.095290E-3;
    const double A3 =   1.001685E-4;
    const double A4 =  -1.120083E-6;
    const double A5 =   6.536332E-9;
    const double B0 =   8.24493E-1;
    const double B1 =  -4.0899E-3;
    const double B2 =   7.6438E-5;
    const double B3 =  -8.2467E-7;
    const double B4 =   5.3875E-9;
    const double C0 =  -5.72466E-3;
    const double C1 =   1.0227E-4;
    const double C2 =  -1.6546E-6;
    const double D0 =   4.8314E-4;

    double TEMP = temperature - 273.15; // internally use temperature in Celsius

    double RHOW = A0 + ( A1 + ( A2 + ( A3 + ( A4 + A5 * TEMP ) * TEMP ) * TEMP ) * TEMP ) * TEMP;    // eq 4
    double B    = ( B0 + ( B1 + ( B2 + ( B3 + B4 * TEMP ) * TEMP ) * TEMP ) * TEMP ) * salinity;
    double C    = ( C0 + ( C1 + C2 * TEMP ) * TEMP ) * salinity * sqrt( salinity );
    double D    = D0 * salinity * salinity;
    double KSTP = BulkModulus( salinity, temperature, pressure );      // separate subroutine
    double RHO  = RHOW + B + C + D;           // rho(salinity,TEMP,0), eq 13
    RHO  = RHO / ( 1 - pressure / KSTP ); // eq 07

    return RHO;
}

/// For pressure in pascals and latitude in radians, return depth in meters
double AuvMath::OceanDepth( const double& pressure, const double& latitude )
{
    const double C1 =  9.72659;
    const double C2 = -2.2512E-5;
    const double C3 =  2.279E-10;
    const double C4 = -1.82E-15;

    const double G0 = 9.780318;
    const double G1 = 5.2788E-3;
    const double G2 = 2.36E-5;

    const double GAMMA = 2.184E-6;

    double PRESS = pressure / 1e4;      // internally use pressure in dbar
    double X = sin( latitude );
    X *= X;
    double GRAVITY = G0 * ( 1.0 + ( G1 + G2 * X ) * X ) + ( GAMMA / 2 ) * PRESS;
    double Z = ( C1 + ( C2 + ( C3 + C4 * PRESS ) * PRESS ) * PRESS ) * PRESS;
    return Z / GRAVITY;
}

/// For depth in meters and latitude in radians, return pressure in pascals
double AuvMath::OceanPressure( const double& depth, const double& latitude )
{
    const double G0 = 9.780318;
    const double G1 = 5.2788E-3;

    double X = sin( latitude );
    X *= X;

    double gLatitude =  G0 * ( 1 + G1 * X );

    double kDepthLatitude = ( gLatitude - 2E-5 * depth ) / ( 9.80612 - 2E-5 * depth );

    double depth2 = depth * depth;

    double hDepth45 = 1.00818E-2 * depth + 2.465E-8 * depth2 - 1.25E-13 * depth2 * depth + 2.8E-19 * depth2 * depth2;

    double hDepthLatitude = hDepth45 * kDepthLatitude;

    double thyh0Depth = depth / ( depth + 100 ) / 100 + 6.2E-6 * depth;

    double pDepthLatitude = hDepthLatitude - thyh0Depth;

    return pDepthLatitude * 1e6;
}

// Hill ratio from in-situ temperature (deg C)
double AuvMath::HillRatioAtSP2( const double&  temperature )
{
    const double a0  =  0.0080;
    const double a1  = -0.1692;
    const double a2  = 25.3851;
    const double a3  = 14.0941;
    const double a4  = -7.0261;
    const double a5  =  2.7081;
    const double b0  =  0.0005;
    const double b1  = -0.0056;
    const double b2  = -0.0066;
    const double b3  = -0.0375;
    const double b4  =  0.0636;
    const double b5  = -0.0144;
    const double k   =  0.0162;

    const double g0  = 2.641463563366498e-1;
    const double g1  = 2.007883247811176e-4;
    const double g2  = -4.107694432853053e-6;
    const double g3  = 8.401670882091225e-8;
    const double g4  = -1.711392021989210e-9;
    const double g5  = 3.374193893377380e-11;
    const double g6  = -5.923731174730784e-13;
    const double g7  = 8.057771569962299e-15;
    const double g8  = -7.054313817447962e-17;
    const double g9  = 2.859992717347235e-19;
    const double sp2 = 2.0;

    double t68  = temperature * 1.00024;
    double ft68 = ( t68 - 15.0 ) / ( 1.0 + k * ( t68 - 15.0 ) );

    // Find the initial estimates of Rtx (Rtx0) and of the derivative dSP_dRtx
    // at SP = 2.
    double rtx0     = g0 + t68 * ( g1 + t68 * ( g2 + t68 * ( g3 + t68 * ( g4 +
                                   t68 * ( g5 + t68 * ( g6 + t68 * ( g7 + t68 * ( g8 + t68 * g9 ) ) ) ) ) ) ) );
    double dsp_drtx = a1 + ( 2 * a2 + ( 3 * a3 + ( 4 * a4 + 5 * a5 * rtx0 ) * rtx0 ) * rtx0 ) * rtx0 +
                      ft68 * ( b1 + ( 2 * b2 + ( 3 * b3 + ( 4 * b4 + 5 * b5 * rtx0 ) * rtx0 ) *
                                      rtx0 ) * rtx0 );

    // Begin a single modified Newton-Raphson iteration to find Rt at SP = 2.
    double sp_est   = a0 + ( a1 + ( a2 + ( a3 + ( a4 + a5 * rtx0 ) * rtx0 ) * rtx0 ) * rtx0 ) * rtx0
                      + ft68 * ( b0 + ( b1 + ( b2 + ( b3 + ( b4 + b5 * rtx0 ) * rtx0 ) * rtx0 ) *
                                        rtx0 ) * rtx0 );
    double rtx      = rtx0 - ( sp_est - sp2 ) / dsp_drtx;
    double rtxm     = 0.5 * ( rtx + rtx0 );
    dsp_drtx = a1 + ( 2 * a2 + ( 3 * a3 + ( 4 * a4 + 5 * a5 * rtxm ) * rtxm ) * rtxm ) * rtxm
               + ft68 * ( b1 + ( 2 * b2 + ( 3 * b3 + ( 4 * b4 + 5 * b5 * rtxm ) * rtxm ) * rtxm ) * rtxm );
    rtx      = rtx0 - ( sp_est - sp2 ) / dsp_drtx;

    // This is the end of one full iteration of the modified Newton-Raphson
    // iterative equation solver. The error in Rtx at this point is equivalent
    // to an error in SP of 9e-16 psu.
    double x                  = 400.0 * rtx * rtx;
    double sqrty              = 10.0 * rtx;
    double part1              = 1.0 + x * ( 1.5 + x );
    double part2              = 1.0 + sqrty * ( 1.0 + sqrty * ( 1.0 + sqrty ) );
    double sp_hill_raw_at_sp2 = sp2 - a0 / part1 - b0 * ft68 / part2;

    return ( 2.0 / sp_hill_raw_at_sp2 );
}

// Practical Salinity (PSS-78) from conductivity (mS/cm), temperature (deg C), and pressure (dbar)
double AuvMath::PracticalSalinity( const double& conductivity, const double& temperature, const double& pressure )
{
    const double c3515 = 42.9140;
    const double a0    =  0.0080;
    const double a1    = -0.1692;
    const double a2    = 25.3851;
    const double a3    = 14.0941;
    const double a4    = -7.0261;
    const double a5    =  2.7081;
    const double b0    =  0.0005;
    const double b1    = -0.0056;
    const double b2    = -0.0066;
    const double b3    = -0.0375;
    const double b4    =  0.0636;
    const double b5    = -0.0144;
    const double c0    =  0.6766097;
    const double c1    =  2.00564e-2;
    const double c2    =  1.104259e-4;
    const double c3    = -6.9698e-7;
    const double c4    =  1.0031e-9;
    const double d1    =  3.426e-2;
    const double d2    =  4.464e-4;
    const double d3    =  4.215e-1;
    const double d4    = -3.107e-3;
    const double e1    =  2.070e-5;
    const double e2    = -6.370e-10;
    const double e3    =  3.989e-15;
    const double k     =  0.0162;

    double t68  = temperature * 1.00024e0;
    double ft68 = ( t68 - 15e0 ) / ( 1e0 + k * ( t68 - 15e0 ) );

    // Dimensionless conductivity ratio (Culkin and Smith, 1980).
    double r = conductivity / c3515;

    // rt_lc corresponds to rt as defined in the UNESCO 44 (1983) routines.
    double rt_lc = c0 + ( c1 + ( c2 + ( c3 + c4 * t68 ) * t68 ) * t68 ) * t68;
    double rp    = 1e0 + ( pressure * ( e1 + e2 * pressure + e3 * pressure * pressure ) ) /
                   ( 1e0 + d1 * t68 + d2 * t68 * t68 + ( d3 + d4 * t68 ) * r );
    double rt    = r / ( rp * rt_lc );

    if( rt < 0.0 )
    {
        return ( nan( "" ) );
    }

    double rtx = sqrt( rt );
    double sp  = a0 + ( a1 + ( a2 + ( a3 + ( a4 + a5 * rtx ) * rtx ) * rtx ) * rtx ) * rtx +
                 ft68 * ( b0 + ( b1 + ( b2 + ( b3 + ( b4 + b5 * rtx ) * rtx ) * rtx ) * rtx ) * rtx );

    // Hill et al. (1986) algorithm.
    // This algorithm is adjusted so that it is exactly equal to the PSS-78 algorithm
    // at SP = 2.
    if( sp < 2 )
    {
        double hill_ratio  = HillRatioAtSP2( temperature );
        double x           = 400e0 * rt;
        double sqrty       = 10e0 * rtx;
        double part1       = 1e0 + x * ( 1.5e0 + x );
        double part2       = 1e0 + sqrty * ( 1e0 + sqrty * ( 1e0 + sqrty ) );
        double sp_hill_raw = sp - a0 / part1 - b0 * ft68 / part2;
        sp                 = hill_ratio * sp_hill_raw;
    }

    // SP is non-negative.
    if( sp < 0.0 )
    {
        sp  = nan( "" );
    }

    return ( sp );
}

double AuvMath::SoundSpeed( const double& salinity, const double& temperature, const double& pressure )
{
    //---------
    // BEGIN
    //--------

    double P = pressure / 10; // convert db to bars as used in UNESCO routines
    double T68 = temperature * 1.00024;

    //------------
    // eqn 34 p.46
    //------------
    const double c00 = 1402.388;
    const double c01 =    5.03711;
    const double c02 =   -5.80852e-2;
    const double c03 =    3.3420e-4;
    const double c04 =   -1.47800e-6;
    const double c05 =    3.1464e-9;

    const double c10 =  0.153563;
    const double c11 =  6.8982e-4;
    const double c12 = -8.1788e-6;
    const double c13 =  1.3621e-7;
    const double c14 = -6.1185e-10;

    const double c20 =  3.1260e-5;
    const double c21 = -1.7107e-6;
    const double c22 =  2.5974e-8;
    const double c23 = -2.5335e-10;
    const double c24 =  1.0405e-12;

    const double c30 = -9.7729e-9;
    const double c31 =  3.8504e-10;
    const double c32 = -2.3643e-12;

    double Cw = ( ( ( ( c32 * T68 + c31 ) * T68 + c30 ) * P +
                    ( ( ( ( c24 * T68 + c23 ) * T68 + c22 ) * T68 + c21 ) * T68 + c20 ) ) * P +
                  ( ( ( ( c14 * T68 + c13 ) * T68 + c12 ) * T68 + c11 ) * T68 + c10 ) ) * P +
                ( ( ( ( c05 * T68 + c04 ) * T68 + c03 ) * T68 + c02 ) * T68 + c01 ) * T68 + c00;

    //-------------
    // eqn 35. p.47
    //-------------
    const double a00 =  1.389;
    const double a01 = -1.262e-2;
    const double a02 =  7.164e-5;
    const double a03 =  2.006e-6;
    const double a04 = -3.21e-8;

    const double a10 =  9.4742e-5;
    const double a11 = -1.2580e-5;
    const double a12 = -6.4885e-8;
    const double a13 =  1.0507e-8;
    const double a14 = -2.0122e-10;

    const double a20 = -3.9064e-7;
    const double a21 =  9.1041e-9;
    const double a22 = -1.6002e-10;
    const double a23 =  7.988e-12;

    const double a30 =  1.100e-10;
    const double a31 =  6.649e-12;
    const double a32 = -3.389e-13;

    double A = ( ( ( ( a32 * T68 + a31 ) * T68 + a30 ) * P +
                   ( ( ( a23 * T68 + a22 ) * T68 + a21 ) * T68 + a20 ) ) * P +
                 ( ( ( ( a14 * T68 + a13 ) * T68 + a12 ) * T68 + a11 ) * T68 + a10 ) ) * P +
               ( ( ( a04 * T68 + a03 ) * T68 + a02 ) * T68 + a01 ) * T68 + a00;

    //------------
    // eqn 36 p.47
    //------------
    const double b00 = -1.922e-2;
    const double b01 = -4.42e-5;
    const double b10 =  7.3637e-5;
    const double b11 =  1.7945e-7;

    double B = b00 + b01 * T68 + ( b10 + b11 * T68 ) * P;

    //------------
    // eqn 37 p.47
    //------------
    const double d00 =  1.727e-3;
    const double d10 = -7.9836e-6;

    double D = d00 + d10 * P;

    //------------
    // eqn 33 p.46
    //------------
    double svel = Cw + A * salinity + B * salinity * sqrt( salinity ) + D * salinity * salinity;

    return svel;

}

// Oxygen Solubility (ml/l) from temperature (deg C) and salinity (psu)
double AuvMath::Oxsol( const double& temperature, const double& salinity )
{
    const double A0 = 2.00907;
    const double A1 = 3.22014;
    const double A2 = 4.0501;
    const double A3 = 4.94457;
    const double A4 = -0.256847;
    const double A5 = 3.88767;
    const double B0 = -0.00624523;
    const double B1 = -0.00737614;
    const double B2 = -0.010341;
    const double B3 = -0.00817083;
    const double C0 = -0.000000488682;

    double Ts = log( ( 298.15 - temperature ) / ( 273.15 + temperature ) );

    double Ts2 = pow( Ts, 2 );
    double Ts3 = pow( Ts, 3 );
    double Ts4 = pow( Ts, 4 );
    double Ts5 = pow( Ts, 5 );

    double A = A0 + A1 * Ts + A2 * Ts2 + A3 * Ts3 + A4 * Ts4 + A5 * Ts5;

    double B = B0 + B1 * Ts + B2 * Ts2 + B3 * Ts3;

    double Co = exp( A + salinity * B + C0 * pow( salinity, 2 ) );

    return Co;
}

bool AuvMath::AreaContains( float x, float y, int segments, float* x0, float* y0, float* x1, float* y1 )
{
    bool c = false;
    for( int i = 0; i < segments; ++i )
    {
        if( ( ( ( y1[i] <= y ) && ( y < y0[i] ) ) ||
                ( ( y0[i] <= y ) && ( y < y1[i] ) ) ) &&
                ( x < ( x0[i] - x1[i] ) * ( y - y1[i] ) / ( y0[i] - y1[i] ) + x1[i] ) )
            c = !c;
    }
    return c;
}

bool AuvMath::TriangleContains( float x, float y, float x0, float y0, float x1, float y1, float x2, float y2 )
{
    float xx0[] = { x0, x1, x2 };
    float yy0[] = { y0, y1, y2 };
    float xx1[] = { x1, x2, x0 };
    float yy1[] = { y1, y2, y0 };
    return AreaContains( x, y, 3, xx0, yy0, xx1, yy1 );
}