Implementing a priority queue in C++