#ifndef Foundation_Math_poly_INCLUDED
#define Foundation_Math_poly_INCLUDED

#include <list>
#include <vector>
#include <deque>

#include <foundation/SharedPtr.h>

#include <foundation/Math/Point.hpp>
#include <foundation/Math/Vec.hpp>
#include <foundation/Math/Sign.hpp>

#include <foundation/Math/misc.hpp>

namespace Tn {

namespace math {

    
/*! Do two line segments intersect? */
    
template <typename T>
std::pair< Point<3,T>,Point<3,T> > FindBounds(typename std::vector< Point<3,T> > const & geometry)
{
    using namespace Tn::math;
    
    if (geometry.size() > 0)
    {
        Point<3,T> p0(geometry[0]);
        Point<3,T> p1(geometry[0]);
        
        for (uint32_t i = 0; i<geometry.size(); ++i)
        {
            Point<3,T> const & p = geometry[i];
            for (int j = 0; j<3; ++j)
            {
                if (p[j] < p0[j]) p0[j] = p[j];
                if (p[j] > p1[j]) p1[j] = p[j];
            }
            ++i;
        }
        
        return std::pair< Point<3,T>,Point<3,T> >(p0, p1);
    }
    else
    {
        assert(false);
        Point3<T> zeroPoint;
        zeroPoint.zero();
        return std::pair< Point<3,T>,Point<3,T> >(zeroPoint, zeroPoint);

    }
}
    
template <typename T>
std::pair< Point<3,T>,Point<3,T> > FindBounds(typename std::deque< Point<3,T> > const & geometry)
{
    using namespace Tn::math;
    
    if (geometry.size() > 0)
    {
        Point<3,T> p0(geometry[0]);
        Point<3,T> p1(geometry[0]);
        
        for (size_t i = 0; i<geometry.size(); ++i)
        {
            Point<3,T> const & p = geometry[i];
            for (size_t j = 0; j<3; ++j)
            {
                if (p[j] < p0[j]) p0[j] = p[j];
                if (p[j] > p1[j]) p1[j] = p[j];
            }
            ++i;
        }
        
        return std::pair< Point<3,T>,Point<3,T> >(p0, p1);
    }
    else
    {
        assert(false);
        Point<3,T> zeroPoint;
        return std::pair< Point<3,T>,Point<3,T> >(zeroPoint, zeroPoint);
        
    }
}

template <typename T>
bool ccw(Point<2,T> const & a,
         Point<2,T> const & b,
         Point<2,T> const & c)
{
    return (cross(b-a,c-a) > T(0));
}

template <typename T>
bool intersects(Point<2,T> const & a,
                Point<2,T> const & b,
                Point<2,T> const & c,
                Point<2,T> const & d)
{
    return ((ccw(a,c,d) != ccw(b,c,d)) && 
            (ccw(a,b,c) != ccw(a,b,d)));
}

template <typename T>
bool surrounds(Point<2,T> const & point,
               Point<2,T> const & p0,
               Point<2,T> const & p1,
               Point<2,T> const & p2,
               Point<2,T> const & p3)
{
    Sign s = sign(cross(p1-p0, point-p0));

    return 
        (s == sign(cross(p2-p1, point-p1))) &&
        (s == sign(cross(p3-p2, point-p2))) &&
        (s == sign(cross(p0-p3, point-p3)));
}


template <typename Point>
typename Point::component_type length(std::deque<Point> const & points) {
    typedef typename Point::component_type T;
    
    T length = 0.0;

    typename std::deque<Point>::const_iterator i = points.begin();
    if (i != points.end()) {

        Point last_p = *i;
        ++i;
        while (i != points.end()) {

            Point p = *i;
            length += magnitude(p - last_p);
            last_p = p;
            ++i;
        }
    }
    return length;
}


// Finds a point along an edge a specified distance.

template <int I, typename T>
Point<I,T> lerp(T length, std::vector<Point<I,T> > const & points)
{
    T to_go = length;

    typename std::vector<Point<I,T> >::const_iterator i = points.begin();
    if (i != points.end()) {

        Point<I,T> prev_p = *i++;

        // If to_go before first point, return first point;
        if (to_go < T(0)) return prev_p;

        if (i != points.end()) {
            
            Point<I,T> p = *i++;

            T seg_len = magnitude(p - prev_p);
            while ((i != points.end()) && (to_go > seg_len)) {

                to_go -= seg_len;
                prev_p = p;
                p = *i++;
                seg_len = magnitude(p - prev_p);
            }

            // If to_go beyond last point, return last point.
            if (to_go > seg_len) return p;      

            return prev_p + (p - prev_p) * (to_go / seg_len);

        } else {

            // Only one point. Return it.
            return prev_p;
        }
    }

    // No points. Undefined. Signal error?
    Point<I,T> bad_result;
    bad_result.zero();
    return bad_result;
}


/*
  This takes an edge (a series of connected points) and replaces it
  with another series of connected points evenly spaced along the
  geometry of the original edge. It's for building dashed lines, but
  might potentially have other uses. It is guaranteed to have enough
  points to start and end in the 'on' case.
*/

template <typename Point>
    std::deque<Point> dash(std::deque<Point> const & points, typename Point::component_type on, typename Point::component_type off)
{
    typedef typename Point::component_type T;
    
    if (points.size() < 2) return points;

    std::deque<Point> result;
    T total_length = length<Point>(points);
    T divideBy = on+off;
    if(divideBy == 0)
    {
        return result;
    }
    int32_t count = int32_t(floor(total_length/divideBy));      // Total 'off' dashes.
    T fudge = (total_length - count * (on+off))/T(2);           // Amount extra on at each end.

    // Do the inefficient thing for now. That is, just lerp into the
    // edge every time instead of iterating. @TODO: In the future,
    // this should build the result points in a single pass through
    // the edge.

    result.push_back(points.front());

    for (int i = 0; i<count; ++i)
    {
        T d0 = fudge + (on/T(2)) + T(i) * (on+off);
        T d1 = d0 + off;
#if 1
        result.push_back(findPointAlongLinearSpline(d0, points));
        result.push_back(findPointAlongLinearSpline(d1, points));
#else
        result.push_back(lerp(d0, points));
        result.push_back(lerp(d1, points));
        
#endif
    }

    result.push_back(points.back());

    return result;
}


template <int I, typename T>
Point<I,T> bezier(T t, Point<I,T> const & p0)
{
    return p0;
}


template <int I, typename T>
Vec<I,T> bezierTan(T t, Point<I,T> const &)
{
    Vec<I,T> result;
    result.zero();
    return result;
}


template <int I, typename T>
Point<I,T> bezier(T t, Point<I,T> const & p0, Point<I,T> const & p1)
{
    return p0 + (p1 - p0) * t;
}


template <int I, typename T>
Vec<I,T> bezierTan(T t, Point<I,T> const & p0, Point<I,T> const & p1)
{
    return p1 - p0;
}


template <int I, typename T>
Point<I,T> bezier(T t, Point<I,T> const & p0, Point<I,T> const & p1, Point<I,T> const & p2)
{
    T s = T(1) - t;
    T s2 = s * s;
    T t2 = t * t;

    T a = s2;
    T b = T(2) * s * t;
    T c = t2;

    Vec<I,T> result = 
        toVec(p0) * a +
        toVec(p1) * b + 
        toVec(p2) * c;

    return toPoint(result);
}


template <int I, typename T>
Vec<I,T> bezierTan(T t, Point<I,T> const & p0, Point<I,T> const & p1, Point<I,T> const & p2)
{
    T a = T(2) * (t - T(1));
    T b = T(2) * (T(1) - T(2) * t);
    T c = T(2) * t;

    Vec<I,T> result = 
        toVec(p0) * a +
        toVec(p1) * b + 
        toVec(p2) * c;

    return result;
}


template <int I, typename T>
Point<I,T> bezier(T t, Point<I,T> const & p0, Point<I,T> const & p1, Point<I,T> const & p2, Point<I,T> const & p3)
{
    T s = T(1) - t;
    T s2 = s * s;
    T s3 = s2 * s;

    T t2 = t * t;
    T t3 = t * t * t;

    T a = s3;
    T b = T(3) * s2 * t;
    T c = T(3) * s * t2;
    T d = t3;

    Vec<I,T> result = 
        toVec(p0) * a +
        toVec(p1) * b + 
        toVec(p2) * c + 
        toVec(p3) * d;

    return toPoint(result);
}


template <int I, typename T>
Vec<I,T> bezierTan(T t, Point<I,T> const & p0, Point<I,T> const & p1, Point<I,T> const & p2, Point<I,T> const & p3)
{
    // Found this online.
    Vec<I,T> result =
        (p1 - p0) * T(3) * (T(1) - t) * (T(1) - t) +
        (p2 - p1) * T(6) * (T(1) - t) * t +
        (p3 - p2) * T(3) * t * t;

    return result;
}


// Bezier interpolate between p1 and p2 using p0 and p3 as tangent
// info. The p to q conversion converts from a piece-wise spline (with
// tangent continuity at the knots) to a proper bezier form.

template <int I, typename T>
Point<I,T> bezierPiecewise(T t, Point<I,T> const & p0, Point<I,T> const & p1, Point<I,T> const & p2, Point<I,T>const & p3)
{
    Point<I,T> q0 = p1;
    Point<I,T> q1 = p1 + (p1 - p0)*(T(1)/T(3));
    Point<I,T> q2 = p2 + (p2 - p3)*(T(1)/T(3));
    Point<I,T> q3 = p2;

    return bezier(t, q0, q1, q2, q3);
}


template <int I, typename T>
Vec<I,T> bezierPiecewiseTan(T t, Point<I,T> const & p0, Point<I,T> const & p1, Point<I,T> const & p2, Point<I,T>const & p3)
{
    Point<I,T> q0 = p1;
    Point<I,T> q1 = p1 + (p1 - p0)*(T(1)/T(3));
    Point<I,T> q2 = p2 + (p2 - p3)*(T(1)/T(3));
    Point<I,T> q3 = p2;

    return bezierTan(t, q0, q1, q2, q3);
}


template <int I, typename T>
Point<I, T> bezierSpline(T t, std::vector<Point<I,T> > const & points)
{
    int32_t count = points.size();
    if (count == 0) {
        Point<I,T> result;
        result.zero();
        return result;
    }

    if (t <= T(0)) return points.front();
    if (t >= T(1)) return points.back();

    // Handle the non-piecewise cases

    switch (count)
    {
    case 1:
        return bezier(t, points[0]);
    case 2:
        // linear 'bezier'
        return bezier(t, points[0], points[1]);
    case 3:
        // quadratic 'bezier'
        return bezier(t, points[0], points[1], points[2]);
    }

    // The piecewise case... (This could be made more efficient if
    // necessary. Won't be necessary for mere letter placement.)

    T seg_t = t * T(count-1);
    T seg_i = floor(seg_t);
    int32_t i = int32_t(seg_i);
    T s = seg_t - seg_i;

    // i shouldn't be less than zero or greater than count-2

    if (i < 1)
    {
        Point<I,T> p_minus_one = points[0] + (points[0] - points[1]);
        return bezierPiecewise(s, p_minus_one, points[0], points[1], points[2]);
    }

    if (i >= count-2)
    {
        Point<I,T> p_count = points[count - 1] + (points[count - 1] - points[count - 2]);
        return bezierPiecewise(s, points[i-1], points[i], points[i+1], p_count);
    }

    return bezierPiecewise(s, points[i-1], points[i], points[i+1], points[i+2]);
}


template <int I, typename T>
Vec<I, T> bezierSplineTan(T t, std::vector<Point<I,T> > const & points)
{
    int32_t count = points.size();
    if (count == 0) {
        Vec<I,T> result;
        result.zero();
        return result;
    }

    // Handle the non-piecewise cases

    switch (count)
    {
    case 1:
        return bezierTan(t, points[0]);
    case 2:
        // linear 'bezier'
        return bezierTan(t, points[0], points[1]);
    case 3:
        // quadratic 'bezier'
        return bezierTan(t, points[0], points[1], points[2]);
    }

    // The piecewise case... (This could be made more efficient if
    // necessary. Won't be necessary for mere letter placement.)

    T seg_t = t * T(count-1);
    T seg_i = floor(seg_t);
    int32_t i = int32_t(seg_i);
    T s = seg_t - seg_i;

    // i shouldn't be less than zero or greater than count-2

    if (i < 1)
    {
        Point<I,T> p_minus_one = points[0] + (points[0] - points[1]);
        return bezierPiecewiseTan(s, p_minus_one, points[0], points[1], points[2]);
    }

    if (i >= count-2)
    {
        Point<I,T> p_count = points[count - 1] + (points[count - 1] - points[count - 2]);
        return bezierPiecewiseTan(s, points[i-1], points[i], points[i+1], p_count);
    }

    return bezierPiecewiseTan(s, points[i-1], points[i], points[i+1], points[i+2]);
}


template <int I, typename T>
T linearSplineLength(std::deque<Point<I,T> > const & points)
{
    if (points.size() < 2) return T(0);

    typename std::deque<Point<I,T> >::const_iterator i = points.begin();
    
    Point<I,T> last_p = *i++;
    T length = T(0);
    for (;
         i != points.end();
         ++i)
    {
        Point<I,T> p = *i;
        length += magnitude(p - last_p);
        last_p = p;
    }
    return length;
}


template <typename Point>
Point findPointAlongLinearSpline(typename Point::component_type t, std::deque<Point> const & points)
{
    typedef typename Point::component_type T;
    
    int count = points.size();
    if (count==0)
    {
        Point result;
        return result;
    }

    if (count==1)
    {
        return points.front();
    }

    typename std::deque<Point>::const_iterator i = points.begin();
    Point p0 = *i++;
    Point p1 = *i++;

    T t0 = T(0);
    T t1 = magnitude(p1 - p0);

    while ((t > t1) && (i != points.end()))
    {
        p0 = p1;
        p1 = *i++;
        t0 = t1;
        t1 += magnitude(p1 - p0);
    }
    
    double s = (t - t0) / (t1 - t0);
    Point result = lerp<Point>(p0, p1, s);

    return result;
}

/*
  This takes a 'linear spline' parameter 't' (from zero to one) and a vector of points.
*/


template <int I, typename T>
Point<I, T> linearSpline(T s, std::deque<Point<I,T> > const & points)
{
    T length = linearSplineLength(points);
    T t = s * length;
    return findPointAlongLinearSpline(t, points);
}


template <int I, typename T>
Vec<I, T> findTanAtPointAlongLinearSpline(T t, std::deque<Point<I,T> > const & points)
{
    int count = points.size();
    if (count<2)
    {
        Vec<I,T> result;
        return result;
    }

    typename std::deque<Point<I,T> >::const_iterator i = points.begin();
    
    Point<I,T> p0 = *i++;
    Point<I,T> p1 = *i++;

    //T t0 = T(0);
    T t1 = magnitude(p1 - p0);

    while ((t > t1) && (i != points.end()))
    {
        p0 = p1;
        p1 = *i++;
        //t0 = t1;
        t1 += magnitude(p1 - p0);
    }
    return (p1 - p0);
}


template <int I, typename T>
Vec<I, T> linearSplineTan(T s, std::vector<Point<I,T> > const & points)
{
    T length = linearSplineLength(points);
    T t = s * length;
    return findTanAtPointAlongLinearSpline(t, points);
}


} // namespace math

} // namespace Tn

#endif
