#ifndef Foundation_Math_Edge_INCLUDED
#define Foundation_Math_Edge_INCLUDED

#include <vector>
#include <deque>

#include <foundation/Math/Point.hpp>

/*
  The map data the engine deals with is composed of 'edges.' An 'edge'
  is a series of points. A route is a collection of edges assumed to
  be contiguous.

  An 'arrow' (or turn indicator) is built from route data. To support
  turn arrows that represent u-turns, it is necessary to iterate
  through the arrow edges as a series of line segments, comparing the
  current segment with the previous to see if it turns back on itself
  in a way that the arrow geometry will wind up overlapping itself.

  Iterating through edges this way is a bookkeeping nightmare. The
  classes here are designed to do that work so that the arrow
  geometry-building logic can be kept tractable. The hope was that
  these classes would have more general use, but it's not at all clear
  why (beyond the task described) these have any advantage over the
  more general STL data structures.

  Consequently, it's not clear that these belong in the math library.

*/


namespace Tn {

namespace math {


template <typename T>
struct Edge {

    typedef std::deque<Point<3,T> > Points;
    typedef std::pair<Point<3,T>, Point<3,T> > Segment;

    void push_back(Point<3,T> const & p) { points.push_back(p); }

    struct iterator {

        iterator() {}
        iterator(typename Points::iterator _i0, typename Points::iterator _i1) 
            : i0(_i0), i1(_i1) {}
        iterator(iterator const & that) : i0(that.i0), i1(that.i1) {}

        typename Points::iterator i0;
        typename Points::iterator i1;

        bool operator==(iterator const & that) {
            return (i1 == that.i1);
        }

        bool operator!=(iterator const & that) {
            return (i1 != that.i1);
        }

        iterator & operator++() {
            ++i0;
            ++i1;
            return *this;
        }

        Segment operator*() const {
            return Segment(*i0, *i1);
        }
    };


    struct reverse_iterator {

        reverse_iterator(typename Points::reverse_iterator _i0, 
                         typename Points::reverse_iterator _i1) 
            : i0(_i0), i1(_i1) {}

        typename Points::reverse_iterator i0;
        typename Points::reverse_iterator i1;

        bool operator==(reverse_iterator const & that) {
            return (i1 == that.i1);
        }

        bool operator!=(reverse_iterator const & that) {
            return (i1 != that.i1);
        }

        iterator & operator++() {
            ++i0;
            ++i1;
            return *this;
        }

        Segment operator*() const {
            return Segment(*i0, *i1);
        }
    };

    
    struct const_iterator {
        
        const_iterator() {}
        const_iterator(typename Points::const_iterator _i0, typename Points::const_iterator _i1) 
        : i0(_i0), i1(_i1) {}
        const_iterator(const_iterator const & that) : i0(that.i0), i1(that.i1) {}
        
        typename Points::const_iterator i0;
        typename Points::const_iterator i1;
        
        bool operator==(const_iterator const & that) {
            return (i1 == that.i1);
        }
        
        bool operator!=(const_iterator const & that) {
            return (i1 != that.i1);
        }
        
        const_iterator & operator++() {
            ++i0;
            ++i1;
            return *this;
        }
        
        Segment operator*() const {
            return Segment(*i0, *i1);
        }
    };
    
    
    struct const_reverse_iterator {
        
        const_reverse_iterator(typename Points::const_reverse_iterator _i0, 
                               typename Points::const_reverse_iterator _i1) 
        : i0(_i0), i1(_i1) {}
        
        typename Points::const_reverse_iterator i0;
        typename Points::const_reverse_iterator i1;
        
        bool operator==(const_reverse_iterator const & that) {
            return (i1 == that.i1);
        }
        
        bool operator!=(const_reverse_iterator const & that) {
            return (i1 != that.i1);
        }
        
        iterator & operator++() {
            ++i0;
            ++i1;
            return *this;
        }
        
        Segment operator*() const {
            return Segment(*i0, *i1);
        }
    };
    

    iterator begin() {
        typename Points::iterator i0 = points.begin();
        typename Points::iterator i1 = points.begin();
        if (i1 != points.end()) ++i1;
        return iterator(i0, i1);
    }

    iterator end() { 
        typename Points::iterator e = points.end();
        return iterator(e, e);
    }


    reverse_iterator rbegin() {
        typename Points::reverse_iterator i0 = points.rbegin();
        typename Points::reverse_iterator i1 = (i0 != points.rend()) ? ++points.rbegin() : i0;
        return reverse_iterator(i0, i1);
    }

    reverse_iterator rend() { 
        typename Points::reverse_iterator e = points.rend();
        return reverse_iterator(e, e);
    }
    

    const_iterator begin() const {
        typename Points::const_iterator i0 = points.begin();
        typename Points::const_iterator i1 = points.begin();
        if (i1 != points.end()) ++i1;
        return const_iterator(i0, i1);
    }
    
    const_iterator end() const { 
        typename Points::const_iterator e = points.end();
        return const_iterator(e, e);
    }
    
    
    const_reverse_iterator rbegin() const {
        typename Points::const_reverse_iterator i0 = points.rbegin();
        typename Points::const_reverse_iterator i1 = (i0 != points.rend()) ? ++points.rbegin() : i0;
        return const_reverse_iterator(i0, i1);
    }
    
    const_reverse_iterator rend() const { 
        typename Points::reverse_iterator e = points.rend();
        return const_reverse_iterator(e, e);
    }
    
    Points points;
};


typedef Edge<float> Edgef;
typedef Edge<double> Edged;


template <typename T>
struct Graph {

    typedef struct Edge<T> Edge;

    typedef std::vector<Edge> Edges;

    Edges edges;

    void NewEdge() {
        edges.push_back(Edge());
    }
    void AddPoint(Point<3,T> const & p) {
        edges.back().push_back(p);
    }

    struct iterator {

        typename Edges::iterator edges_end;
        typename Edges::iterator edges_iter;
        typename Edge::iterator edge_iter;

        iterator(typename Edges::iterator _edges_end, 
                 typename Edges::iterator _edges_iter, 
                 typename Edge::iterator _edge_iter)
            : edges_end(_edges_end), 
              edges_iter(_edges_iter), 
              edge_iter(_edge_iter) {}

        iterator(typename Edges::iterator _edges_end)
            : edges_end(_edges_end), 
              edges_iter(_edges_end) {}

        iterator(iterator const & that) 
            : edges_end(that.edges_end),
              edges_iter(that.edges_iter),
              edge_iter(that.edge_iter) {}

        iterator & operator ++() {
            ++edge_iter;
            if (edge_iter == edges_iter->end()) {
                ++edges_iter;
                if (edges_iter != edges_end)
                {
                    edge_iter = edges_iter->begin();
                }
            }
            return *this;
        }

        bool operator==(iterator const & that) {
            bool this_end = (edges_iter == edges_end);
            bool that_end = (that.edges_iter == that.edges_end);
            if (this_end) return that_end;
            if (that_end) return false;
            // All this is to avoid comparing uninitialized iterators,
            // because Windows will throw an exception if you do.
            bool same_iter = (edge_iter == that.edge_iter);
            return same_iter;
        }

        bool operator!=(iterator const & that) {
            return !operator==(that);
        }

        typename Edge::Segment operator*()
        {
            return *edge_iter;
        }
    };

    iterator begin() {
        typename Edges::iterator edges_iter = edges.begin();
        if (edges_iter == edges.end()) return iterator(edges_iter);

        typename Edge::iterator edge_iter = edges_iter->begin();

        return iterator(edges.end(), edges_iter, edge_iter);
    }
        
    iterator end() {
        typename Edges::iterator edges_end = edges.end();
        return iterator(edges_end);
    }
};


typedef Graph<float> Graphf;
typedef Graph<double> Graphd;


/*
  http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
*/

template <typename T>
T distance(typename Edge<T>::Segment const & segment, Point<3,T> const & point)
{
    Vec<3,T> v0 = point - segment.first;
    Vec<3,T> v1 = point - segment.second;

    Vec<3,T> v2 = segment.second - segment.first;

    T d = magnitude(cross(v0, v1))/magnitude(v2);

    return d;
}


} // namespace math

} // namespace Tn

#endif

