#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 TQueue>
static void OutputQueue(TQueue queue);

int main(int, char*[])
{
  srand((unsigned int)time(NULL));
  srand48((unsigned int)time(NULL));

  boost::array<std::size_t, 2> lengths = { { 2,2 } };
  typedef boost::grid_graph<2> GraphType;
  GraphType graph(lengths);
  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);

  typedef boost::graph_traits<GraphType>::vertex_iterator VertexIteratorType;

  typedef boost::vector_property_map<float, GridIndexMapType> PriorityMapType;
  PriorityMapType priorityMap(gridIndexMap);
  VertexIteratorType vertexIterator, vertexIteratorEnd;

  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;
    std::cout << "New priority for " << vertexIterator->operator[](0) << ", " << vertexIterator->operator[](1) << " " << newPriority << std::endl;
    put(priorityMap, *vertexIterator, newPriority);
  }

  for( tie(vertexIterator, vertexIteratorEnd) = vertices(graph); vertexIterator != vertexIteratorEnd; ++vertexIterator)
  {
    mutableQueue.push(*vertexIterator);
  }

  std::cout << "There are " << mutableQueue.size() << " items in the queue." << std::endl;

  std::cout << "The priority queue is: " << std::endl;
  OutputQueue(mutableQueue);

  // Insert another set of random values for each vertex
  for( tie(vertexIterator, vertexIteratorEnd) = vertices(graph); vertexIterator != vertexIteratorEnd; ++vertexIterator)
  {
    float newPriority = rand() % 1000;
    std::cout << "New priority for " << vertexIterator->operator[](0) << ", " << vertexIterator->operator[](1) << " " << newPriority << std::endl;
    put(priorityMap, *vertexIterator, newPriority);
    mutableQueue.update(*vertexIterator);
  }

  std::cout << "There are " << mutableQueue.size() << " items in the queue." << std::endl;

  std::cout << "The priority queue is: " << std::endl;
//  OutputQueue(mutableQueue);

  while( ! mutableQueue.empty() )
  {
    MutableQueueType::value_type u = mutableQueue.top();

    // These two lines are equivalent
    std::cout << "vertex: " << u[0] << " " << u[1]
              << " priority: " << get(mutableQueue.keys(), u)
//              << " indexInHeap: " << get(queue.index_in_heap(), u) // private
              << " indexInHeap: " << get(index_in_heap, u)
              << std::endl;

    mutableQueue.pop();
  }

  std::cout << std::endl;

  return 0;
}

template <typename TQueue>
static void OutputQueue(TQueue queue)
{
  while( ! queue.empty() )
  {
    typename TQueue::value_type u = queue.top();

    // These two lines are equivalent
    std::cout << "vertex: " << u[0] << " " << u[1]
              << " priority: " << get(queue.keys(), u)
              << std::endl;

    queue.pop();
  }
}