Fixed Pathfinding Implementation