implemented pqueue with pathfinder and fixed bug in pathfinder