Fix some logic in priority queue