Fixed priority queue bug and added testing algorithm