Added pqueue, in the process of adding astar