#include <iostream>
#include <iomanip>
 
#include <boost/graph/grid_graph.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/property_map/property_map.hpp>
 
#include <cstdlib>
 
template <typename TGraph, typename TIndexInHeapMap>
static void OutputIndexHeapMap(TGraph g, TIndexInHeapMap indexHeapMap);
 
int main(int, char*[])
{
  // Seed random number gererators
  srand((unsigned int)time(NULL));
  srand48((unsigned int)time(NULL));
 
  // Create a graph
  boost::array<std::size_t, 2> lengths = { { 2,2 } };
  typedef boost::grid_graph<2> GraphType;
  GraphType graph(lengths);
 
  // Declare iterators that we will use
  typedef boost::graph_traits<GraphType>::vertex_iterator VertexIteratorType;
 
  VertexIteratorType vertexIterator, vertexIteratorEnd;
 
  // Setup the graph properties
  typedef boost::graph_traits<GraphType>::vertex_descriptor Vertex;
  typedef boost::property_map<GraphType, boost::vertex_index_t>::const_type GridIndexMapType;
  GridIndexMapType gridIndexMap(get(boost::vertex_index, graph));
 
  typedef boost::vector_property_map<std::size_t, GridIndexMapType> IndexInHeapMap;
  IndexInHeapMap index_in_heap(gridIndexMap);
 
  // Initialize
  for( tie(vertexIterator, vertexIteratorEnd) = vertices(graph); vertexIterator != vertexIteratorEnd; ++vertexIterator)
  {
    put(index_in_heap, *vertexIterator, (size_t)(-1));
  }
 
  std::cout << "Initial heap map" << std::endl;
  OutputIndexHeapMap(graph, index_in_heap);
 
  typedef boost::vector_property_map<float, GridIndexMapType> PriorityMapType;
  PriorityMapType priorityMap(gridIndexMap);
 
  // Setup the queue
  typedef std::greater<float> ComparisonFunctor;
  typedef boost::d_ary_heap_indirect<Vertex, 4, IndexInHeapMap,
      PriorityMapType, ComparisonFunctor > MutableQueueType;
 
  ComparisonFunctor comparisonFunctor;
  MutableQueueType mutableQueue(priorityMap, index_in_heap, comparisonFunctor);
 
  std::cout << "There are " << mutableQueue.size() << " items in the queue." << std::endl;
 
  // Add random values to the vertices and add them to the queue
  for( tie(vertexIterator, vertexIteratorEnd) = vertices(graph); vertexIterator != vertexIteratorEnd; ++vertexIterator)
  {
    float newPriority = rand() % 1000;
    Vertex v = *vertexIterator;
 
    std::cout << "Original priority for " << v[0] << ", " << v[1] << " " << newPriority << std::endl;
    put(priorityMap, *vertexIterator, newPriority);
    mutableQueue.push(*vertexIterator);
  }
 
  std::cout << "Heap map after initial pushes:" << std::endl;
  OutputIndexHeapMap(graph, index_in_heap);
 
  std::cout << "There are " << mutableQueue.size() << " items in the queue." << std::endl;
 
  // Insert another set of random values for each vertex
  for( tie(vertexIterator, vertexIteratorEnd) = vertices(graph); vertexIterator != vertexIteratorEnd; ++vertexIterator)
  {
    float newPriority = rand() % 1000;
 
    Vertex v = *vertexIterator;
    std::cout << "New priority for " << v[0] << ", " << v[1] << " " << newPriority << std::endl;
    put(priorityMap, *vertexIterator, newPriority);
    mutableQueue.update(*vertexIterator);
  }
 
  std::cout << "Heap map after updates:" << std::endl;
  OutputIndexHeapMap(graph, index_in_heap);
 
  std::cout << "There are " << mutableQueue.size() << " items in the queue." << std::endl;
 
  // Output (and empty) the queue
  while( ! mutableQueue.empty() )
  {
//    MutableQueueType::value_type u = mutableQueue.top();
    Vertex u = mutableQueue.top();
 
    // These two lines are equivalent
    std::cout << "vertex: " << u[0] << " " << u[1]
              << " priority: " << get(mutableQueue.keys(), u)
              << std::endl;
 
    mutableQueue.pop();
  }
 
  std::cout << "Heap map at end:" << std::endl; // All (size_t)(-1)
  OutputIndexHeapMap(graph, index_in_heap);
 
  std::cout << std::endl;
 
  return 0;
}
 
template <typename TGraph, typename TIndexInHeapMap>
static void OutputIndexHeapMap(TGraph g, TIndexInHeapMap indexHeapMap)
{
  typedef typename boost::graph_traits<TGraph>::vertex_iterator VertexIteratorType;
  VertexIteratorType vertexIterator, vertexIteratorEnd;
  for( tie(vertexIterator, vertexIteratorEnd) = vertices(g); vertexIterator != vertexIteratorEnd; ++vertexIterator)
  {
    std::cout << "Index: " << get(indexHeapMap, *vertexIterator) << std::endl;
  }
}