Pathfinding algorithm

Further changes