// -*- c++ -*- //

 /**

 @defgroup paths Path Structures

 @ingroup datas

 \brief Path structures implemented in Hugo.

 Hugolib provides flexible data structures

 to work with paths.

 All of them have the same interface, especially they can be built or extended

 using a standard Builder subclass. This make is easy to have e.g. the Dijkstra

 algorithm to store its result in any kind of path structure.

 \sa hugo::skeleton::Path

 */

 ///\ingroup paths

 ///\file

 ///\brief Classes for representing paths in graphs.

 #ifndef HUGO_PATH_H

 #define HUGO_PATH_H

 #include <deque>

 #include <vector>

 #include <algorithm>

 #include <hugo/invalid.h>

 namespace hugo {

   /// \addtogroup paths

   /// @{

   //! \brief A structure for representing directed paths in a graph.

   //!

   //! A structure for representing directed path in a graph.

   //! \param Graph The graph type in which the path is.

   //! \param DM DebugMode, defaults to DefaultDebugMode.

   //! 

   //! In a sense, the path can be treated as a graph, for is has \c NodeIt

   //! and \c EdgeIt with the same usage. These types converts to the \c Node

   //! and \c Edge of the original graph.

   //!

   //! \todo Thoroughfully check all the range and consistency tests.

   template<typename Graph>

   class DirPath {

   public:

     /// Edge type of the underlying graph.

     typedef typename Graph::Edge GraphEdge; 

     /// Node type of the underlying graph.

     typedef typename Graph::Node GraphNode;

     class NodeIt;

     class EdgeIt;

   protected:

     const Graph *gr;

     typedef std::vector<GraphEdge> Container;

     Container edges;

   public:

     /// \param _G The graph in which the path is.

     ///

     DirPath(const Graph &_G) : gr(&_G) {}

     /// \brief Subpath constructor.

     ///

     /// Subpath defined by two nodes.

     /// \warning It is an error if the two edges are not in order!

     DirPath(const DirPath &P, const NodeIt &a, const NodeIt &b) {

       gr = P.gr;

       edges.insert(edges.end(), P.edges.begin()+a.idx, P.edges.begin()+b.idx);

     }

     /// \brief Subpath constructor.

     ///

     /// Subpath defined by two edges. Contains edges in [a,b)

     /// \warning It is an error if the two edges are not in order!

     DirPath(const DirPath &P, const EdgeIt &a, const EdgeIt &b) {

       gr = P.gr;

       edges.insert(edges.end(), P.edges.begin()+a.idx, P.edges.begin()+b.idx);

     }

     /// Length of the path.

     size_t length() const { return edges.size(); }

     /// Returns whether the path is empty.

     bool empty() const { return edges.empty(); }

     /// Resets the path to an empty path.

     void clear() { edges.clear(); }

     /// \brief Starting point of the path.

     ///

     /// Starting point of the path.

     /// Returns INVALID if the path is empty.

     GraphNode tail() const {

       return empty() ? INVALID : gr->tail(edges[0]);

     }

     /// \brief End point of the path.

     ///

     /// End point of the path.

     /// Returns INVALID if the path is empty.

     GraphNode head() const {

       return empty() ? INVALID : gr->head(edges[length()-1]);

     }

     /// \brief Initializes node or edge iterator to point to the first

     /// node or edge.

     ///

     /// \sa nth

     template<typename It>

     It& first(It &i) const { return i=It(*this); }

     /// \brief Initializes node iterator to point to the node of a given index.

     NodeIt& nth(NodeIt &i, int n) const {

       return i=NodeIt(*this, n);

     }

     /// \brief Initializes edge iterator to point to the edge of a given index.

     EdgeIt& nth(EdgeIt &i, int n) const {

       return i=EdgeIt(*this, n);

     }

     /// Checks validity of a node or edge iterator.

     template<typename It>

     static

     bool valid(const It &i) { return i.valid(); }

     /// Steps the given node or edge iterator.

     template<typename It>

     static

     It& next(It &e) {

       return ++e;

     }

     /// \brief Returns node iterator pointing to the head node of the

     /// given edge iterator.

     NodeIt head(const EdgeIt& e) const {

       return NodeIt(*this, e.idx+1);

     }

     /// \brief Returns node iterator pointing to the tail node of the

     /// given edge iterator.

     NodeIt tail(const EdgeIt& e) const {

       return NodeIt(*this, e.idx);

     }

     /* Iterator classes */

     /**

      * \brief Iterator class to iterate on the edges of the paths

      * 

      * \ingroup paths

      * This class is used to iterate on the edges of the paths

      *

      * Of course it converts to Graph::Edge

      * 

      * \todo Its interface differs from the standard edge iterator.

      * Yes, it shouldn't.

      */

     class EdgeIt {

       friend class DirPath;

       int idx;

       const DirPath *p;

     public:

       /// Default constructor

       EdgeIt() {}

       /// Invalid constructor

       EdgeIt(Invalid) : idx(-1), p(0) {}

       /// Constructor with starting point

       EdgeIt(const DirPath &_p, int _idx = 0) :

 	idx(_idx), p(&_p) { validate(); }

       ///Validity check

       bool valid() const { return idx!=-1; }

       ///Conversion to Graph::Edge

       operator GraphEdge () const {

 	return valid() ? p->edges[idx] : INVALID;

       }

       /// Next edge

       EdgeIt& operator++() { ++idx; validate(); return *this; }

       /// Comparison operator

       bool operator==(const EdgeIt& e) const { return idx==e.idx; }

       /// Comparison operator

       bool operator!=(const EdgeIt& e) const { return idx!=e.idx; }

       /// Comparison operator

       bool operator<(const EdgeIt& e) const { return idx<e.idx; }

     private:

       // FIXME: comparison between signed and unsigned...

       // Jo ez igy? Vagy esetleg legyen a length() int?

       void validate() { if( size_t(idx) >= p->length() ) idx=-1; }

     };

     /**

      * \brief Iterator class to iterate on the nodes of the paths

      * 

      * \ingroup paths

      * This class is used to iterate on the nodes of the paths

      *

      * Of course it converts to Graph::Node

      * 

      * \todo Its interface differs from the standard node iterator.

      * Yes, it shouldn't.

      */

     class NodeIt {

       friend class DirPath;

       int idx;

       const DirPath *p;

     public:

       /// Default constructor

       NodeIt() {}

       /// Invalid constructor

       NodeIt(Invalid) : idx(-1), p(0) {}

       /// Constructor with starting point

       NodeIt(const DirPath &_p, int _idx = 0) :

 	idx(_idx), p(&_p) { validate(); }

       ///Validity check

       bool valid() const { return idx!=-1; }

       ///Conversion to Graph::Node

       operator const GraphNode& () const {

 	if(idx >= p->length())

 	  return p->head();

 	else if(idx >= 0)

 	  return p->gr->tail(p->edges[idx]);

 	else

 	  return INVALID;

       }

       /// Next node

       NodeIt& operator++() { ++idx; validate(); return *this; }

       /// Comparison operator

       bool operator==(const NodeIt& e) const { return idx==e.idx; }

       /// Comparison operator

       bool operator!=(const NodeIt& e) const { return idx!=e.idx; }

       /// Comparison operator

       bool operator<(const NodeIt& e) const { return idx<e.idx; }

     private:

       void validate() { if( size_t(idx) > p->length() ) idx=-1; }

     };

     friend class Builder;    

     /**

      * \brief Class to build paths

      * 

      * \ingroup paths

      * This class is used to fill a path with edges.

      *

      * You can push new edges to the front and to the back of the path in

      * arbitrary order then you should commit these changes to the graph.

      *

      * Fundamentally, for most "Paths" (classes fulfilling the

      * PathConcept) while the builder is active (after the first modifying

      * operation and until the commit()) the original Path is in a

      * "transitional" state (operations on it have undefined result). But

      * in the case of DirPath the original path remains unchanged until the

      * commit. However we don't recomend that you use this feature.

      */

     class Builder {

       DirPath &P;

       Container front, back;

     public:

       ///\param _P the path you want to fill in.

       ///

       Builder(DirPath &_P) : P(_P) {}

       /// Sets the starting node of the path.

       /// Sets the starting node of the path. Edge added to the path

       /// afterwards have to be incident to this node.

       /// It should be called iff the path is empty and before any call to

       /// \ref pushFront() or \ref pushBack()

       void setStartNode(const GraphNode &) {}

       ///Push a new edge to the front of the path

       ///Push a new edge to the front of the path.

       ///\sa setStartNode

       void pushFront(const GraphEdge& e) {

 	front.push_back(e);

       }

       ///Push a new edge to the back of the path

       ///Push a new edge to the back of the path.

       ///\sa setStartNode

       void pushBack(const GraphEdge& e) {

 	back.push_back(e);

       }

       ///Commit the changes to the path.

       void commit() {

 	if( !(front.empty() && back.empty()) ) {

 	  Container tmp;

 	  tmp.reserve(front.size()+back.size()+P.length());

 	  tmp.insert(tmp.end(), front.rbegin(), front.rend());

 	  tmp.insert(tmp.end(), P.edges.begin(), P.edges.end());

 	  tmp.insert(tmp.end(), back.begin(), back.end());

 	  P.edges.swap(tmp);

 	  front.clear();

 	  back.clear();

 	}

       }

       // FIXME: Hmm, pontosan hogy is kene ezt csinalni?

       // Hogy kenyelmes egy ilyet hasznalni?

       ///Reserve storage for the builder in advance.

       ///If you know an reasonable upper bound of the number of the edges

       ///to add, using this function you can speed up the building.

       void reserve(size_t r) {

 	front.reserve(r);

 	back.reserve(r);

       }

       void reserveFront(size_t r) {}

       void reserveBack(size_t r) {}

     private:

       bool empty() {

 	return front.empty() && back.empty() && P.empty();

       }

       GraphNode tail() const {

 	if( ! front.empty() )

 	  return P.gr->tail(front[front.size()-1]);

 	else if( ! P.empty() )

 	  return P.gr->tail(P.edges[0]);

 	else if( ! back.empty() )

 	  return P.gr->tail(back[0]);

 	else

 	  return INVALID;

       }

       GraphNode head() const {

 	if( ! back.empty() )

 	  return P.gr->head(back[back.size()-1]);

 	else if( ! P.empty() )

 	  return P.gr->head(P.edges[P.length()-1]);

 	else if( ! front.empty() )

 	  return P.gr->head(front[0]);

 	else

 	  return INVALID;

       }

     };

   };

   /**********************************************************************/

   //! \brief A structure for representing undirected path in a graph.

   //!

   //! A structure for representing undirected path in a graph. Ie. this is

   //! a path in a \e directed graph but the edges should not be directed

   //! forward.

   //!

   //! \param Graph The graph type in which the path is.

   //! \param DM DebugMode, defaults to DefaultDebugMode.

   //! 

   //! In a sense, the path can be treated as a graph, for is has \c NodeIt

   //! and \c EdgeIt with the same usage. These types converts to the \c Node

   //! and \c Edge of the original graph.

   //!

   //! \todo Thoroughfully check all the range and consistency tests.

   template<typename Graph>

   class UndirPath {

   public:

     /// Edge type of the underlying graph.

     typedef typename Graph::Edge GraphEdge;

      /// Node type of the underlying graph.

    typedef typename Graph::Node GraphNode;

     class NodeIt;

     class EdgeIt;

   protected:

     const Graph *gr;

     typedef std::vector<GraphEdge> Container;

     Container edges;

   public:

     /// \param _G The graph in which the path is.

     ///

     UndirPath(const Graph &_G) : gr(&_G) {}

     /// \brief Subpath constructor.

     ///

     /// Subpath defined by two nodes.

     /// \warning It is an error if the two edges are not in order!

     UndirPath(const UndirPath &P, const NodeIt &a, const NodeIt &b) {

       gr = P.gr;

       edges.insert(edges.end(), P.edges.begin()+a.idx, P.edges.begin()+b.idx);

     }

     /// \brief Subpath constructor.

     ///

     /// Subpath defined by two edges. Contains edges in [a,b)

     /// \warning It is an error if the two edges are not in order!

     UndirPath(const UndirPath &P, const EdgeIt &a, const EdgeIt &b) {

       gr = P.gr;

       edges.insert(edges.end(), P.edges.begin()+a.idx, P.edges.begin()+b.idx);

     }

     /// Length of the path.

     size_t length() const { return edges.size(); }

     /// Returns whether the path is empty.

     bool empty() const { return edges.empty(); }

     /// Resets the path to an empty path.

     void clear() { edges.clear(); }

     /// \brief Starting point of the path.

     ///

     /// Starting point of the path.

     /// Returns INVALID if the path is empty.

     GraphNode tail() const {

       return empty() ? INVALID : gr->tail(edges[0]);

     }

     /// \brief End point of the path.

     ///

     /// End point of the path.

     /// Returns INVALID if the path is empty.

     GraphNode head() const {

       return empty() ? INVALID : gr->head(edges[length()-1]);

     }

     /// \brief Initializes node or edge iterator to point to the first

     /// node or edge.

     ///

     /// \sa nth

     template<typename It>

     It& first(It &i) const { return i=It(*this); }

     /// \brief Initializes node iterator to point to the node of a given index.

     NodeIt& nth(NodeIt &i, int n) const {

       return i=NodeIt(*this, n);

     }

     /// \brief Initializes edge iterator to point to the edge of a given index.

     EdgeIt& nth(EdgeIt &i, int n) const {

       return i=EdgeIt(*this, n);

     }

     /// Checks validity of a node or edge iterator.

     template<typename It>

     static

     bool valid(const It &i) { return i.valid(); }

     /// Steps the given node or edge iterator.

     template<typename It>

     static

     It& next(It &e) {

       return ++e;

     }

     /// \brief Returns node iterator pointing to the head node of the

     /// given edge iterator.

     NodeIt head(const EdgeIt& e) const {

       return NodeIt(*this, e.idx+1);

     }

     /// \brief Returns node iterator pointing to the tail node of the

     /// given edge iterator.

     NodeIt tail(const EdgeIt& e) const {

       return NodeIt(*this, e.idx);

     }

     /**

      * \brief Iterator class to iterate on the edges of the paths

      * 

      * \ingroup paths

      * This class is used to iterate on the edges of the paths

      *

      * Of course it converts to Graph::Edge

      * 

      * \todo Its interface differs from the standard edge iterator.

      * Yes, it shouldn't.

      */

     class EdgeIt {

       friend class UndirPath;

       int idx;

       const UndirPath *p;

     public:

       /// Default constructor

       EdgeIt() {}

       /// Invalid constructor

       EdgeIt(Invalid) : idx(-1), p(0) {}

       /// Constructor with starting point

       EdgeIt(const UndirPath &_p, int _idx = 0) :

 	idx(_idx), p(&_p) { validate(); }

       ///Validity check

       bool valid() const { return idx!=-1; }

       ///Conversion to Graph::Edge

       operator GraphEdge () const {

 	return valid() ? p->edges[idx] : INVALID;

       }

       /// Next edge

      EdgeIt& operator++() { ++idx; validate(); return *this; }

       /// Comparison operator

       bool operator==(const EdgeIt& e) const { return idx==e.idx; }

       /// Comparison operator

       bool operator!=(const EdgeIt& e) const { return idx!=e.idx; }

       /// Comparison operator

       bool operator<(const EdgeIt& e) const { return idx<e.idx; }

     private:

       // FIXME: comparison between signed and unsigned...

       // Jo ez igy? Vagy esetleg legyen a length() int?

       void validate() { if( size_t(idx) >= p->length() ) idx=-1; }

     };

     /**

      * \brief Iterator class to iterate on the nodes of the paths

      * 

      * \ingroup paths

      * This class is used to iterate on the nodes of the paths

      *

      * Of course it converts to Graph::Node

      * 

      * \todo Its interface differs from the standard node iterator.

      * Yes, it shouldn't.

      */

     class NodeIt {

       friend class UndirPath;

       int idx;

       const UndirPath *p;

     public:

       /// Default constructor

       NodeIt() {}

       /// Invalid constructor

       NodeIt(Invalid) : idx(-1), p(0) {}

       /// Constructor with starting point

       NodeIt(const UndirPath &_p, int _idx = 0) :

 	idx(_idx), p(&_p) { validate(); }

       ///Validity check

       bool valid() const { return idx!=-1; }

       ///Conversion to Graph::Node

       operator const GraphNode& () const {

 	if(idx >= p->length())

 	  return p->head();

 	else if(idx >= 0)

 	  return p->gr->tail(p->edges[idx]);

 	else

 	  return INVALID;

       }

       /// Next node

       NodeIt& operator++() { ++idx; validate(); return *this; }

       /// Comparison operator

       bool operator==(const NodeIt& e) const { return idx==e.idx; }

       /// Comparison operator

       bool operator!=(const NodeIt& e) const { return idx!=e.idx; }

        /// Comparison operator

      bool operator<(const NodeIt& e) const { return idx<e.idx; }

     private:

       void validate() { if( size_t(idx) > p->length() ) idx=-1; }

     };

     friend class Builder;    

     /**

      * \brief Class to build paths

      * 

      * \ingroup paths

      * This class is used to fill a path with edges.

      *

      * You can push new edges to the front and to the back of the path in

      * arbitrary order then you should commit these changes to the graph.

      *

      * Fundamentally, for most "Paths" (classes fulfilling the

      * PathConcept) while the builder is active (after the first modifying

      * operation and until the commit()) the original Path is in a

      * "transitional" state (operations ot it have undefined result). But

      * in the case of UndirPath the original path is unchanged until the

      * commit. However we don't recomend that you use this feature.

      */

     class Builder {

       UndirPath &P;

       Container front, back;

     public:

       ///\param _P the path you want to fill in.

       ///

       Builder(UndirPath &_P) : P(_P) {}

       /// Sets the starting node of the path.

       /// Sets the starting node of the path. Edge added to the path

       /// afterwards have to be incident to this node.

       /// It should be called iff the path is empty and before any call to

       /// \ref pushFront() or \ref pushBack()

       void setStartNode(const GraphNode &) {}

       ///Push a new edge to the front of the path

       ///Push a new edge to the front of the path.

       ///\sa setStartNode

       void pushFront(const GraphEdge& e) {

 	front.push_back(e);

       }

       ///Push a new edge to the back of the path

       ///Push a new edge to the back of the path.

       ///\sa setStartNode

       void pushBack(const GraphEdge& e) {

 	if( !empty() && P.gr->tail(e)!=head() ) {

 	  fault("UndirPath::Builder::pushBack: nonincident edge");

 	}

 	back.push_back(e);

       }

       ///Commit the changes to the path.

       void commit() {

 	if( !(front.empty() && back.empty()) ) {

 	  Container tmp;

 	  tmp.reserve(front.size()+back.size()+P.length());

 	  tmp.insert(tmp.end(), front.rbegin(), front.rend());

 	  tmp.insert(tmp.end(), P.edges.begin(), P.edges.end());

 	  tmp.insert(tmp.end(), back.begin(), back.end());

 	  P.edges.swap(tmp);

 	  front.clear();

 	  back.clear();

 	}

       }

       // FIXME: Hmm, pontosan hogy is kene ezt csinalni?

       // Hogy kenyelmes egy ilyet hasznalni?

       ///Reserve storage for the builder in advance.

       ///If you know an reasonable upper bound of the number of the edges

       ///to add, using this function you can speed up the building.

        void reserve(size_t r) {

 	front.reserve(r);

 	back.reserve(r);

       }

       void reserveFront(size_t r) {}

       void reserveBack(size_t r) {}

     private:

       bool empty() {

 	return front.empty() && back.empty() && P.empty();

       }

       GraphNode tail() const {

 	if( ! front.empty() )

 	  return P.gr->tail(front[front.size()-1]);

 	else if( ! P.empty() )

 	  return P.gr->tail(P.edges[0]);

 	else if( ! back.empty() )

 	  return P.gr->tail(back[0]);

 	else

 	  return INVALID;

       }

       GraphNode head() const {

 	if( ! back.empty() )

 	  return P.gr->head(back[back.size()-1]);

 	else if( ! P.empty() )

 	  return P.gr->head(P.edges[P.length()-1]);

 	else if( ! front.empty() )

 	  return P.gr->head(front[0]);

 	else

 	  return INVALID;

       }

     };

   };

   /**********************************************************************/

   /* Ennek az allocatorosdinak sokkal jobban utana kene nezni a hasznalata

      elott. Eleg bonyinak nez ki, ahogyan azokat az STL-ben hasznaljak. */

   template<typename Graph>

   class DynamicPath {

   public:

     typedef typename Graph::Edge GraphEdge;

     typedef typename Graph::Node GraphNode;

     class NodeIt;

     class EdgeIt;

   protected:

     Graph& G;

     // FIXME: ehelyett eleg lenne tarolni ket boolt: a ket szelso el

     // iranyitasat:

     GraphNode _first, _last;

     typedef std::deque<GraphEdge> Container;

     Container edges;

   public:

     DynamicPath(Graph &_G) : G(_G), _first(INVALID), _last(INVALID) {}

     /// Subpath defined by two nodes.

     /// Nodes may be in reversed order, then

     /// we contstruct the reversed path.

     DynamicPath(const DynamicPath &P, const NodeIt &a, const NodeIt &b);

     /// Subpath defined by two edges. Contains edges in [a,b)

     /// It is an error if the two edges are not in order!

     DynamicPath(const DynamicPath &P, const EdgeIt &a, const EdgeIt &b);

     size_t length() const { return edges.size(); }

     GraphNode tail() const { return _first; }

     GraphNode head() const { return _last; }

     NodeIt& first(NodeIt &n) const { return nth(n, 0); }

     EdgeIt& first(EdgeIt &e) const { return nth(e, 0); }

     template<typename It>

     It first() const { 

       It e;

       first(e);

       return e; 

     }

     NodeIt& nth(NodeIt &, size_t) const;

     EdgeIt& nth(EdgeIt &, size_t) const;

     template<typename It>

     It nth(size_t n) const { 

       It e;

       nth(e, n);

       return e; 

     }

     bool valid(const NodeIt &n) const { return n.idx <= length(); }

     bool valid(const EdgeIt &e) const { return e.it < edges.end(); }

     bool isForward(const EdgeIt &e) const { return e.forw; }

     /// index of a node on the path. Returns length+2 for the invalid NodeIt

     int index(const NodeIt &n) const { return n.idx; }

     /// index of an edge on the path. Returns length+1 for the invalid EdgeIt

     int index(const EdgeIt &e) const { return e.it - edges.begin(); }

     EdgeIt& next(EdgeIt &e) const;

     NodeIt& next(NodeIt &n) const;

     template <typename It>

     It getNext(It it) const {

       It tmp(it); return next(tmp);

     }

     // A path is constructed using the following four functions.

     // They return false if the requested operation is inconsistent

     // with the path constructed so far.

     // If your path has only one edge you MUST set either "from" or "to"!

     // So you probably SHOULD call it in any case to be safe (and check the

     // returned value to check if your path is consistent with your idea).

     bool pushFront(const GraphEdge &e);

     bool pushBack(const GraphEdge &e);

     bool setFrom(const GraphNode &n);

     bool setTo(const GraphNode &n);

     // WARNING: these two functions return the head/tail of an edge with

     // respect to the direction of the path!

     // So G.head(P.graphEdge(e)) == P.graphNode(P.head(e)) holds only if 

     // P.forward(e) is true (or the edge is a loop)!

     NodeIt head(const EdgeIt& e) const;

     NodeIt tail(const EdgeIt& e) const;

     // FIXME: ezeknek valami jobb nev kellene!!!

     GraphEdge graphEdge(const EdgeIt& e) const;

     GraphNode graphNode(const NodeIt& n) const;

     /*** Iterator classes ***/

     class EdgeIt {

       friend class DynamicPath;

       typename Container::const_iterator it;

       bool forw;

     public:

       // FIXME: jarna neki ilyen is...

       // EdgeIt(Invalid);

       bool forward() const { return forw; }

       bool operator==(const EdgeIt& e) const { return it==e.it; }

       bool operator!=(const EdgeIt& e) const { return it!=e.it; }

       bool operator<(const EdgeIt& e) const { return it<e.it; }

     };

     class NodeIt {

       friend class DynamicPath;

       size_t idx;

       bool tail;  // Is this node the tail of the edge with same idx?

     public:

       // FIXME: jarna neki ilyen is...

       // NodeIt(Invalid);

       bool operator==(const NodeIt& n) const { return idx==n.idx; }

       bool operator!=(const NodeIt& n) const { return idx!=n.idx; }

       bool operator<(const NodeIt& n) const { return idx<n.idx; }

     };

   private:

     bool edgeIncident(const GraphEdge &e, const GraphNode &a,

 		      GraphNode &b);

     bool connectTwoEdges(const GraphEdge &e, const GraphEdge &f);

   };

   template<typename Gr>

   typename DynamicPath<Gr>::EdgeIt&

   DynamicPath<Gr>::next(DynamicPath::EdgeIt &e) const {

     if( e.it == edges.end() ) 

       return e;

     GraphNode common_node = ( e.forw ? G.head(*e.it) : G.tail(*e.it) );

     ++e.it;

     // Invalid edgeit is always forward :)

     if( e.it == edges.end() ) {

       e.forw = true;

       return e;

     }

     e.forw = ( G.tail(*e.it) == common_node );

     return e;

   }

   template<typename Gr>

   typename DynamicPath<Gr>::NodeIt& DynamicPath<Gr>::next(NodeIt &n) const {

     if( n.idx >= length() ) {

       // FIXME: invalid

       n.idx = length()+1;

       return n;

     }

     GraphNode next_node = ( n.tail ? G.head(edges[n.idx]) :

 			      G.tail(edges[n.idx]) );

     ++n.idx;

     if( n.idx < length() ) {

       n.tail = ( next_node == G.tail(edges[n.idx]) );

     }

     else {

       n.tail = true;

     }

     return n;

   }

   template<typename Gr>

   bool DynamicPath<Gr>::edgeIncident(const GraphEdge &e, const GraphNode &a,

 			  GraphNode &b) {

     if( G.tail(e) == a ) {

       b=G.head(e);

       return true;

     }

     if( G.head(e) == a ) {

       b=G.tail(e);

       return true;

     }

     return false;

   }

   template<typename Gr>

   bool DynamicPath<Gr>::connectTwoEdges(const GraphEdge &e,

 			     const GraphEdge &f) {

     if( edgeIncident(f, G.tail(e), _last) ) {

       _first = G.head(e);

       return true;

     }

     if( edgeIncident(f, G.head(e), _last) ) {

       _first = G.tail(e);

       return true;

     }

     return false;

   }

   template<typename Gr>

   bool DynamicPath<Gr>::pushFront(const GraphEdge &e) {

     if( G.valid(_first) ) {

 	if( edgeIncident(e, _first, _first) ) {

 	  edges.push_front(e);

 	  return true;

 	}

 	else

 	  return false;

     }

     else if( length() < 1 || connectTwoEdges(e, edges[0]) ) {

       edges.push_front(e);

       return true;

     }

     else

       return false;

   }

   template<typename Gr>

   bool DynamicPath<Gr>::pushBack(const GraphEdge &e) {

     if( G.valid(_last) ) {

 	if( edgeIncident(e, _last, _last) ) {

 	  edges.push_back(e);

 	  return true;

 	}

 	else

 	  return false;

     }

     else if( length() < 1 || connectTwoEdges(edges[0], e) ) {

       edges.push_back(e);

       return true;

     }

     else

       return false;

   }

   template<typename Gr>

   bool DynamicPath<Gr>::setFrom(const GraphNode &n) {

     if( G.valid(_first) ) {

       return _first == n;

     }

     else {

       if( length() > 0) {

 	if( edgeIncident(edges[0], n, _last) ) {

 	  _first = n;

 	  return true;

 	}

 	else return false;

       }

       else {

 	_first = _last = n;

 	return true;

       }

     }

   }

   template<typename Gr>

   bool DynamicPath<Gr>::setTo(const GraphNode &n) {

     if( G.valid(_last) ) {

       return _last == n;

     }

     else {

       if( length() > 0) {

 	if( edgeIncident(edges[0], n, _first) ) {

 	  _last = n;

 	  return true;

 	}

 	else return false;

       }

       else {

 	_first = _last = n;

 	return true;

       }

     }

   }

   template<typename Gr>

   typename DynamicPath<Gr>::NodeIt

   DynamicPath<Gr>::tail(const EdgeIt& e) const {

     NodeIt n;

     if( e.it == edges.end() ) {

       // FIXME: invalid-> invalid

       n.idx = length() + 1;

       n.tail = true;

       return n;

     }

     n.idx = e.it-edges.begin();

     n.tail = e.forw;

     return n;

   }

   template<typename Gr>

   typename DynamicPath<Gr>::NodeIt

   DynamicPath<Gr>::head(const EdgeIt& e) const {

     if( e.it == edges.end()-1 ) {

       return _last;

     }

     EdgeIt next_edge = e;

     next(next_edge);

     return tail(next_edge);

   }

   template<typename Gr>

   typename DynamicPath<Gr>::GraphEdge

   DynamicPath<Gr>::graphEdge(const EdgeIt& e) const {

     if( e.it != edges.end() ) {

       return *e.it;

     }

     else {

       return INVALID;

     }

   }

   template<typename Gr>

   typename DynamicPath<Gr>::GraphNode

   DynamicPath<Gr>::graphNode(const NodeIt& n) const {

     if( n.idx < length() ) {

       return n.tail ? G.tail(edges[n.idx]) : G.head(edges[n.idx]);

     }

     else if( n.idx == length() ) {

       return _last;

     }

     else {

       return INVALID;

     }

   }

   template<typename Gr>

   typename DynamicPath<Gr>::EdgeIt&

   DynamicPath<Gr>::nth(EdgeIt &e, size_t k) const {

     if( k>=length() ) {

       // FIXME: invalid EdgeIt

       e.it = edges.end();

       e.forw = true;

       return e;

     }

     e.it = edges.begin()+k;

     if(k==0) {

       e.forw = ( G.tail(*e.it) == _first );

     }

     else {

       e.forw = ( G.tail(*e.it) == G.tail(edges[k-1]) ||

 		 G.tail(*e.it) == G.head(edges[k-1]) );

     }

     return e;

   }

   template<typename Gr>

   typename DynamicPath<Gr>::NodeIt&

   DynamicPath<Gr>::nth(NodeIt &n, size_t k) const {

     if( k>length() ) {

       // FIXME: invalid NodeIt

       n.idx = length()+1;

       n.tail = true;

       return n;

     }

     if( k==length() ) {

       n.idx = length();

       n.tail = true;

       return n;

     }

     n = tail(nth<EdgeIt>(k));

     return n;

   }

   // Reszut konstruktorok:

   template<typename Gr>

   DynamicPath<Gr>::DynamicPath(const DynamicPath &P, const EdgeIt &a,

 			       const EdgeIt &b) :

     G(P.G), edges(a.it, b.it)    // WARNING: if b.it < a.it this will blow up! 

   {

     if( G.valid(P._first) && a.it < P.edges.end() ) {

       _first = ( a.forw ? G.tail(*a.it) : G.head(*a.it) );

       if( b.it < P.edges.end() ) {

 	_last = ( b.forw ? G.tail(*b.it) : G.head(*b.it) );

       }

       else {

 	_last = P._last;

       }

     }

   }

   template<typename Gr>

   DynamicPath<Gr>::DynamicPath(const DynamicPath &P, const NodeIt &a,

 			       const NodeIt &b) : G(P.G)

   {

     if( !P.valid(a) || !P.valid(b) )

       return;

     int ai = a.idx, bi = b.idx;

     if( bi<ai )

       std::swap(ai,bi);

     edges.resize(bi-ai);

     copy(P.edges.begin()+ai, P.edges.begin()+bi, edges.begin());

     _first = P.graphNode(a);

     _last = P.graphNode(b);

   }

   ///@}

 } // namespace hugo

 #endif // HUGO_PATH_H