/*
 *  edgeCutter.hpp
 *
 *  Created by thant on 6/6/2012.
 *  Copyright 2012 TeleNav. All rights reserved.
 *
 */

#ifndef Foundation_Math_edgeCutter_INCLUDED
#define Foundation_Math_edgeCutter_INCLUDED

#include <iostream>

#include <boost/optional.hpp>
#include <list>
#include <deque>
#include <utility>

#include <foundation/Math/Point.hpp>
#include <foundation/SharedPtr.h>

#include <foundation/Log.hpp>

#include <limits>

namespace Tn {

namespace routeQuadTree {
    extern Tn::Foundation::Topic topic;
}   // namespace routeQuadTree
    
namespace math {

namespace edge_cutter {


/*  Using point lists here so we can "cut" an edge on a boundary by
 *  inserting a point. A "segment" is represented by two list
 *  iterators. This allows tiles to share the same point list while
 *  representing the segments within their bounds to be represnted by
 *  segments.
 *
 */
template<typename Point>
class EdgeCutterType
{
public:
    typedef typename std::list<Point> Point3dList;
    typedef typename Tn::Foundation::shared_ptr<Point3dList> Point3dListPtr;
    typedef typename Point3dList::iterator Point3dListIter;
    
    typedef typename std::pair<Point3dListIter,Point3dListIter> Segment;
    typedef typename std::deque<Segment> SegmentDeq;
    typedef typename Tn::Foundation::shared_ptr<SegmentDeq> SegmentDeqPtr;
    
    typedef typename std::deque<Point> Point3dDeq;
    typedef typename Tn::Foundation::shared_ptr<Point3dDeq> Point3dDeqPtr;
    typedef typename Point3dDeq::iterator Point3dDeqIter;
    
    struct Edge {
        Point3dListPtr points;
        SegmentDeqPtr segments;
    };
    
    typedef typename std::pair<Edge,Edge> EdgePair;
};

template<typename Point>
size_t size(typename EdgeCutterType<Point>::Segment const & segment)
{
    typename EdgeCutterType<Point>::Point3dListIter const & end = segment.second;
    typename EdgeCutterType<Point>::Point3dListIter i = segment.first;
    
    size_t count = 0;
    while (i != end) {
        ++count;
        ++i;
    }
    
    return count;    
}

enum eSide {
    eSide_Below,
    eSide_Above
};

template<typename Point>
void add(typename EdgeCutterType<Point>::EdgePair & edges,
         eSide side,
         typename EdgeCutterType<Point>::Point3dListIter i0,
         typename EdgeCutterType<Point>::Point3dListIter i1)
{
    switch (side)
    {
        case eSide_Below:
            if (!edges.first.segments)
                edges.first.segments = typename EdgeCutterType<Point>::SegmentDeqPtr
                (new typename EdgeCutterType<Point>::SegmentDeq());
            
            edges.first.segments->push_back(typename EdgeCutterType<Point>::Segment(i0, i1));
            break;
            
        case eSide_Above:
            if (!edges.second.segments)
                edges.second.segments = typename EdgeCutterType<Point>::SegmentDeqPtr
                (new typename EdgeCutterType<Point>::SegmentDeq());
            
            edges.second.segments->push_back(typename EdgeCutterType<Point>::Segment(i0, i1));
            break;
    }
    
}

/*  Only switch sides if the edge actually crosses. Not
 *  enough to land on the boundary. So use different
 *  tests.
 */
template <int I, typename Point>
eSide getSide(eSide prevSide, Point const & p, double x)
{
    int const X = (I+0) % 3;

    switch (prevSide) {

    case eSide_Below:
        return (p[X] > x) ? eSide_Above : eSide_Below;

    case eSide_Above:
        return (p[X] < x) ? eSide_Below : eSide_Above;
    }
}

/*  Cuts a line segment at the specified axis-aligned plane. If either
 *  point is exactly on the plane, doesn't cut.
 */
template <int I, typename Point>
typename boost::optional<Point> cutSegment(Point const & p0, Point const & p1, double x)
{
    int const X = (I+0) % 3;

    Point result;

    // points on opposite sides of plane?

    bool spans = (((p0[X] < x) && (p1[X] > x)) ||
                  ((p1[X] < x) && (p0[X] > x)));

    if (!spans) return typename boost::optional<Point>();

    // Because of above test, this won't be zero
    double dx = p1[X] - p0[X];

    double t = (x - p0[X])/dx;

    if ((t<0) || (t>=1)) return typename boost::optional<Point>();
    
    return boost::optional<Point>(Tn::math::lerp(p0, p1, t));
}


template <int I, typename Point>
typename EdgeCutterType<Point>::EdgePair cutEdge(typename EdgeCutterType<Point>::Edge const & edge, double x) {

    typename EdgeCutterType<Point>::EdgePair result;    // starts out empty

    if (!edge.points || !edge.segments) return result;
    
    result.first.points = edge.points;
    result.second.points = edge.points;

    for (typename EdgeCutterType<Point>::SegmentDeq::const_iterator
         segItr = edge.segments->begin();
         segItr != edge.segments->end();
         ++segItr)
    {
        typename EdgeCutterType<Point>::Segment const & seg = *segItr;
        typename EdgeCutterType<Point>::Point3dListIter begin = seg.first;
        typename EdgeCutterType<Point>::Point3dListIter end = seg.second;
        typename EdgeCutterType<Point>::Point3dListIter i = begin;
        typename EdgeCutterType<Point>::Point3dListIter i_prev = i;

        // Which side do we start on?
        eSide side = getSide<I>(eSide_Below, *i, x);

        while (++i != end)
        {
            Point const & p0 = *i_prev;
            Point const & p1 = *i;

            eSide nextSide = getSide<I>(eSide_Below, p1, x);

            if (side != nextSide)
            {
                typename boost::optional<Point> pOpt = cutSegment<I>(p0, p1, x);

                if (pOpt)
                {
                    // prev new i
                    typename EdgeCutterType<Point>::Point3dListIter i_new = edge.points->insert(i, *pOpt);
                    add<Point>(result, side, begin, i);
                    i_prev = i_new;
                    begin = i_new;
                }
                else
                {
                    // new segment from border, so no cut point
                    typename EdgeCutterType<Point>::Point3dListIter i_next = i;
                    ++i_next;
                    add<Point>(result, side, begin, i_next);
                    begin = i;
                }
                side = nextSide;
            }
            i_prev = i;
        }
        // add last segment if not empty
        if (begin != i_prev)
        {
            add<Point>(result, side, begin, i);
        }
    }
    return result;
}

    
/*  This finds the (sqare of the) distance of the point p0 from the
 *  line formed by the points p1, p2 (in the z=0 plane).
 *
 *  http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
 */
template<typename Point>
double getDistanceSquared(Point const & p0, Point const & p1, Point const & p2)
{
    using namespace Tn::math;
    
    // Get perpenicular normal:
    double x = p2[1] - p1[1];
    double y = -(p2[0] - p1[0]);
    
    if ((tnabs(x) <= std::numeric_limits<double>::epsilon()) &&
        (tnabs(y) <= std::numeric_limits<double>::epsilon()))
    {
        /*  Points p1 and p2 are the same point. Just return distance
         *  between p0 and p1.
         */
        
        return magnitude(p1 - p0);
    }
    
    Vec2d v = normalize(Vec2<double>(x, y));
    
    Vec2d r = Vec2<double>(p1[0] - p0[0], p1[1] - p0[1]);
    
    Vec2d d = v * dot(v, r);
    
    return dot(d, d);
}


/* Given the line formed by the first and last point in the segment,
 * this finds the intermediate point farthest from the line (measured
 * perpendicularly) and returns its iterator. */
template<typename Point>
std::pair<typename EdgeCutterType<Point>::Point3dListIter,double>
findFarthest(typename EdgeCutterType<Point>::Segment const & segment)
{
    typename EdgeCutterType<Point>::Point3dListIter const & begin = segment.first;
    typename EdgeCutterType<Point>::Point3dListIter const & end = segment.second;
    
    if (begin == end) return std::make_pair(end,0.0); /* shouldn't happen */
    
    /* End is one past the last point. */
    
    typename EdgeCutterType<Point>::Point3dListIter last = end;
    --last;
    
    Point const & front = *begin;
    Point const & back = *last;
    
    double max = 0.0;
    typename EdgeCutterType<Point>::Point3dListIter result = end;
    typename EdgeCutterType<Point>::Point3dListIter i = begin;
    while (++i != last)
    {
        double distance = getDistanceSquared(*i, front, back);
        if (distance > max)
        {
            result = i;
            max = distance;
        }
    }
    
    return std::make_pair(result, sqrt(max));
}

    
/*  http://www.codeproject.com/Articles/114797/Polyline-Simplification
 *
 *  Using Douglas-Peucker method.
 *
 */


/*  This takes a segment and builds a new simplified point list based
 *  on the tolerance passed in.  Tolerance is the distance a point
 *  must deviate from the polyline to be considered significant. The
 *  origin is the vector used to translate to the tile origin.
 */

/*  This is the recursive version. */

template<typename Point>
void simplify(typename EdgeCutterType<Point>::Point3dDeqPtr const & result,
              typename EdgeCutterType<Point>::Segment const & src,
              Vec3d const & origin,
              double tolerance)
{
    typename EdgeCutterType<Point>::Point3dListIter const & begin = src.first;
    typename EdgeCutterType<Point>::Point3dListIter const & end = src.second;
    
    if (begin == end)
    {
        /* Zero-length segment. Shouldn't happen. */
        assert(false);
        return;
    }
    
    std::pair<typename EdgeCutterType<Point>::Point3dListIter,double> farthest = findFarthest<Point>(src);
    
    typename EdgeCutterType<Point>::Point3dListIter const & middle = farthest.first;
    double d = farthest.second;
    
    if ((middle == end) ||
        (d < tolerance))
    {
        /*  This is either a segment with only one point, or with no
         *  points that deviate more than tolerance. Push the first
         *  point on to the result. */
        
        
        TN_LOGT(TnVerbose, Tn::routeQuadTree::topic)
        << "pushing point ("
        << (*begin)[0] << ", "
        << (*begin)[1] << ")";
        
        Point pnt(*begin);
        pnt -= origin;
        result->push_back(pnt);
        
        /*  The second point of the first segment is also the first
         *  point of the second segment, so no need to push anything
         *  but the first point. (Except the very last top-level
         *  second point.)
         */
    }
    else
    {
        /*  Otherwise keep recursing. */
        
        {
            typename EdgeCutterType<Point>::Point3dListIter last = end;
            --last;
            
            TN_LOGT(TnVerbose, Tn::routeQuadTree::topic)
            << "recursing: \n"
            << "\t(" << (*begin)[0] << ", " << (*begin)[1] << ")\n"
            << "\t(" << (*middle)[0] << ", " << (*middle)[1] << ")\n"
            << "\t(" << (*last)[0] << ", " << (*last)[1] << ")\n";
        }
        
        if (begin != middle) {
            typename EdgeCutterType<Point>::Point3dListIter pastMiddle = middle;
            ++pastMiddle;
            simplify<Point>(result, typename EdgeCutterType<Point>::Segment(begin, pastMiddle), origin, tolerance);
        }
        
        simplify<Point>(result, typename EdgeCutterType<Point>::Segment(middle, end), origin, tolerance);
    }
}

    
/*  This is the public API */

template<typename Point>
typename EdgeCutterType<Point>::Point3dDeqPtr
simplify(typename EdgeCutterType<Point>::Segment const & src,
         Vec3d const & origin, double tolerance)
{
    typename EdgeCutterType<Point>::Point3dDeqPtr
    result = typename EdgeCutterType<Point>::Point3dDeqPtr
                (new typename EdgeCutterType<Point>::Point3dDeq());
    
    simplify<Point>(result, src, origin, tolerance);
    
    typename EdgeCutterType<Point>::Point3dListIter const & begin = src.first;
    typename EdgeCutterType<Point>::Point3dListIter const & end = src.second;
    
    /*  Get last point. */
    
    if (begin != end)
    {
        typename EdgeCutterType<Point>::Point3dListIter last = end;
        --last;
        if (begin != last)
        {
            Point pnt(*last);
            pnt -= origin;
            result->push_back(pnt);
        }
    }
    
    return result;
}






} // namespace edge_cutter

} // namespace math

} // namespace Tn

#endif


