feat: A* algorithm

search.py

@@ -197,7 +197,41 @@ def nullHeuristic(state, problem=None):
 def aStarSearch(problem: SearchProblem, heuristic=nullHeuristic):
     """Search the node that has the lowest combined cost and heuristic first."""
     "*** YOUR CODE HERE ***"
-    util.raiseNotDefined()
+    actions = []
+
+    curr_state = problem.getStartState()
+    isGoal = problem.isGoalState(curr_state)
+    costs = 0
+
+    visited = set([curr_state])
+    queue = util.PriorityQueue()
+
+    while not isGoal:
+        # push in the P Queue
+        successors = problem.getSuccessors(curr_state)
+        for (state, direction, cost) in successors:
+            if state not in visited:
+                queue.push({
+                    'state': (state, direction, cost),
+                    'actions': actions,
+                    'costs': costs
+                }, costs + cost + heuristic(state, problem))                # if visited this successor node, how much cost will the agent need
+
+        # check whether reachable
+        if queue.isEmpty():
+            return []
+        else:
+            obj = queue.pop()
+            (curr_state, direction, cost) = obj['state']
+            visited.add(curr_state)
+            actions = obj['actions'] + [ direction ]
+            costs = obj['costs'] + cost
+
+            isGoal = problem.isGoalState(curr_state)
+
+    return actions
+    
+
 
 
 # Abbreviations