#ifndef MARCI_BFS_HH
#define MARCI_BFS_HH

#include <queue>

#include <marci_property_vector.hh>

namespace hugo {

  template <typename graph_type>
  struct bfs {
    typedef typename graph_type::node_iterator node_iterator;
    typedef typename graph_type::edge_iterator edge_iterator;
    typedef typename graph_type::each_node_iterator each_node_iterator;
    typedef typename graph_type::out_edge_iterator out_edge_iterator;
    graph_type& G;
    node_iterator s;
    node_property_vector<graph_type, bool> reached;
    node_property_vector<graph_type, edge_iterator> pred;
    node_property_vector<graph_type, int> dist;
    std::queue<node_iterator> bfs_queue;
    bfs(graph_type& _G, node_iterator _s) : G(_G), s(_s), reached(_G), pred(_G), dist(_G) { 
      bfs_queue.push(s); 
      for(each_node_iterator i=G.first_node(); i.valid(); ++i) 
	reached.put(i, false);
      reached.put(s, true);
      dist.put(s, 0); 
    }
    
    void run() {
      while (!bfs_queue.empty()) {
	node_iterator v=bfs_queue.front();
	out_edge_iterator e=G.first_out_edge(v);
	bfs_queue.pop();
	for( ; e.valid(); ++e) {
	  node_iterator w=G.head(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.put(w, dist.get(v)+1);
	    pred.put(w, e);
	    reached.put(w, true);
	  } else {
	    std::cout << G.id(w) << " is already reached" << std::endl;
	  }
	}
      }
    }
  };

  template <typename graph_type> 
  struct bfs_visitor {
    typedef typename graph_type::node_iterator node_iterator;
    typedef typename graph_type::edge_iterator edge_iterator;
    typedef typename graph_type::out_edge_iterator out_edge_iterator;
    graph_type& G;
    bfs_visitor(graph_type& _G) : G(_G) { }
    void at_previously_reached(out_edge_iterator& e) { 
      //node_iterator v=G.tail(e);
      node_iterator w=G.head(e);
      std::cout << G.id(w) << " is already reached" << std::endl;
   }
    void at_newly_reached(out_edge_iterator& e) { 
      //node_iterator v=G.tail(e);
      node_iterator w=G.head(e);
      std::cout << G.id(w) << " is newly reached :-)" << std::endl;
    }
  };

  template <typename graph_type, typename reached_type, typename visitor_type>
  struct bfs_iterator {
    typedef typename graph_type::node_iterator node_iterator;
    typedef typename graph_type::edge_iterator edge_iterator;
    typedef typename graph_type::out_edge_iterator out_edge_iterator;
    graph_type& G;
    std::queue<out_edge_iterator>& bfs_queue;
    reached_type& reached;
    visitor_type& visitor;
    void process() {
      while ( !bfs_queue.empty() && !bfs_queue.front().valid() ) { bfs_queue.pop(); } 
      if (bfs_queue.empty()) return;
      out_edge_iterator e=bfs_queue.front();
      //node_iterator v=G.tail(e);
      node_iterator w=G.head(e);
      if (!reached.get(w)) {
	visitor.at_newly_reached(e);
	bfs_queue.push(G.first_out_edge(w));
	reached.put(w, true);
      } else {
	visitor.at_previously_reached(e);
      }
    }
    bfs_iterator(graph_type& _G, std::queue<out_edge_iterator>& _bfs_queue, reached_type& _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_type, reached_type, 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 edge_iterator () { return bfs_queue.front(); }

  };

  template <typename graph_type, typename reached_type>
  struct bfs_iterator1 {
    typedef typename graph_type::node_iterator node_iterator;
    typedef typename graph_type::edge_iterator edge_iterator;
    typedef typename graph_type::out_edge_iterator out_edge_iterator;
    graph_type& G;
    std::queue<out_edge_iterator>& bfs_queue;
    reached_type& reached;
    bool _newly_reached;
    bfs_iterator1(graph_type& _G, std::queue<out_edge_iterator>& _bfs_queue, reached_type& _reached) : G(_G), bfs_queue(_bfs_queue), reached(_reached) { 
      valid();
      if (!bfs_queue.empty() && bfs_queue.front().valid()) { 
	out_edge_iterator e=bfs_queue.front();
	node_iterator w=G.head(e);
	if (!reached.get(w)) {
	  bfs_queue.push(G.first_out_edge(w));
	  reached.put(w, true);
	  _newly_reached=true;
	} else {
	  _newly_reached=false;
	}
      }
    }
    bfs_iterator1<graph_type, reached_type>& operator++() { 
      if (!valid()) return *this;
      ++(bfs_queue.front());
      valid();
      if (!bfs_queue.empty() && bfs_queue.front().valid()) { 
	out_edge_iterator e=bfs_queue.front();
	node_iterator w=G.head(e);
	if (!reached.get(w)) {
	  bfs_queue.push(G.first_out_edge(w));
	  reached.put(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 edge_iterator () { return bfs_queue.front(); }
    bool newly_reached() { return _newly_reached; }

  };

} // namespace hugo

#endif //MARCI_BFS_HH