Fixed bug in priority queue swap function.

Also added an initial test function for pqueue.