Update A*

Same as UCS but with heuristics now