#ifndef BFS_ITERATOR_HH
#define BFS_ITERATOR_HH

#include <queue>
#include <stack>

namespace marci {

  template <typename Graph>
  struct bfs {
    typedef typename Graph::NodeIt NodeIt;
    typedef typename Graph::EdgeIt EdgeIt;
    typedef typename Graph::EachNodeIt EachNodeIt;
    typedef typename Graph::OutEdgeIt OutEdgeIt;
    Graph& G;
    NodeIt s;
    typename Graph::NodeMap<bool> reached;
    typename Graph::NodeMap<EdgeIt> pred;
    typename Graph::NodeMap<int> dist;
    std::queue<NodeIt> bfs_queue;
    bfs(Graph& _G, NodeIt _s) : G(_G), s(_s), reached(_G), pred(_G), dist(_G) { 
      bfs_queue.push(s); 
      for(EachNodeIt i=G.template first<EachNodeIt>(); i.valid(); ++i) 
	reached.set(i, false);
      reached.set(s, true);
      dist.set(s, 0); 
    }
    
    void run() {
      while (!bfs_queue.empty()) {
	NodeIt v=bfs_queue.front();
	OutEdgeIt e=G.template first<OutEdgeIt>(v);
	bfs_queue.pop();
	for( ; e.valid(); ++e) {
	  NodeIt w=G.bNode(e);
	  std::cout << "scan node " << G.id(w) << " from node " << G.id(v) << std::endl;
	  if (!reached.get(w)) {
	    std::cout << G.id(w) << " is newly reached :-)" << std::endl;
	    bfs_queue.push(w);
	    dist.set(w, dist.get(v)+1);
	    pred.set(w, e);
	    reached.set(w, true);
	  } else {
	    std::cout << G.id(w) << " is already reached" << std::endl;
	  }
	}
      }
    }
  };

  template <typename Graph> 
  struct bfs_visitor {
    typedef typename Graph::NodeIt NodeIt;
    typedef typename Graph::EdgeIt EdgeIt;
    typedef typename Graph::OutEdgeIt OutEdgeIt;
    Graph& G;
    bfs_visitor(Graph& _G) : G(_G) { }
    void at_previously_reached(OutEdgeIt& e) { 
      //NodeIt v=G.aNode(e);
      NodeIt w=G.bNode(e);
      std::cout << G.id(w) << " is already reached" << std::endl;
   }
    void at_newly_reached(OutEdgeIt& e) { 
      //NodeIt v=G.aNode(e);
      NodeIt w=G.bNode(e);
      std::cout << G.id(w) << " is newly reached :-)" << std::endl;
    }
  };

  template <typename Graph, typename ReachedMap, typename visitor_type>
  struct bfs_iterator {
    typedef typename Graph::NodeIt NodeIt;
    typedef typename Graph::EdgeIt EdgeIt;
    typedef typename Graph::OutEdgeIt OutEdgeIt;
    Graph& G;
    std::queue<OutEdgeIt>& bfs_queue;
    ReachedMap& reached;
    visitor_type& visitor;
    void process() {
      while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
      if (bfs_queue.empty()) return;
      OutEdgeIt e=bfs_queue.front();
      //NodeIt v=G.aNode(e);
      NodeIt w=G.bNode(e);
      if (!reached.get(w)) {
	visitor.at_newly_reached(e);
	bfs_queue.push(G.template first<OutEdgeIt>(w));
	reached.set(w, true);
      } else {
	visitor.at_previously_reached(e);
      }
    }
    bfs_iterator(Graph& _G, std::queue<OutEdgeIt>& _bfs_queue, ReachedMap& _reached, visitor_type& _visitor) : G(_G), bfs_queue(_bfs_queue), reached(_reached), visitor(_visitor) { 
      //while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
      valid();
    }
    bfs_iterator<Graph, ReachedMap, visitor_type>& operator++() { 
      //while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
      //if (bfs_queue.empty()) return *this;
      if (!valid()) return *this;
      ++(bfs_queue.front());
      //while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
      valid();
      return *this;
    }
    //void next() { 
    //  while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
    //  if (bfs_queue.empty()) return;
    //  ++(bfs_queue.front());
    //  while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
    //}
    bool valid() { 
      while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
      if (bfs_queue.empty()) return false; else return true;
    }
    //bool finished() { 
    //  while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
    //  if (bfs_queue.empty()) return true; else return false;
    //}
    operator EdgeIt () { return bfs_queue.front(); }

  };

  template <typename Graph, typename ReachedMap>
  struct bfs_iterator1 {
    typedef typename Graph::NodeIt NodeIt;
    typedef typename Graph::EdgeIt EdgeIt;
    typedef typename Graph::OutEdgeIt OutEdgeIt;
    Graph& G;
    std::queue<OutEdgeIt>& bfs_queue;
    ReachedMap& reached;
    bool _newly_reached;
    bfs_iterator1(Graph& _G, std::queue<OutEdgeIt>& _bfs_queue, ReachedMap& _reached) : G(_G), bfs_queue(_bfs_queue), reached(_reached) { 
      valid();
      if (!bfs_queue.empty() && bfs_queue.front().valid()) { 
	OutEdgeIt e=bfs_queue.front();
	NodeIt w=G.bNode(e);
	if (!reached.get(w)) {
	  bfs_queue.push(G.template first<OutEdgeIt>(w));
	  reached.set(w, true);
	  _newly_reached=true;
	} else {
	  _newly_reached=false;
	}
      }
    }
    bfs_iterator1<Graph, ReachedMap>& operator++() { 
      if (!valid()) return *this;
      ++(bfs_queue.front());
      valid();
      if (!bfs_queue.empty() && bfs_queue.front().valid()) { 
	OutEdgeIt e=bfs_queue.front();
	NodeIt w=G.bNode(e);
	if (!reached.get(w)) {
	  bfs_queue.push(G.template first<OutEdgeIt>(w));
	  reached.set(w, true);
	  _newly_reached=true;
	} else {
	  _newly_reached=false;
	}
      }
      return *this;
    }
    bool valid() { 
      while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
      if (bfs_queue.empty()) return false; else return true;
    }
    operator OutEdgeIt() { return bfs_queue.front(); }
    //ize
    bool newly_reached() { return _newly_reached; }

  };

  template <typename Graph, typename OutEdgeIt, typename ReachedMap>
  struct BfsIterator {
    typedef typename Graph::NodeIt NodeIt;
    Graph& G;
    std::queue<OutEdgeIt>& bfs_queue;
    ReachedMap& reached;
    bool b_node_newly_reached;
    OutEdgeIt actual_edge;
    BfsIterator(Graph& _G, 
		std::queue<OutEdgeIt>& _bfs_queue, 
		ReachedMap& _reached) : 
      G(_G), bfs_queue(_bfs_queue), reached(_reached) { 
      actual_edge=bfs_queue.front();
      if (actual_edge.valid()) { 
	NodeIt w=G.bNode(actual_edge);
	if (!reached.get(w)) {
	  bfs_queue.push(G.firstOutEdge(w));
	  reached.set(w, true);
	  b_node_newly_reached=true;
	} else {
	  b_node_newly_reached=false;
	}
      }
    }
    BfsIterator<Graph, OutEdgeIt, ReachedMap>& 
    operator++() { 
      if (bfs_queue.front().valid()) { 
	++(bfs_queue.front());
	actual_edge=bfs_queue.front();
	if (actual_edge.valid()) {
	  NodeIt w=G.bNode(actual_edge);
	  if (!reached.get(w)) {
	    bfs_queue.push(G.firstOutEdge(w));
	    reached.set(w, true);
	    b_node_newly_reached=true;
	  } else {
	    b_node_newly_reached=false;
	  }
	}
      } else {
	bfs_queue.pop(); 
	actual_edge=bfs_queue.front();
	if (actual_edge.valid()) {
	  NodeIt w=G.bNode(actual_edge);
	  if (!reached.get(w)) {
	    bfs_queue.push(G.firstOutEdge(w));
	    reached.set(w, true);
	    b_node_newly_reached=true;
	  } else {
	    b_node_newly_reached=false;
	  }
	}
      }
      return *this;
    }
    bool finished() { return bfs_queue.empty(); }
    operator OutEdgeIt () { return actual_edge; }
    bool bNodeIsNewlyReached() { return b_node_newly_reached; }
    bool aNodeIsExamined() { return !(actual_edge.valid()); }
  };


  template <typename Graph, typename OutEdgeIt, typename ReachedMap>
  struct DfsIterator {
    typedef typename Graph::NodeIt NodeIt;
    Graph& G;
    std::stack<OutEdgeIt>& bfs_queue;
    ReachedMap& reached;
    bool b_node_newly_reached;
    OutEdgeIt actual_edge;
    DfsIterator(Graph& _G, 
		std::stack<OutEdgeIt>& _bfs_queue, 
		ReachedMap& _reached) : 
      G(_G), bfs_queue(_bfs_queue), reached(_reached) { 
      actual_edge=bfs_queue.top();
      if (actual_edge.valid()) { 
	NodeIt w=G.bNode(actual_edge);
	if (!reached.get(w)) {
	  bfs_queue.push(G.firstOutEdge(w));
	  reached.set(w, true);
	  b_node_newly_reached=true;
	} else {
	  ++(bfs_queue.top());
	  b_node_newly_reached=false;
	}
      } else {
	bfs_queue.pop();
      }
    }
    DfsIterator<Graph, OutEdgeIt, ReachedMap>& 
    operator++() { 
      actual_edge=bfs_queue.top();
      if (actual_edge.valid()) { 
	NodeIt w=G.bNode(actual_edge);
	if (!reached.get(w)) {
	  bfs_queue.push(G.firstOutEdge(w));
	  reached.set(w, true);
	  b_node_newly_reached=true;
	} else {
	  ++(bfs_queue.top());
	  b_node_newly_reached=false;
	}
      } else {
	bfs_queue.pop();
      }
      return *this;
    }
    bool finished() { return bfs_queue.empty(); }
    operator OutEdgeIt () { return actual_edge; }
    bool bNodeIsNewlyReached() { return b_node_newly_reached; }
    bool aNodeIsLeaved() { return !(actual_edge.valid()); }
  };

  template <typename Graph, typename OutEdgeIt, typename ReachedMap>
  struct BfsIterator1 {
    typedef typename Graph::NodeIt NodeIt;
    Graph& G;
    std::queue<OutEdgeIt>& bfs_queue;
    ReachedMap& reached;
    bool b_node_newly_reached;
    OutEdgeIt actual_edge;
    BfsIterator1(Graph& _G, 
		std::queue<OutEdgeIt>& _bfs_queue, 
		ReachedMap& _reached) : 
      G(_G), bfs_queue(_bfs_queue), reached(_reached) { 
      actual_edge=bfs_queue.front();
      if (actual_edge.valid()) { 
      	NodeIt w=G.bNode(actual_edge);
	if (!reached.get(w)) {
	  bfs_queue.push(OutEdgeIt(G, w));
	  reached.set(w, true);
	  b_node_newly_reached=true;
	} else {
	  b_node_newly_reached=false;
	}
      }
    }
    void next() { 
      if (bfs_queue.front().valid()) { 
	++(bfs_queue.front());
	actual_edge=bfs_queue.front();
	if (actual_edge.valid()) {
	  NodeIt w=G.bNode(actual_edge);
	  if (!reached.get(w)) {
	    bfs_queue.push(OutEdgeIt(G, w));
	    reached.set(w, true);
	    b_node_newly_reached=true;
	  } else {
	    b_node_newly_reached=false;
	  }
	}
      } else {
	bfs_queue.pop(); 
	actual_edge=bfs_queue.front();
	if (actual_edge.valid()) {
	  NodeIt w=G.bNode(actual_edge);
	  if (!reached.get(w)) {
	    bfs_queue.push(OutEdgeIt(G, w));
	    reached.set(w, true);
	    b_node_newly_reached=true;
	  } else {
	    b_node_newly_reached=false;
	  }
	}
      }
      //return *this;
    }
    bool finished() { return bfs_queue.empty(); }
    operator OutEdgeIt () { return actual_edge; }
    bool bNodeIsNewlyReached() { return b_node_newly_reached; }
    bool aNodeIsExamined() { return !(actual_edge.valid()); }
  };


  template <typename Graph, typename OutEdgeIt, typename ReachedMap>
  struct DfsIterator1 {
    typedef typename Graph::NodeIt NodeIt;
    Graph& G;
    std::stack<OutEdgeIt>& bfs_queue;
    ReachedMap& reached;
    bool b_node_newly_reached;
    OutEdgeIt actual_edge;
    DfsIterator1(Graph& _G, 
		std::stack<OutEdgeIt>& _bfs_queue, 
		ReachedMap& _reached) : 
      G(_G), bfs_queue(_bfs_queue), reached(_reached) { 
      //actual_edge=bfs_queue.top();
      //if (actual_edge.valid()) { 
      //	NodeIt w=G.bNode(actual_edge);
      //if (!reached.get(w)) {
      //  bfs_queue.push(OutEdgeIt(G, w));
      //  reached.set(w, true);
      //  b_node_newly_reached=true;
      //} else {
      //  ++(bfs_queue.top());
      //  b_node_newly_reached=false;
      //}
      //} else {
      //	bfs_queue.pop();
      //}
    }
    void next() { 
      actual_edge=bfs_queue.top();
      if (actual_edge.valid()) { 
	NodeIt w=G.bNode(actual_edge);
	if (!reached.get(w)) {
	  bfs_queue.push(OutEdgeIt(G, w));
	  reached.set(w, true);
	  b_node_newly_reached=true;
	} else {
	  ++(bfs_queue.top());
	  b_node_newly_reached=false;
	}
      } else {
	bfs_queue.pop();
      }
      //return *this;
    }
    bool finished() { return bfs_queue.empty(); }
    operator OutEdgeIt () { return actual_edge; }
    bool bNodeIsNewlyReached() { return b_node_newly_reached; }
    bool aNodeIsLeaved() { return !(actual_edge.valid()); }
  };

  template <typename Graph, typename OutEdgeIt, typename ReachedMap>
  class BfsIterator2 {
    typedef typename Graph::NodeIt NodeIt;
    const Graph& G;
    std::queue<OutEdgeIt> bfs_queue;
    ReachedMap reached;
    bool b_node_newly_reached;
    OutEdgeIt actual_edge;
  public:
    BfsIterator2(const Graph& _G) : G(_G), reached(G, false) { }
    void pushAndSetReached(const NodeIt s) { 
      reached.set(s, true);
      if (bfs_queue.empty()) {
	bfs_queue.push(G.template first<OutEdgeIt>(s));
	actual_edge=bfs_queue.front();
	if (actual_edge.valid()) { 
	  NodeIt w=G.bNode(actual_edge);
	  if (!reached.get(w)) {
	    bfs_queue.push(G.template first<OutEdgeIt>(w));
	    reached.set(w, true);
	    b_node_newly_reached=true;
	  } else {
	    b_node_newly_reached=false;
	  }
	} //else {
	//}
      } else {
	bfs_queue.push(G.template first<OutEdgeIt>(s));
      }
    }
    BfsIterator2<Graph, OutEdgeIt, ReachedMap>& 
    operator++() { 
      if (bfs_queue.front().valid()) { 
	++(bfs_queue.front());
	actual_edge=bfs_queue.front();
	if (actual_edge.valid()) {
	  NodeIt w=G.bNode(actual_edge);
	  if (!reached.get(w)) {
	    bfs_queue.push(G.template first<OutEdgeIt>(w));
	    reached.set(w, true);
	    b_node_newly_reached=true;
	  } else {
	    b_node_newly_reached=false;
	  }
	}
      } else {
	bfs_queue.pop(); 
	if (!bfs_queue.empty()) {
	  actual_edge=bfs_queue.front();
	} else {
	  actual_edge=OutEdgeIt();
	}
	if (actual_edge.valid()) {
	  NodeIt w=G.bNode(actual_edge);
	  if (!reached.get(w)) {
	    bfs_queue.push(G.template first<OutEdgeIt>(w));
	    reached.set(w, true);
	    b_node_newly_reached=true;
	  } else {
	    b_node_newly_reached=false;
	  }
	}
      }
      return *this;
    }
    bool finished() const { return bfs_queue.empty(); }
    operator OutEdgeIt () const { return actual_edge; }
    bool isBNodeNewlyReached() const { return b_node_newly_reached; }
    bool isANodeExamined() const { return !(actual_edge.valid()); }
    const ReachedMap& getReachedMap() const { return reached; }
    const std::queue<OutEdgeIt>& getBfsQueue() const { return bfs_queue; }
 };


} // namespace marci

#endif //BFS_ITERATOR_HH