Slightly fixed pathfinding

Note: still very buggy!