Worked on the pathfinding algorithm