Finished pathfinding algorithm