def astar(problem, heur):
"""
Implement A* search.
The given heuristic function will take in a state of the search problem
and produce a real number
Your implementation should be able to work with any heuristic, heur
that is for the given search problem (but, of course, without a
guarantee of optimality if heur is not admissible).
"""
start = problem.get_start_state()
pq = PriorityQueue(True)
visited = {}
cost = {}
cur = start
pq.push_with_priority(start, heur(start))
visited[start] = None
cost[start] = 0
while not pq.is_empty():
cur = pq.pop()
cur_cost = cost[cur]
if problem.is_goal_state(cur):
break
dict = problem.get_successors(cur)
for state in dict:
if state not in visited:
pq.push_with_priority(state,
cur_cost + dict[state] + heur(state))
visited[state] = cur
cost[state] = cur_cost + dict[state]
path = []
while cur is not None:
path.append(cur)
cur = visited[cur]
path.reverse()
print len(visited)
return path
