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

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