Debugged pathfinding some more