Finished AStar Algorithm