/* -*-c++-*- */
/* osgEarth - Dynamic map generation toolkit for OpenSceneGraph
 * Copyright 2008-2012 Pelican Mapping
 * http://osgearth.org
 *
 * osgEarth is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>
 */

#ifndef OSGEARTH_MATH_H
#define OSGEARTH_MATH_H 1

#include <osgEarth/Common>
#include <osgEarth/Geometry>
#include <osg/Quat>
#include <osg/Vec3>
#include <osg/BoundingBox>
#include <osg/Array>
#include <osg/Geometry>
#include <osg/Matrix>

#include <iterator>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace osgEarth
{
    struct Line2d;
    struct Segment2d;
    struct Ray2d;
    struct Segment3d;

    /** Infinite 2D line. */
    struct OSGEARTH_EXPORT Line2d
    {
        osg::Vec3d _a, _b; // two points on the line (Z ignored)
        Line2d() { }
        Line2d(const osg::Vec3d& a, const osg::Vec3d& b) : _a(a), _b(b) { }
        Line2d(const osg::Vec2d& a, const osg::Vec2d& b) : _a(a.x(), a.y(), 0), _b(b.x(), b.y(), 0) { }
        Line2d(const osg::Vec4d& a, const osg::Vec4d& b) : _a(a.x()/a.w(), a.y()/a.w(), a.z()/a.w()), _b(b.x()/b.w(), b.y()/b.w(), b.z()/b.w()) { }
        Line2d(const Line2d& rhs) : _a(rhs._a), _b(rhs._b) { }
        bool intersect( const Line2d&    rhs, osg::Vec2d& out ) const;
        bool intersect( const Ray2d&     rhs, osg::Vec2d& out ) const;
        bool intersect( const Segment2d& rhs, osg::Vec2d& out ) const;
        bool intersect( const Line2d&    rhs, osg::Vec3d& out ) const;
        bool intersect( const Ray2d&     rhs, osg::Vec3d& out ) const;
        bool intersect( const Segment2d& rhs, osg::Vec3d& out ) const;
        bool intersect( const Line2d&    rhs, osg::Vec4d& out ) const;
        bool isPointOnLeft( const osg::Vec2d& p ) const;
        bool isPointOnLeft( const osg::Vec3d& p ) const;
    };
    
    //// rotate a Line
    //inline Line2d operator* (const osg::Quat& q, const Line2d& rhs) {
    //    return Line( q * rhs._a, q * rhs._b );
    //}

    /** Endpoint and a direction vector */
    struct OSGEARTH_EXPORT Ray2d
    {
        osg::Vec3d _a;  // endpoint
        osg::Vec3d _dv; // directional vector
        Ray2d() { }
        Ray2d(const osg::Vec3d& a, const osg::Vec3d& dv) : _a(a), _dv(dv) { }
        Ray2d(const osg::Vec2d& a, const osg::Vec2d& dv) : _a(a.x(), a.y(), 0), _dv(dv.x(), dv.y(), 0) { }
        Ray2d(const Ray2d& rhs) : _a(rhs._a), _dv(rhs._dv) { }
        bool intersect( const Line2d&    rhs, osg::Vec2d& out ) const;
        bool intersect( const Ray2d&     rhs, osg::Vec2d& out ) const;
        bool intersect( const Segment2d& rhs, osg::Vec2d& out ) const;
        bool intersect( const Line2d&    rhs, osg::Vec3d& out ) const;
        bool intersect( const Ray2d&     rhs, osg::Vec3d& out ) const;
        bool intersect( const Segment2d& rhs, osg::Vec3d& out ) const;
        bool isPointOnLeft( const osg::Vec2d& p ) const;
        bool isPointOnLeft( const osg::Vec3d& p ) const;
        double angle(const Segment2d& rhs) const;
    };
    
    //// rotate a Ray
    //inline Ray operator* (const osg::Quat& q, const Ray& rhs) {
    //    return Ray( q * rhs._a, q * rhs._dv );
    //}

    /** Finite line between two endpoints */
    struct OSGEARTH_EXPORT Segment2d
    {
        osg::Vec3d _a, _b; // endpoints
        Segment2d() { }
        Segment2d(const osg::Vec3d& a, const osg::Vec3d& b) : _a(a), _b(b) { }
        Segment2d(const Segment2d& rhs) : _a(rhs._a), _b(rhs._b) { }
        bool intersect( const Line2d&    rhs, osg::Vec2d& out ) const;
        bool intersect( const Ray2d&     rhs, osg::Vec2d& out ) const;
        bool intersect( const Segment2d& rhs, osg::Vec2d& out ) const;
        bool intersect( const Line2d&    rhs, osg::Vec3d& out ) const;
        bool intersect( const Ray2d&     rhs, osg::Vec3d& out ) const;
        bool intersect( const Segment2d& rhs, osg::Vec3d& out ) const;
        bool isPointOnLeft( const osg::Vec2d& p ) const;
        bool isPointOnLeft( const osg::Vec3d& p ) const;
        Segment3d unrotateTo3D(const osg::Quat& q) const;
        double angle(const Segment2d& rhs) const;
        // Distance of point to segment; left side is positive
        double leftDistanceXY( const osg::Vec3d&p ) const
        {
            osg::Vec2d base(_b.x() - _a.x(), _b.y() - _a.y());
            // 2D cross product, 2 * area of triangle
            double cross = base.x() * (p.y() - _a.y()) - base.y() * (p.x() - _a.x());
            return cross / base.length();
        }
        double squaredDistanceTo(const osg::Vec3d& point) const;
        osg::Vec3d closestPointTo(const osg::Vec3d& point) const;
    };

    struct OSGEARTH_EXPORT Segment3d
    {
        osg::Vec3d _a, _b; // endpoints
        Segment3d() { }
        Segment3d(const osg::Vec3d& a, const osg::Vec3d& b) : _a(a), _b(b) { }
        Segment3d(const Segment3d& rhs) : _a(rhs._a), _b(rhs._b) { }
        Segment3d(const Segment2d& seg2d, const osg::Vec3& planeNormal);
        Segment2d rotateTo2D(const osg::Quat& q) { return Segment2d(q*_a, q*_b); }
    };
    
    //// rotate a Segment
    //inline Segment operator* (const osg::Quat& q, const Segment& rhs) {
    //    return Segment( q * rhs._a, q * rhs._b );
    //}

    /** 2D traingle with CCW winding. */
    struct OSGEARTH_EXPORT Triangle2d
    {
        osg::Vec3d _a, _b, _c;
        Triangle2d(const osg::Vec3d& a, const osg::Vec3d& b, const osg::Vec3d& c) : _a(a), _b(b), _c(c) { }
        bool contains(const osg::Vec3d& p) const;
    };

    // Work in progress. We want to use Ray without doing the work to
    // fully support the other types.

    struct Line;
    struct Ray;

#if 0
    /** Infinite 3D line. */
    struct Line
    {
        osg::Vec3d _a, _b; // two points on the line
        Line(const osg::Vec3d& a, const osg::Vec3d& b) : _a(a), _b(b) { }
        Line(const Line& rhs) : _a(rhs._a), _b(rhs._b) { }
        bool intersectXY( const Line&    rhs, osg::Vec3d& out ) const;
        bool intersectXY( const Ray&     rhs, osg::Vec3d& out ) const;
        bool intersectXY( const Segment& rhs, osg::Vec3d& out ) const;
    };
    
    // rotate a Line
    inline Line operator* (const osg::Quat& q, const Line& rhs) {
        return Line( q * rhs._a, q * rhs._b );
    }
#endif

    /** Endpoint and a direction vector */
    struct Ray
    {
        osg::Vec3d _a;  // endpoint
        osg::Vec3d _dv; // directional vector
        Ray(const osg::Vec3d& a, const osg::Vec3d& dv) : _a(a), _dv(dv) { }
        Ray(const Ray& rhs) : _a(rhs._a), _dv(rhs._dv) { }
#if 0
        bool intersectXY( const Line&    rhs, osg::Vec3d& out ) const;
        bool intersectXY( const Ray&     rhs, osg::Vec3d& out ) const;
        bool intersectXY( const Segment& rhs, osg::Vec3d& out ) const;
        bool isPointOnLeftXY( const osg::Vec3d& p ) const;
#endif
    };
    
    // rotate a Ray
    inline Ray operator* (const osg::Quat& q, const Ray& rhs) {
        return Ray( q * rhs._a, q * rhs._dv );
    }

#if 0
    /** Finite line between two endpoints */
    // This will need a new name before it's enabled; conflicts with a
    // type in osgEarth/Geometry.
    struct Segment
    {
        osg::Vec3d _a, _b; // endpoints
        Segment(const osg::Vec3d& a, const osg::Vec3d& b) : _a(a), _b(b) { }
        Segment(const Segment& rhs) : _a(rhs._a), _b(rhs._b) { }
        bool intersectXY( const Line&    rhs, osg::Vec3d& out ) const;
        bool intersectXY( const Ray&     rhs, osg::Vec3d& out ) const;
        bool intersectXY( const Segment& rhs, osg::Vec3d& out ) const;
        bool isPointOnLeftXY( const osg::Vec3d& p ) const;
    };
    
    // rotate a Segment
    inline Segment operator* (const osg::Quat& q, const Segment& rhs) {
        return Segment( q * rhs._a, q * rhs._b );
    }
#endif

    // Utility functions for arrays of points treated as a 2d polygon
    OSGEARTH_EXPORT osg::BoundingBoxd polygonBBox2d(const osg::Vec3dArray& points);

    template<typename Iterator>
    osg::BoundingBoxd polygonBBox2d(Iterator begin, Iterator end)
{
    osg::BoundingBoxd result(DBL_MAX, DBL_MAX, DBL_MAX, -DBL_MAX, -DBL_MAX, -DBL_MAX);
    for (Iterator itr = begin; itr != end; ++itr)
    {
        result.xMin() = std::min(result.xMin(), (*itr).x());
        result.xMax() = std::max(result.xMax(), (*itr).x());
        result.yMin() = std::min(result.yMin(), (*itr).y());
        result.yMax() = std::max(result.yMax(), (*itr).y());
    }
    return result;
}
    OSGEARTH_EXPORT bool pointInPoly2d(const osg::Vec3d& pt, const Polygon& polyPoints,
                                       double tolerance = 0.0);
    OSGEARTH_EXPORT bool pointInPoly2d(const osg::Vec3d& pt, const osg::Geometry* polyPoints,
                                       float tolerance = 0.0f);

    // Winding number test; see http://geomalgorithms.com/a03-_inclusion.html
    template<typename Pt, typename PtItr>
    bool
    pointInPoly2d(const Pt& pt, const PtItr& begin, const PtItr& end, double tolerance = 0.0)
    {
        int windingNum = 0;

        for (PtItr itr = begin; itr != end; ++itr)
        {
            Segment2d seg = (std::next(itr) == end
                             ? Segment2d(*itr, *begin)
                             : Segment2d(*itr, *std::next(itr)));
            // if the segment is horizontal, then the "is left" test
            // isn't meaningful. We count the point as in if it's on or
            // to the left of the segment.


            if (seg._a.y() == seg._b.y() && fabs(pt.y() - seg._a.y()) <= tolerance)
            {
                if (pt.x() < seg._a.x() || pt.x() < seg._b.x())
                {
                    windingNum++;
                }
            }
            else if (seg._a.y() <= pt.y())
            {
                if (seg._b.y() > pt.y())
                {
                    double dist = seg.leftDistanceXY(pt);
                    if (dist > -tolerance)
                    {
                        windingNum++;
                    }
                }
            }
            else if (seg._b.y() <= pt.y())
            {
                double dist = seg.leftDistanceXY(pt);
                if (dist < tolerance)
                {
                    windingNum--;
                }
            }
        }
        return windingNum != 0;
    }

    template<typename T>
    inline T deg2rad(T v) {
        return v * static_cast<T>(M_PI)/static_cast<T>(180.0);
    }

    template<typename T>
    inline T rad2deg(T v) {
        return v * static_cast<T>(180.0) / static_cast<T>(M_PI);
    }

    template<typename T>
    inline T step(const T& edge, const T& x)
    {
        return x < edge ? static_cast<T>(0.0) : static_cast<T>(1.0);
    }
    
    template<typename T>
    inline T clamp(const T& x, const T& lo, const T& hi)
    {
        return x<lo ? lo : x>hi ? hi : x;
    }

    template<typename T>
    inline T lerpstep(T lo, T hi, T x)
    {
        if (x <= lo) return static_cast<T>(0.0);
        else if (x >= hi) return static_cast<T>(1.0);
        else return (x - lo) / (hi - lo);
    }

    template<typename T>
    inline T smoothstep(T lo, T hi, T x)
    {
        T t = clamp((x - lo) / (hi - lo), static_cast<T>(0.0), static_cast<T>(1.0));
        return t * t*(static_cast<T>(3.0) - static_cast<T>(2.0)*t);
    }

    // move closer to one
    template<typename T>
    inline T harden(T x)
    {
        return static_cast<T>(1.0) - (static_cast<T>(1.0) - x)*(static_cast<T>(1.0) - x);
    }
    template<typename T>
    inline T decel(T x) { return harden(x); }

    // move closer to zero
    template<typename T>
    inline T soften(T x)
    {
        return x * x;
    }
    template<typename T>
    inline T accel(T x) { return soften(x); }

    template<typename T>
    inline T threshold(T x, T thresh, T buf)
    {
        if (x < thresh - buf) return static_cast<T>(0.0);
        else if (x > thresh + buf) return static_cast<T>(1.0);
        else return clamp((x - (thresh - buf)) / (buf*static_cast<T>(2.0)), static_cast<T>(0.0), static_cast<T>(1.0));
    }

    template<typename T>
    inline T fract(T x)
    {
        return x - floor(x);
    }

    template<typename T>
    inline double unitremap(T a, T lo, T hi)
    {
        return clamp((a - lo) / (hi - lo), static_cast<T>(0.0), static_cast<T>(1.0));
    }

    template<typename T>
    inline T mix(const T& a, const T& b, float c)
    {
        return a * (1.0 - c) + b * c;
    }

    template<typename T>
    inline double dot2D(const T& a, const T& b)
    {        
        return a.x()*b.x() + a.y()*b.y();
    }

    template<typename T>
    inline double dot3D(const T& a, const T& b)
    {
        return a.x()*b.x() + a.y()*b.y() + a.z()*b.z();
    }

    template<typename T>
    inline double distanceSquared2D(const T& a, const T& b)
    {
        return 
            (b.x() - a.x())*(b.x() - a.x()) + 
            (b.y() - a.y())*(b.y() - a.y());
    }

    template<typename T>
    inline double distanceSquared3D(const T& a, const T& b)
    {
        return
            (b.x() - a.x())*(b.x() - a.x()) +
            (b.y() - a.y())*(b.y() - a.y()) +
            (b.z() - a.z())*(b.z() - a.z());
    }

    template<typename T>
    inline double distance2D(const T& a, const T& b)
    {
        return sqrt(distanceSquared2D(a, b));
    }

    template<typename T>
    inline double distance3D(const T& a, const T& b)
    {
        return sqrt(distanceSquared3D(a, b));
    }
    template<typename T>
    inline T normalize(const T& a)
    {
        T temp = a;
        temp.normalize();
        return temp;
    }
    // Newton-Raphson solver
    template<typename Func, typename FuncDeriv>
    double solve(Func func, FuncDeriv deriv, double guess, double tolerance, bool& valid, int maxIterations = 16)
    {
        double xn = guess;
        for (int i = 0; i <= maxIterations; ++i)
        {
            double f = func(xn);
            if (fabs(f) <= tolerance)
            {
                valid = true;
                return xn;
            }
            xn = xn - f / deriv(xn);
        }
        valid = false;
        return xn;
    }

    // Courtesy of stackoverflow, return -1, 0, +1 based on the sign
    // of a number
    template<typename T>
    int sgn(T val)
    {
        return (T(0) < val) - (val < T(0));
    }
    // Bisection solver, useful when the derivative of a function is
    // unknown or expensive to calculate.
    // x0 and x1 are initial guesses surrounding the root. The signs
    // of func(x0) and func(x1) should be different; otherwise we bail
    // immediately.
    template<typename Func>
    double solveBisect(const Func& func, double x0, double x1, double tolerance, int maxIterations = 8)
    {
        double f0 = func(x0);
        double f1 =func(x1);
        if (sgn(f0) == sgn(f1))
        {
            return x0;
        }
        double midPoint = 0.0;
        for (int i = 0; i < maxIterations; ++i)
        {
            midPoint = (x0 + x1) / 2.0;
            double fMidpoint = func(midPoint);
            if (fabs(fMidpoint) <= tolerance)
            {
                return midPoint;
            }
            else if (sgn(f0) == sgn(fMidpoint))
            {
                x0 = midPoint;
                f0 = fMidpoint;
            }
            else
            {
                x1 = midPoint;
                f1 = fMidpoint;
            }
        }
        return midPoint;
    }

    // Project osg::Vec3 a onto b.
    template<typename VecType>
    VecType vecProjection(const VecType& a, const VecType& b)
    {
        return b * ((a * b) / (b * b));
    }

    // Project osg::Vec3 a onto the plane perpendicular to b.
    template<typename VecType>
    VecType vecRejection(const VecType& a, const VecType& b)
    {
        return a - vecProjection(a, b);
    }

    // Round integral x to the nearest multiple of "multiple" greater than or equal to x
    template<typename T>
    T align(T x, T multiple) {
        T isPositive = (T)(x >= 0);
        return ((x + isPositive * (multiple - 1)) / multiple) * multiple;
    }

    // equal within a default threshold
    template<typename T>
    bool equivalent(T x, T y) {
        return osg::equivalent(x, y);
    }

    // equal within a threshold
    template<typename T>
    bool equivalent(T x, T y, T epsilon) {
        return osg::equivalent(x, y, epsilon);
    }

    inline int nextPowerOf2(int x)
    {
        --x;
        x |= x >> 1;
        x |= x >> 2;
        x |= x >> 4;
        x |= x >> 8;
        x |= x >> 16;
        return x + 1;
    }

    // Adapted from Boost - see boost license
    // https://www.boost.org/users/license.html
    template <typename T> inline std::size_t hash_value_unsigned(T val) {
        const int size_t_bits = std::numeric_limits<std::size_t>::digits;
        const int length = (std::numeric_limits<T>::digits - 1) / size_t_bits;
        std::size_t seed = 0;
        for(unsigned int i = length * size_t_bits; i > 0; i -= size_t_bits)
            seed ^= (std::size_t) (val >> i) + (seed<<6) + (seed>>2);
        seed ^= (std::size_t) val + (seed<<6) + (seed>>2);
        return seed;
    }

    inline std::size_t hash_value_unsigned(bool val) {
        return hash_value_unsigned((unsigned)val ? 0x1111111 : 0x2222222);
    }

    template<typename T>
    inline std::size_t hash_value_unsigned(const optional<T>& val) {
        if (val.isSet())
            return hash_value_unsigned(0x3333333u, val.get());
        else
            return (std::size_t)0;
    }

    template <typename A, typename B> inline std::size_t hash_value_unsigned(A a, B b) {
        std::size_t seed = hash_value_unsigned(a);
        seed ^= hash_value_unsigned(b) + 0x9e3779b9 + (seed<<6) + (seed>>2);
        return seed;
    }

    template <typename A, typename B, typename C> inline std::size_t hash_value_unsigned(A a, B b, C c) {
        std::size_t seed = hash_value_unsigned(a);
        seed ^= hash_value_unsigned(b) + 0x9e3779b9 + (seed<<6) + (seed>>2);
        seed ^= hash_value_unsigned(c) + 0x9e3779b9 + (seed<<6) + (seed>>2);
        return seed;
    }

    template <typename A, typename B, typename C, typename D> inline std::size_t hash_value_unsigned(A a, B b, C c, D d) {
        std::size_t seed = hash_value_unsigned(a);
        seed ^= hash_value_unsigned(b) + 0x9e3779b9 + (seed<<6) + (seed>>2);
        seed ^= hash_value_unsigned(c) + 0x9e3779b9 + (seed<<6) + (seed>>2);
        seed ^= hash_value_unsigned(d) + 0x9e3779b9 + (seed<<6) + (seed>>2);
        return seed;
    }

    template <typename A, typename B, typename C, typename D, typename E> inline std::size_t hash_value_unsigned(A a, B b, C c, D d, E e) {
        std::size_t seed = hash_value_unsigned(a);
        seed ^= hash_value_unsigned(b) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
        seed ^= hash_value_unsigned(c) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
        seed ^= hash_value_unsigned(d) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
        seed ^= hash_value_unsigned(e) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
        return seed;
    }

    // https://github.com/matteo65/mzHash64 (Public Domain)
    inline std::size_t hash_string(const std::string& input, std::size_t seed = 0) {
        auto hash = seed ^ 0xD45E69F901E72147L;
        auto len = input.length();
        for(std::size_t i=0; i<len; ++i)
            hash = 0x3631754B22FF2D5CL * (i + input[i]) ^ (hash << 2) ^ (hash >> 2);
            return hash;
    };

    /**
     * Utilities to manipulate a projection matrix.
     * These functions will automatically handle standard OGL versus Reverse-Z
     * projection matrices.
     */
    struct OSGEARTH_EXPORT ProjectionMatrix
    {
        //! Projection matrix type
        using Type = enum {
            STANDARD,
            REVERSE_Z,
            UNKNOWN
        };

        //! True for an orthographic projection matrix
        static inline bool isOrtho(const osg::Matrix& m) {
            return !m.isIdentity() && m(3, 3) > 0.0;
        }

        //! True for a perspective projection matrix
        static inline bool isPerspective(const osg::Matrix& m) {
            return m(3, 3) == 0.0;
        }

        //! Detected type of the projection matrix
        static Type getType(const osg::Matrix& m);

        //! Construct a perspective matrix, deriving the type from the
        //! existing values in the matrix if possible
        static void setPerspective(
            osg::Matrix& m,
            double vfov, double ar, double N, double F,
            Type = UNKNOWN);

        //! Extract values from a perspective matrix
        static bool getPerspective(
            const osg::Matrix& m,
            double& vfov, double& ar, double& N, double& F);

        //! Extract raw frustum values from a perspective matrix
        static bool getPerspective(
            const osg::Matrix& m,
            double& L, double& R, double& B, double& T, double& N, double& F);

        //! Construct an orthographc matrix, deriving the type from the
        //! existing values in the matrix if possible
        static void setOrtho(
            osg::Matrix& m,
            double L, double R, double B, double T, double N, double F,
            Type = UNKNOWN);

        //! Extract frustum values from an orthographic matrix
        static bool getOrtho(
            const osg::Matrix& m,
            double& L, double& R, double& B, double& T, double& N, double& F);
    };

}
#endif