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

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