PriorityQueue + costFunction, basic tests pass