Fix Bugs in Pathfinding