Use a PriortyQueue.

Simplifies the code and is faster.
Merge pull request #1 from stefanbirkner/priority-queue

Use a PriortyQueue.