Issues with PriorityQueue fixed, seems to work properly.