Added pathfinding algorithm