Rewrite the algorithm with open list and heuristic distance estimation