Completed MappedPriorityQueue and tests