Use a PriortyQueue.

Simplifies the code and is faster.