#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);

  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();
  }
}

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