Complete and test priority queue code