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

/**
 *  \defgroup utils Utilities
 *
 *  Utility classes (see http://tellum/vectorcast/utils.html for test results)
 */

#ifndef AUVMATH_H_
#define AUVMATH_H_

#include <math.h>
#include "data/Point3D.h"
#include "data/Matrix3x3.h"

#ifndef M_2PI
#   define M_2PI       (6.28318530717958647692)
#endif

#ifndef M_1_2PI
#   define M_1_2PI     (0.15915494309189533577)
#endif

#ifndef UINT16_MAX
#   define UINT16_MAX  (65535)
#endif

#ifndef UINT32_MAX
#   define UINT32_MAX (4294967295U)
#endif

/// A macro for converting degrees to radians
#ifndef D2R
#define D2R(x) ( ( x ) * M_PI / 180.0 )
#endif

/// A macro for converting radians to degrees
#define R2D(x) ( ( x ) * 180.0 / M_PI)

/// Minimum of two values -- also available as AuvMath::Min() template function
/// If either argument is a NaN, will return NaN
#ifndef MIN
#   define MIN(a,b) ( ( a ) != ( a ) ? ( a ) : ( b ) != ( b ) ? ( b ) : ( a ) <= ( b ) ? ( a ) : ( a ) > ( b ) ? ( b ) : ( a ) )
#endif

/// Maximum of two values -- also available as AuvMath::Max() template function
/// If either argument is a NaN, will return NaN
#ifndef MAX
#   define MAX(a,b) ( ( a ) != ( a ) ? ( a ) : ( b ) != ( b ) ? ( b ) : ( a ) >= ( b ) ? ( a ) : ( a ) < ( b ) ? ( b ) : ( a ) )
#endif

/**
 *  Contains useful math-related utilities.
 *  Originally called Math.h, but could be confused with math.h
 *
 *  \ingroup utils
 */
class AuvMath
{
public:

    // 2 times pi
#ifdef __GAZEBO
    static constexpr double TWOPI = M_PI * 2.0;
#else
    static const double TWOPI = M_PI * 2.0;
#endif
    static const double DEG_TO_RAD;
    static const double RAD_TO_DEG;

    /// Makes sure a variable falls within defined limits...
    /// Either limit may be min or max.
    /// If any input is nan, nan is returned
    /// %Unit tests in AuvMath_Test.testLimit
    template <typename T>
    static T Limit( const T& value, const T& limit1, const T& limit2 );

    /// Tests if a variable falls within defined limits...
    /// Either limit may be min or max.
    /// If any input is nan, false is returned
    /// %Unit tests in AuvMath_Test.testLimit
    template <typename T>
    static bool InLimit( const T& value, const T& limit1, const T& limit2 );

    /// Returns the lesser of the two arguments
    /// If either input is nan, nan is returned
    /// %Unit tests in AuvMath_Test.testMin
    template <typename T>
    inline static T Min( const T& a, const T& b )
    {
        return MIN( a, b );
    }

    /// Returns the greater of the two arguments
    /// If either input is nan, nan is returned
    /// %Unit tests in AuvMath_Test.testMin
    template <typename T>
    inline static T Max( const T& a, const T& b )
    {
        return MAX( a, b );
    }

    /// Square function (opposite of sqrt(T))
    template <typename T>
    static T Square( T number )
    {
        return number * number;
    }

    /// Swaps the two arguments
    template <typename T>
    inline static void Swap( T& a, T& b )
    {
        T t( a );
        a = b;
        b = t;
    }

    /// Does not test the endianness of the input value,
    /// Just flips it.
    template<typename T>
    static T FlipEndian( T value );

    /// Makes sure a variable is divisible by specified divisor...
    /// If any input is nan, nan is returned
    /// %Unit tests in AuvMath_Test.testRound
    template <typename T>
    static T Round( const T& value, const T& divisor );

    /// Returns signum function:
    /// If input is nan, nan is returned
    /// %Unit tests in AuvMath_Test.testSign
    template <typename T>
    static T Sign( const T& x );

    /// modulo Pi function:
    /// If input is nan, nan is returned
    /// %Unit tests in AuvMath_Test.testModPi
    template <typename T>
    static T ModPi( const T& angle );

    /// Normalizes the angle to be 0 to 2*M_PI
    /// If input is nan, nan is returned
    /// %Unit tests in AuvMath_Test.testModPi
    template <typename T>
    static T NormalizeAngle( const T& value, const T& start = 0, const T& end = M_2PI );

    /// Casts nan values to zero -- otherwise returns the value
    /// %Unit tests in AuvMath_Test.testNanZero
    template <typename T>
    static T NanZero( const T& value );

    /// Finds the index within an array where the value is <= the supplied value
    /// Searches from indexMin to indexMax within the array.
    /// Assumes the array is in increasing order.
    /// %Unit tests in AuvMath_Test.testFindLtEqIndex
    template <typename T>
    static int FindLtEqIndex( const T* values, const T& value,
                              const unsigned int indexMin, const unsigned int indexMax );

    /// Examines a 2 element data field for the value, where x0 <= x <= x1
    /// %Unit tests in AuvMath_Test.testInterpolate1D
    template <typename T>
    static T Interpolate1D( const T& x, const T& data0, const T& data1,
                            const T& x0, const T& x1 );

    /// Examines a 2x2 data field for the value, where x0 <= x <= x1,
    ///   y0 <= y <= y1,
    /// %Unit tests in AuvMath_Test.testInterpolate2D
    template <typename T>
    static T Interpolate2D( const T& x, const T& y, T data[][ 2 ],
                            const T& x0, const T& x1, const T& y0, const T& y1 );

    /// Examines a 2x2x2 data field for the value, where x0 <= x <= x1,
    ///   y0 <= y <= y1, z0 <= z <= z1
    /// %Unit tests in AuvMath_Test.testInterpolate3D
    template <typename T>
    static T 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 );

    /// Examines a 2x2x2x2 data field for the value, where w0 <= w <= w1,
    ///   x0 <= x <= x1, y0 <= y <= y1, z0 <= z <= z1
    /// %Unit tests in AuvMath_Test.testInterpolate4D
    template <typename T>
    static T 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 );

    /// Extracts azimuth and elevation angles in radians from a 3D unit vector
    template <typename T>
    static void UnitVectorToAzimuthAndElevation( Point3D &uhat, T &azimuth, T &elevation );

    /// Converts azimuth and elevation angles in radians to a 3D unit vector
    template <typename T>
    static void AzimuthAndElevationToUnitVector( T &azimuth, T &elevation, Point3D &uhat );

    /** Seawater bulk modulus (S,T,P)
     *
     * UNESCO Tech Paper Mar Sci 44 (1983)
     *
     * Use As:  bulkmod_(S,T,P)
     *
     * Input:   salinity (psu)
     *          temperature (K)
     *          pressure (Pa)
     *
     * Output:  Bulk Modulus in Pa
     *
     * Example: BulkModulus(35,283.15,4e7) -> 2.404604869e9
     *          BulkModulus(35,298.15,1e8)-> 2.710894504e9   UNESCO 36 p19
     *
     * Copyright (c) 1996 by Moss Landing Marine Laboratories
     * Separated out from DENSITY_83
     * 15 February 1992;  W. Broenkow
     */
    /// %Unit tests in AuvMath_Test.testBulkModulus
    static double BulkModulus( double salinity, double temperature, double pressure );


    // Simple conversion from binary representation to decimal
    static int binaryToDecimal( int n );


    /**
     * Seawater density (S,T,P)
     *
     * International Equation of State of Seawater (1980)
     * UNESCO Tech Paper Mar Sci 44 (1983)
     *
     * Use As:   density_(S,T,P)
     *
     * Input:    S = Salinity (psu)
     *           T = Temperature (K)
     *           P = Pressure (Pa)
     * Output:   density (kg/m3)
     *
     * Example:  Density(34.567,278.15,2e7) -> 1036.409
     *           Density(35,298.15,1e8)     -> 1062.53817  UNESCO 44 p19
     *
     * Copyright (c) 1996 by Moss Landing Marine Laboratories
     * William Broenkow
     * Checks perfectly with UNESCO 44.
     * This function requires Bulkmod
     */
    /// %Unit tests in AuvMath_Test.testDensity
    static double Density( double salinity, double temperature, double pressure );

    /**
     * OceanDepth (pressure, latitude)
     *
     * Use As:  OceanDepth(pressure, latitude)
     *
     * Input:   pressure (Pa),
     *          latitude (radians)
     * Output:  Depth (m)
     *
     * Example: Depth(5e7,0.628318531) -> 4906.08
     *          Depth(1e8,1.570796327) -> 9674.23  UNESCO 44 p28
     *
     * Note:    For more accurate results an additional factor of the
     *          ratio of the actual geopotential anomaly/gravity must
     *          be added. This correction will be less than 2 m.
     *
     * Ref:  UNESCO Tech Paper Mar Sci 44 (1983)
     * Copyright (c) 1996 by Moss Landing Marine Laboratories
     * 17 Jan 1996; W. Broenkow Matlabized from UNESCO83.FOR
     */
    /// %Unit tests in AuvMath_Test.testOceanDepth
    static double OceanDepth( const double& pressure, const double& latitude );

    /**
     * OceanPressure (depth, latitude)
     *
     * Use As:  OceanPressure(depth, latitude)
     *
     * Input:   depth (m),
     *          latitude (radians)
     * Output:  pressure (Pa)
     *
     * Example: OceanPressure(4906.08,0.628318531) -> 5e7
     *          OceanPressure(9674.23,1.570796327) -> 1e8  UNESCO 44 p28
     *
     * Ref:  UNESCO Tech Paper Mar Sci 44 (1983)
     */
    /// %Unit tests in AuvMath_Test.testOceanPressure
    static double OceanPressure( const double& depth, const double& latitude );

    /** HillRatioAtSP2
     *
     * Hill ratio at a Practical Salinity of 2
     *
     * Use As:  HillRatioAtSP2(temperature)
     *
     * Input:   temperature ITS-90 (deg C)
     *
     * Output:  Hill ratio (dimensionless)
     *
     * Example: HillRatioAtSP2(10.0) -> 0.999959
     *          HillRatioAtSP2(2.5) -> 0.999871
     *
     * Note:    Calculates the Hill ratio, which is the adjustment needed to apply
     *          for Practical Salinities smaller than 2. This ratio is defined at a
     *          Practical Salinity = 2 and in-situ temperature using PSS-78. The
     *          Hill ratio is the ratio of 2 to the output of the Hill et al. (1986)
     *          formula for Practical Salinity at the conductivity ratio, Rt, at which
     *          Practical Salinity on the PSS-78 scale is exactly 2.
     *
     * Author:  Trevor McDougall and Paul Barker (help@teos-10.org)
     *
     * Ref:     Hill, K.D., T.M. Dauphinee and D.J. Woods, 1986: The extension of the
     *          Practical Salinity Scale 1978 to low salinities. IEEE J. Oceanic Eng.,
     *          OE-11, 1, 109 - 112.
     */
    /// %Unit tests in AuvMath_Test.testHillRatioAtSP2
    static double HillRatioAtSP2( const double&  temperature );

    /** PracticalSalinity
     *
     * Calculates practical salinity from conductivity primarily using
     * the PSS-78 algorithm.
     *
     * Use As:  PracticalSalinity(conductivity, temperature, pressure)
     *
     * Input:   conductivity        [mS/cm]
     *          in-situ temperature [deg C  (ITS-90)]
     *          sea pressure        [dbar]
     *
     * Output:  practical salinity  [psu    (PSS-78)]
     *
     * Example: PracticalSalinity(22.0, 2.5, 10.0) -> 23.8149
     *          PracticalSalinity(39.9516, 9.4867, 1174.49) -> 36.8879
     *          PracticalSalinity(3.5, 25.0, 0.0) -> 1.83477
     *
     * Note:    Note that the PSS-78 algorithm for Practical
     *          Salinity is only valid in the range 2 < SP < 42. If the PSS-78
     *          algorithm produces a Practical Salinity that is less than 2 then the
     *          Practical Salinity is recalculated with a modified form of the Hill et
     *          al. (1986) formula. The modification of the Hill et al. (1986)
     *          expression is to ensure that it is exactly consistent with PSS-78
     *          at SP = 2. Note that the input values of conductivity need to be in
     *          units of mS/cm (not S/m).
     *
     * Authors: Trevor McDougall and Paul Barker (help@teos-10.org)
     *
     * Ref:     Hill, K.D., T.M. Dauphinee and D.J. Woods, 1986: The extension of the
     *          Practical Salinity Scale 1978 to low salinities. IEEE J. Oceanic Eng.,
     *          OE-11, 1, 109 - 112.
     *
     *          IOC, SCOR and IAPSO, 2010: The international thermodynamic equation of
     *          seawater - 2010: Calculation and use of thermodynamic properties.
     *          Intergovernmental Oceanographic Commission, Manuals and Guides No. 56,
     *          UNESCO (English), 196 pp.  Available from the TEOS-10 web site.
     *          See appendix E of the TEOS-10 Manual.
     *
     *          Unesco, 1983: Algorithms for computation of fundamental properties of
     *          seawater. Unesco Technical Papers in Marine Science, 44, 53 pp.
     */
    /// %Unit tests in AuvMath_Test.testPracticalSalinity
    static double PracticalSalinity( const double& conductivity, const double& temperature, const double& pressure );

    /**
     * Sound speed in sea water using UNESCO 1983 polynomial.
     *
     * USAGE:  speed = AUVMath::SoundSpeed(salinity,temperature,pressure)
     *
     * Input: salinity    [psu      (PSS-78)]
     *        temperature [degree C (ITS-90)]
     *        pressure    [dBar]
     *
     * Output: sound speed [m/s]
     *
     * Author: Phil Morgan 93-04-20, Lindsay Pender (Lindsay.Pender@csiro.au)
     *         03-12-12. Lindsay Pender, Converted to ITS-90.
     *
     * Ref: Fofonoff, P. and Millard, R.C. Jr
     *      Unesco 1983. Algorithms for computation of fundamental properties of
     *      seawater, 1983. _Unesco Tech. Pap. in Mar. Sci._, No. 44, 53 pp.
     */
    static double SoundSpeed( const double& salinity, const double& temperature, const double& pressure );

    /** Oxsol
     *
     * Oxygen saturation value after Garcia and Gordon (1992)
     *
     * USAGE:  speed = AUVMath::Oxsol(temperature,salinity)
     *
     * Input:  temperature [deg C (ITS-90)]
     *         salinity    [psu   (PSS-78)]
     *
     * Output: oxygen saturation value [ml/l]
     *
     * Note:   The output is the volume of oxygen gas at standard temperature and
     *         pressure conditions (STP) absorbed from humidity-saturated air at a
     *         total pressure of one atmosphere, per unit volume of the liquid at the
     *         temperature of measurement.
     *
     * Ref:    Garcia and Gordon (1992) "Oxygen solubility in seawater: Better
     *         fitting equations", Limnology & Oceanography, vol 37(6), p1307-1312.
     *
     *         SBE APPLICATION NOTE NO. 64: SBE 43 Dissolved Oxygen Sensor -
     *         Background Information, Deployment Recommendations, and Cleaning and
     *         Storage. Appendix A, 8 pp.
     *
     */
    static double Oxsol( const double& temperature, const double& salinity );

    //
    // Simple sort routine, invented by Donald Shell 1959. It is 0(n^2).
    // The code is short, and appropriate for small arrays here.
    //
    template <typename T>
    static void ShellSort( T array[], int arraySize )
    {
        int i, j, increment;
        T temp;

        increment = 3;
        while( increment > 0 )
        {
            for( i = 0; i < arraySize; i++ )
            {
                j = i;
                temp = array[i];
                while( ( j >= increment ) && ( array[j - increment] > temp ) )
                {
                    array[j] = array[j - increment];
                    j = j - increment;
                }
                array[j] = temp;
            }
            if( increment / 2 != 0 )
                increment = increment / 2;
            else if( increment == 1 )
                increment = 0;
            else
                increment = 1;
        }
    }

    static bool AreaContains( float x, float y, int segments,
                              float* x0, float* y0, float* x1, float* y1 );

    static bool TriangleContains( float x, float y,
                                  float x0, float y0, float x1, float y1,
                                  float x2, float y2 );

};

#endif /*AUVMATH_H_*/