//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2004, 2005 Trustees of Indiana University
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek,
//          Doug Gregor, D. Kevin McGrath
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================//
#ifndef BOOST_GRAPH_KING_HPP
#define BOOST_GRAPH_KING_HPP

#include <boost/config.hpp>
#include <boost/graph/detail/sparse_ordering.hpp>
#include <boost/graph/graph_utility.hpp>

/*
  King Algorithm for matrix reordering
*/

namespace boost {
  namespace detail {
    template<typename OutputIterator, typename Buffer, typename Compare, 
             typename PseudoDegreeMap, typename VecMap, typename VertexIndexMap>
    class bfs_king_visitor:public default_bfs_visitor
    {
    public:
      bfs_king_visitor(OutputIterator *iter, Buffer *b, Compare compare, 
                       PseudoDegreeMap deg, std::vector<int> loc, VecMap color, 
                       VertexIndexMap vertices): 
        permutation(iter), Qptr(b), degree(deg), comp(compare), 
        Qlocation(loc), colors(color), vertex_map(vertices) { }
      
      template <typename Vertex, typename Graph>
      void finish_vertex(Vertex, Graph& g) {
        typename graph_traits<Graph>::out_edge_iterator ei, ei_end;
        Vertex v, w;

        typedef typename std::deque<Vertex>::iterator iterator;
        typedef typename std::deque<Vertex>::reverse_iterator reverse_iterator;

        reverse_iterator rend = Qptr->rend()-index_begin;
        reverse_iterator rbegin = Qptr->rbegin();


        //heap the vertices already there
        std::make_heap(rbegin, rend, boost::bind<bool>(comp, _2, _1));

        unsigned i = 0;
        
        for(i = index_begin; i != Qptr->size(); ++i){
          colors[get(vertex_map, (*Qptr)[i])] = 1;
          Qlocation[get(vertex_map, (*Qptr)[i])] = i;
        }

        i = 0;

        for( ; rbegin != rend; rend--){
          percolate_down<Vertex>(i);
          w = (*Qptr)[index_begin+i];
          for (boost::tie(ei, ei_end) = out_edges(w, g); ei != ei_end; ++ei) {
            v = target(*ei, g);
            put(degree, v, get(degree, v) - 1);
    
            if (colors[get(vertex_map, v)] == 1) {
              percolate_up<Vertex>(get(vertex_map, v), i);            
            }
          }
          
          colors[get(vertex_map, w)] = 0;
          i++;
        }
      }
    
      template <typename Vertex, typename Graph>
      void examine_vertex(Vertex u, const Graph&) {
        
        *(*permutation)++ = u;
        index_begin = Qptr->size();
        
      }
    protected:


      //this function replaces pop_heap, and tracks state information
      template <typename Vertex>
      void percolate_down(int offset){
        typedef typename std::deque<Vertex>::reverse_iterator reverse_iterator;
        
        int heap_last = index_begin + offset;
        int heap_first = Qptr->size() - 1;
        
        //pop_heap functionality:
        //swap first, last
        std::swap((*Qptr)[heap_last], (*Qptr)[heap_first]);
        
        //swap in the location queue
        std::swap(Qlocation[heap_first], Qlocation[heap_last]);

        //set drifter, children
        int drifter = heap_first;
        int drifter_heap = Qptr->size() - drifter;

        int right_child_heap = drifter_heap * 2 + 1;
        int right_child = Qptr->size() - right_child_heap;

        int left_child_heap = drifter_heap * 2;
        int left_child = Qptr->size() - left_child_heap;

        //check that we are staying in the heap
        bool valid = (right_child < heap_last) ? false : true;
        
        //pick smallest child of drifter, and keep in mind there might only be left child
        int smallest_child = (valid && get(degree, (*Qptr)[left_child]) > get(degree,(*Qptr)[right_child])) ? 
          right_child : left_child;
        
        while(valid && smallest_child < heap_last && comp((*Qptr)[drifter], (*Qptr)[smallest_child])){
          
          //if smallest child smaller than drifter, swap them
          std::swap((*Qptr)[smallest_child], (*Qptr)[drifter]);
          std::swap(Qlocation[drifter], Qlocation[smallest_child]);

          //update the values, run again, as necessary
          drifter = smallest_child;
          drifter_heap = Qptr->size() - drifter;

          right_child_heap = drifter_heap * 2 + 1;
          right_child = Qptr->size() - right_child_heap;

          left_child_heap = drifter_heap * 2;
          left_child = Qptr->size() - left_child_heap;

          valid = (right_child < heap_last) ? false : true;

          smallest_child = (valid && get(degree, (*Qptr)[left_child]) > get(degree,(*Qptr)[right_child])) ? 
            right_child : left_child;
        }

      }


      // this is like percolate down, but we always compare against the
      // parent, as there is only a single choice
      template <typename Vertex>
      void percolate_up(int vertex, int offset){
        
        int child_location = Qlocation[vertex];
        int heap_child_location = Qptr->size() - child_location;
        int heap_parent_location = (int)(heap_child_location/2);
        unsigned parent_location = Qptr->size() - heap_parent_location; 

        bool valid = (heap_parent_location != 0 && child_location > index_begin + offset && 
                      parent_location < Qptr->size());

        while(valid && comp((*Qptr)[child_location], (*Qptr)[parent_location])){
          
          //swap in the heap
          std::swap((*Qptr)[child_location], (*Qptr)[parent_location]);
          
          //swap in the location queue
          std::swap(Qlocation[child_location], Qlocation[parent_location]);

          child_location = parent_location;
          heap_child_location = heap_parent_location;
          heap_parent_location = (int)(heap_child_location/2);
          parent_location = Qptr->size() - heap_parent_location; 
          valid = (heap_parent_location != 0 && child_location > index_begin + offset);
        }
      }
      
      OutputIterator *permutation;
      int index_begin;
      Buffer *Qptr;
      PseudoDegreeMap degree;
      Compare comp;
      std::vector<int> Qlocation;
      VecMap colors;
      VertexIndexMap vertex_map;
    };
  

  } // namespace detail  
  

  template<class Graph, class OutputIterator, class ColorMap, class DegreeMap,
           typename VertexIndexMap> 
  OutputIterator
  king_ordering(const Graph& g,
                std::deque< typename graph_traits<Graph>::vertex_descriptor >
                  vertex_queue,
                OutputIterator permutation, 
                ColorMap color, DegreeMap degree,
                VertexIndexMap index_map)
  {
    typedef typename property_traits<DegreeMap>::value_type ds_type;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef iterator_property_map<typename std::vector<ds_type>::iterator, VertexIndexMap, ds_type, ds_type&> PseudoDegreeMap;
    typedef indirect_cmp<PseudoDegreeMap, std::less<ds_type> > Compare;
    typedef typename boost::sparse::sparse_ordering_queue<Vertex> queue;
    typedef typename detail::bfs_king_visitor<OutputIterator, queue, Compare,             
      PseudoDegreeMap, std::vector<int>, VertexIndexMap > Visitor;
    typedef typename graph_traits<Graph>::vertices_size_type
      vertices_size_type;
    std::vector<ds_type> pseudo_degree_vec(num_vertices(g));
    PseudoDegreeMap pseudo_degree(pseudo_degree_vec.begin(), index_map);
    
    typename graph_traits<Graph>::vertex_iterator ui, ui_end;    
    queue Q;
    // Copy degree to pseudo_degree
    // initialize the color map
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui){
      put(pseudo_degree, *ui, get(degree, *ui));
      put(color, *ui, Color::white());
    }
    
    Compare comp(pseudo_degree);
    std::vector<int> colors(num_vertices(g));

    for(vertices_size_type i = 0; i < num_vertices(g); i++) 
      colors[i] = 0;

    std::vector<int> loc(num_vertices(g));

    //create the visitor
    Visitor vis(&permutation, &Q, comp, pseudo_degree, loc, colors, index_map);
    
    while( !vertex_queue.empty() ) {
      Vertex s = vertex_queue.front();
      vertex_queue.pop_front();
      
      //call BFS with visitor
      breadth_first_visit(g, s, Q, vis, color);
    }

    return permutation;
  }

  
  // This is the case where only a single starting vertex is supplied.
  template <class Graph, class OutputIterator,
            class ColorMap, class DegreeMap, typename VertexIndexMap>
  OutputIterator
  king_ordering(const Graph& g,
                typename graph_traits<Graph>::vertex_descriptor s,
                OutputIterator permutation, 
                ColorMap color, DegreeMap degree, VertexIndexMap index_map)
  {

    std::deque< typename graph_traits<Graph>::vertex_descriptor > vertex_queue;
    vertex_queue.push_front( s );
    return king_ordering(g, vertex_queue, permutation, color, degree,
                         index_map);
  }

  
  template < class Graph, class OutputIterator, 
             class ColorMap, class DegreeMap, class VertexIndexMap>
  OutputIterator 
  king_ordering(const Graph& G, OutputIterator permutation, 
                ColorMap color, DegreeMap degree, VertexIndexMap index_map)
  {
    if (has_no_vertices(G))
      return permutation;

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename boost::graph_traits<Graph>::vertex_iterator   VerIter;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;

    std::deque<Vertex>      vertex_queue;

    // Mark everything white
    BGL_FORALL_VERTICES_T(v, G, Graph) put(color, v, Color::white());

    // Find one vertex from each connected component 
    BGL_FORALL_VERTICES_T(v, G, Graph) {
      if (get(color, v) == Color::white()) {
        depth_first_visit(G, v, dfs_visitor<>(), color);
        vertex_queue.push_back(v);
      }
    }

    // Find starting nodes for all vertices
    // TBD: How to do this with a directed graph?
    for (typename std::deque<Vertex>::iterator i = vertex_queue.begin();
         i != vertex_queue.end(); ++i)
      *i = find_starting_node(G, *i, color, degree);
    
    return king_ordering(G, vertex_queue, permutation, color, degree,
                         index_map);
  }

  template<typename Graph, typename OutputIterator, typename VertexIndexMap>
  OutputIterator 
  king_ordering(const Graph& G, OutputIterator permutation, 
                VertexIndexMap index_map)
  {
    if (has_no_vertices(G))
      return permutation;

    typedef out_degree_property_map<Graph> DegreeMap;
    std::vector<default_color_type> colors(num_vertices(G));
    return king_ordering(G, permutation, 
                         make_iterator_property_map(&colors[0], index_map,
                                                    colors[0]),
                         make_out_degree_map(G), index_map);
  }

  template<typename Graph, typename OutputIterator>
  inline OutputIterator 
  king_ordering(const Graph& G, OutputIterator permutation)
  { return king_ordering(G, permutation, get(vertex_index, G)); }

} // namespace boost


#endif // BOOST_GRAPH_KING_HPP