Added stage 1 pathfinding