Finished A* algorithm