//=======================================================================

 // 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>

 /*

   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 (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 (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 (vertices(G).first == vertices(G).second)

       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 (vertices(G).first == vertices(G).second)

       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