priority_queue.h

#ifndef PRIORITY_QUEUE_H_
#define PRIORITY_QUEUE_H_

#include <iostream>

#include <vector>
#include <unordered_map>

#include <utility>
#include <functional>
#include <exception>

namespace mm {

	template <typename _Key, typename _Value, typename _KeyComparator=std::less<_Key>, typename _ValueHasher=std::hash<_Value>>
	class priority_queue {
	public:
		priority_queue()                                                                         : nodeIndexMap(0, _ValueHasher()), key_comparator(_KeyComparator()) { }
		priority_queue(const _KeyComparator& _key_comparator)                                    : nodeIndexMap(0, _ValueHasher()), key_comparator(_key_comparator)  { }
		priority_queue(const _ValueHasher& _value_hasher)                                        : nodeIndexMap(0, _value_hasher),  key_comparator(_KeyComparator()) { }
		priority_queue(const _KeyComparator& _key_comparator, const _ValueHasher& _value_hasher) : nodeIndexMap(0, _value_hasher),  key_comparator(_key_comparator)  { }
		~priority_queue() {
			nodeHeapVector.clear();
			nodeIndexMap.clear();
		}

		void push(_Key key, _Value value) {
			_push(key, value);
		}

		void update(_Key key, _Value value) {
			_update(key, value);
		}

		void pop() {
			if (!empty()) {
				_pop_index(0);
			} else {
				throw std::exception();
			}
		}

		_Key top_key() {
			if (!empty()) {
				return nodeHeapVector[0].key;
			} else {
				throw std::exception();
			}
		}

		_Key key(_Value value) {
			auto it = nodeIndexMap.find(value);
			if (it != nodeIndexMap.end()) {
				size_type nodeIndex = it->second;
				return nodeHeapVector[nodeIndex].key;
			} else {
				throw std::exception();
			}
		}

		_Value top_value() {
			if (!empty()) {
				return nodeHeapVector[0].value;
			} else {
				throw std::exception();
			}
		}

		bool empty() {
			return nodeHeapVector.empty();
		}

	private:

		struct queue_node {
			queue_node() { }
			queue_node(_Key _key, _Value _value) : key(_key), value(_value) { }

			_Key key;
			_Value value;
		};

		std::vector<queue_node> nodeHeapVector;
		std::unordered_map<_Value, long long> nodeIndexMap;

		_KeyComparator key_comparator;

		typedef typename decltype(nodeHeapVector)::size_type size_type;		

		void _heapify_up(size_type nodeIndex) {
			if (nodeIndex > 0) {
				size_type parentIndex = (nodeIndex - 1) / 2; // Will be >= 0 because nodeIndex > 0
				if (key_comparator(nodeHeapVector[parentIndex].key, nodeHeapVector[nodeIndex].key) ) {
					_swap(nodeIndex, parentIndex);
					_heapify_up(parentIndex);
				}
			}
		}

		void _heapify_down(size_type nodeIndex) {
			if (nodeIndex < nodeHeapVector.size()) {
				size_type leftChildIndex  = 2 * nodeIndex + 1;
				size_type rightChildIndex = 2 * nodeIndex + 2;
				if (leftChildIndex < nodeHeapVector.size()) {
					size_type maxChildIndex = leftChildIndex;
					if (rightChildIndex < nodeHeapVector.size() && key_comparator(nodeHeapVector[maxChildIndex].key, nodeHeapVector[rightChildIndex].key)) {
						maxChildIndex = rightChildIndex;
					}
					if (key_comparator(nodeHeapVector[nodeIndex].key, nodeHeapVector[maxChildIndex].key)) {
						_swap(nodeIndex, maxChildIndex);
						_heapify_down(maxChildIndex);
					}
				}
			}
		}

		void _push(_Key key, _Value value) {
			_pop_value(value);

			queue_node node(key, value);
			nodeHeapVector.push_back(node);
			nodeIndexMap[value] = nodeHeapVector.size() - 1;

			_heapify_up(nodeHeapVector.size() - 1);
		}

		void _update(_Key key, _Value value) {
			auto it = nodeIndexMap.find(value);
			if (it != nodeIndexMap.end()) {
				size_type nodeIndex = it->second;
				nodeHeapVector[nodeIndex].key = key;
				_heapify_up(nodeIndex);
				_heapify_down(nodeIndex);
			} else {
				_push(key, value);
			}
		}

		void _pop_value(_Value value) {
			auto it = nodeIndexMap.find(value);
			if (it != nodeIndexMap.end()) {
				size_type nodeIndex = it->second;
				_pop_index(nodeIndex);
			}
		}

		void _pop_index(size_type index) {
			_Value value = nodeHeapVector[index].value;

			_swap(index, nodeHeapVector.size() - 1);
			nodeHeapVector.pop_back();
			_erase_map(value);

			_heapify_up(index);
			_heapify_down(index);
		}

		struct node_comparator {
			node_comparator(_KeyComparator _key_comparator) : key_comparator_local(_key_comparator) { }
			bool operator() (const queue_node& nodeA, const queue_node& nodeB) {
				return key_comparator_local(nodeA.key, nodeB.key);
			}
		private:
			_KeyComparator key_comparator_local;
		};

		void _erase_map(_Value value) {
			auto it = nodeIndexMap.find(value);
			if (it != nodeIndexMap.end()) {
				nodeIndexMap.erase(it);
			}
		}

		void _print_heap() {
			for (auto node : nodeHeapVector) {
				std::cout << node.key << " ";
			}
			std::cout << std::endl;
		}

		void _swap(size_type nodeIndexA, size_type nodeIndexB) {
			_Value nodeValueA = nodeHeapVector[nodeIndexA].value;
			_Value nodeValueB = nodeHeapVector[nodeIndexB].value;

			std::swap(nodeHeapVector[nodeIndexA], nodeHeapVector[nodeIndexB]);
			std::swap(nodeIndexMap[nodeValueA], nodeIndexMap[nodeValueB]);
		}

	};

}

#endif