Rewrote AStarAlgo