class ApproximateSearchAgent(Agent):
    "Implement your contest entry here.  Change anything but the class name."

    def registerInitialState(self, state):
        "This method is called before any moves are made."
        "*** YOUR CODE HERE ***"
        # ApproximateSearchAgent.create(state,set(state.foodGrid.asList()),state.walls)

    def getAction(self, state):
        """
        From game.py:
        The Agent will receive a GameState and must return an action from
        Directions.{North, South, East, West, Stop}
        """
        "*** YOUR CODE HERE ***"
        if 'actionIndex' not in dir(self): self.actionIndex = 0
        i = self.actionIndex
        self.actionIndex += 1
        if i < len(self.actions):
            return self.actions[i]
        else:
            return Directions.STOP

        # util.raiseNotDefined()

    def __init__(self):
        from util import PriorityQueue
        self.pacman = tuple()
        self.food = []
        self.walls = list()
        self.Queue = PriorityQueue()
        self.min = float("inf")

    def create(self,p,f,w):
        self.pacman = p
        self.food = f
        self.walls = w
        self.myWalls = w.asList()
    def getWalls(self):
        return self.walls
    def getPacman(self):
        return self.pacman
    def getFood(self):
        return self.food
    def getPacmanPosition(self):
        return self.pacman
    def getNumFood(self):
        return len(self.food)
    def hasFood(self,*goal):
        return len(list(*goal))
    def mazeDistance(self,point1,point2):
        for food in self.getFood():
            if food not in self.myWalls:
                prob = PositionSearchProblem(self, start=point1, goal=point2, warn=False)
                dist = len(search.bfs(prob))
                self.Queue.push(food,dist)
                self.min = self.min if self.min<dist else dist
    def newQueue(self):
        from util import PriorityQueue
        self.Queue = PriorityQueue()

def uniformCostSearch(problem):
  "Search the node of least total cost first. "
  "*** YOUR CODE HERE ***"
  
  from util import PriorityQueue

  frontier = PriorityQueue()
  explored_states = set([])
  node = (problem.getStartState(), [], 0)

  if problem.isGoalState(node[0]):
      return node[1]
  PriorityQueue.push(frontier, node, node[2])
  while True:
      if not frontier:
          return None
      node = PriorityQueue.pop(frontier)
      explored_states.add(node[0])
      successor_candidates = problem.getSuccessors(node[0])
      successor_candidates.reverse()
      for candidate in successor_candidates:
	  if candidate[0] not in explored_states:
	      path_to_node = list(node[1])
	      path_to_node.append(candidate[1])
	      child_node = (candidate[0], path_to_node, node[2] + candidate[2])
	      # if candidate[0] not in explored_states:
	      if problem.isGoalState(child_node[0]):
	          return child_node[1]
	      PriorityQueue.push(frontier, child_node, child_node[2])

    def __guess_and_check(self):
        """If it proves impossible to solve the puzzle by pure deduction, this
method is called to guess (using minimum remaining values and least constraining
choice heuristics) a number, which is then used to solve a reduced game (if
possible). All possible choices are exhausted in this manner.
        """
        queue = PriorityQueue()
        impossible_list = []
        for x, y in product(range(9), repeat=2):
            position = self.__positions[x][y]
            n = position.num_possible()
            if n <= 1:
                continue
            for val in position.possible:
                new_board = deepcopy(self)
                new_board.__set_position_val(x, y, val)
                new_board.__enforce_consistency()
                priority = new_board.__possible_count()
                queue.push((new_board, x, y, val), priority*n)
        while not queue.isEmpty():
            new_board, x, y, val = queue.pop()
            for xx, yy, valval in impossible_list:
                new_board.__positions[xx][yy].remove_possible(valval)
            b = new_board.__solver()
            if not isinstance(b, SudokuGame):
                impossible_list.append((x, y, val))
                continue
            return b
        return False

def uniformCostSearch(problem):
    successor_idx = {'dir': 1, 'state': 0, 'cost': -1}

    MAX_ITER = int(2000)
    stateQ = PriorityQueue()
    visitedSet = set()
    successorDict = {}

    curState = problem.getStartState()
    actionList = [] # add actions to get to the state, use pop to remove last one
    currentCost = 0

    for it in range(MAX_ITER):

        if problem.isGoalState(curState):
            return actionList
        
        if curState not in visitedSet:
            successorDict[curState] = problem.getSuccessors(curState)
        visitedSet.add(curState)

        for node in successorDict[curState]:

            if node[successor_idx['state']] not in visitedSet:
                tmp_state = {'cost' : currentCost + node[successor_idx['cost']], 'action' : actionList + [node[successor_idx['dir']]], 'state': node[successor_idx['state']]}
                stateQ.push(tmp_state, tmp_state['cost'])
        # get next state to test
        nState = stateQ.pop()
        actionList = nState['action']
        curState = nState['state']
        currentCost = nState['cost']

    return []

def aStarSearch(problem, heuristic=nullHeuristic):
  
    
    from collections import defaultdict
    from util import PriorityQueue
    
    initial_node=problem.getStartState()
    
    current_node=defaultdict(list)
    current_node[initial_node].append("No parent")
    current_node[initial_node].append("No direction")
    priority_queue=PriorityQueue()
    cost_array={}
    #priority_queue.push(current_node,0)
    cost_list={}
    cost_list[initial_node]=0
    cost_array[initial_node]=heuristic(initial_node,problem)
    
    direction_list={}
    Child=current_node.keys()
    Parent=current_node.values()[0]
    visited_list={}
          
    
    while (problem.isGoalState(Child[0]) is not True):
            
        if(Child[0] in visited_list.keys()):
            current_node=priority_queue.pop()
            Child=current_node.keys()
            Parent=current_node.values()[0]
        else:
            visited_list[Child[0]]=Parent[0]
            direction_list[Child[0]]=Parent[1]
            Successor_node=problem.getSuccessors(Child[0])
            for index in range(len(Successor_node)):
                temp_dict=defaultdict(list)
                
                temp_dict[Successor_node[index][0]].append(Child[0])
                temp_dict[Successor_node[index][0]].append(Successor_node[index][1])
                cost_list[Successor_node[index][0]]= cost_list[Child[0]]+Successor_node[index][2] 
                cost_array[Successor_node[index][0]]=cost_list[Successor_node[index][0]]+heuristic(Successor_node[index][0],problem)
                priority_queue.push(temp_dict, cost_array[Successor_node[index][0]])
                  
            current_node=priority_queue.pop()
            Child=current_node.keys()
            Parent=current_node.values()[0]
    
    visited_list[Child[0]]= Parent[0]
    direction_list[Child[0]]=Parent[1]
    backtracking =[]
    path = Child[0]
    backtracking.append(direction_list[path])
    path = visited_list[path]
    while (path!= problem.getStartState()):
        backtracking.append(direction_list[path])
        path = visited_list[path]
    backtracking.reverse()
    return backtracking
    util.raiseNotDefined()

 def __init__(self):
     from util import PriorityQueue
     self.pacman = tuple()
     self.food = []
     self.walls = list()
     self.Queue = PriorityQueue()
     self.min = float("inf")

 def __init__(self, position, blocks, game_state, elements):
     self.elements = elements
     self.ds = PriorityQueue()
     self.position = round_tuple(position)
     self.blocks = [round_tuple(t) for t in blocks]
     self.game_state = game_state
     self.antecessor = {}

def uniformCostSearch(problem):
  "Search the node of least total cost first. "
  start = problem.getStartState()
  frontier = PriorityQueue()  # A priority queue of (state,route_to_state,cost)
  frontier_set = set()  #A set recording what's in the frontier
  explored_set = set()  # A set of states recording the explored nodes
  frontier.push((start,list(),0),0)
  frontier_set.add(start)
  solution_node = ((0,0),list(),-1)

  while not frontier.isEmpty():
    current_node = frontier.pop()
    frontier_set.remove(current_node[0])
    if solution_node[2] == -1 or current_node[2] < solution_node[2]:
      if problem.isGoalState(current_node[0]):
        solution_node = current_node
        successors = list()
      else:
        successors = problem.getSuccessors(current_node[0])
      explored_set.add(current_node[0])
      for s in successors:
        if s[0] not in explored_set.union(frontier_set):
          current_route = list(current_node[1])
          current_route.append(s[1])
          frontier.push((s[0],current_route,current_node[2]+s[2]),current_node[2]+s[2])
          frontier_set.add(s[0])
        elif s[0] in frontier_set:
          #retrieve the frontier to see if current_node has lower cost
          found = False
          nodes = list()
          while not found:
            node = frontier.pop()
            if s[0] == node[0]:
              found = True
              if current_node[2]+s[2] < node[2]:
                current_route = list(current_node[1])
                current_route.append(s[1])
                nodes.append((s[0],current_route,current_node[2]+s[2]))
              else:
                nodes.append(node)
            else:
              nodes.append(node)
          for n in nodes:
            frontier.push(n,n[2])
  if solution_node[2] == -1:
    print "No route found!"
  return solution_node[1]

def uniformCostSearch(problem):
    "Search the node of least total cost first. "
    "*** YOUR CODE HERE ***"
    from util import PriorityQueue

    # fringe
    fringe = PriorityQueue()
    # closed set
    closed = set([])

    fringe.push((problem.getStartState(), [], 0), 0)

    while True:
        if fringe.isEmpty() == True:
            print 'fail to find a path to the goal state'
            return []
        else:
            node = fringe.pop()
        if problem.isGoalState(node[0]) == True:
            return node[1]
        if node[0] not in closed:
            closed.add(node[0])
            actionsSoFar = node[1]
            costSoFar = node[2]
            for successor in problem.getSuccessors(node[0]):
                newActions = actionsSoFar[:]
                newActions.append(successor[1])
                newCost = costSoFar
                newCost += successor[2]
                fringe.push((successor[0], newActions, newCost), newCost)

def aStarSearch(problem, heuristic=nullHeuristic):
  "Search the node that has the lowest combined cost and heuristic first."
  "*** YOUR CODE HERE ***"

  from util import PriorityQueue 
 
  frontier = PriorityQueue()
  node = (problem.getStartState(), [], 0)
  PriorityQueue.push(frontier, node, node[2] + heuristic(node[0], problem))
  explored_states = set([])

  if problem.isGoalState(node[0]):
      return node[1]
  elif PriorityQueue.isEmpty(frontier):
      return None
  else:
      while not PriorityQueue.isEmpty(frontier):
          new_node = PriorityQueue.pop(frontier)
          explored_states.add(new_node[0])
	  successor_candidates = problem.getSuccessors(new_node[0])
          if node[0][0] == (13, 5):
	      print "successors: ", successor_candidates
          if successor_candidates:
	      successor_candidates.reverse()
              for candidate in successor_candidates:
		  if candidate[0] not in explored_states:
		      c_path = list(new_node[1])
	              c_path.append(candidate[1])
		      # if candidate[0] not in explored_states:
	              if problem.isGoalState(candidate[0]):
		          return c_path
	              PriorityQueue.push(frontier, (candidate[0], c_path, new_node[2] + candidate[2]), new_node[2] + heuristic(candidate[0], problem))

def uniformCostSearch(problem):
    """Search the node of least total cost first."""
    from game import Directions
    from util import PriorityQueue

    pqueue = PriorityQueue()
    startNode = problem.getStartState()
    pqueue.push((startNode, []), 0)
    visitedList = []
    oldCost = {}
    oldCost[startNode] = None

    while not pqueue.isEmpty():
        current, actions = pqueue.pop()
        if problem.isGoalState(current):
            return actions
        visitedList.append(current)
        for coord, direction, steps in problem.getSuccessors(current):
            new_actions = actions + [direction]
            priority = problem.getCostOfActions(new_actions)
            if not coord in visitedList or priority < oldCost[coord]:
                oldCost[coord] = priority
                visitedList.append(coord)
                pqueue.push((coord, new_actions), priority)
    return []

    util.raiseNotDefined()

def aStarSearch(problem, heuristic):
    """Search the node that has the lowest combined cost and heuristic first."""
    "*** YOUR CODE HERE ***"

    """
    Only A* can find dots in openMaze. Path looks like a staircase.
    """
    path = list()
    parentChild = list()
    print "Problem: ", problem
    print "Start:", problem.getStartState()
    print "Is the start a goal?", problem.isGoalState(problem.getStartState())
    print "Start's successors:", problem.getSuccessors(problem.getStartState())
    if problem.isGoalState(problem.getStartState()):
        return None
    explored = list()
    frontier = PriorityQueue()
    #tuple = (problem.getStartState(), float("inf"))
    frontier.push(problem.getStartState(), float("inf"))
    while(not frontier.isEmpty()):
        state = frontier.pop()      #state = [(x, y), cost]
        explored.append(state)
        if (problem.isGoalState(state)):
            #print "Found..."
            path = backtracking(problem, state, parentChild)
            return path
        for successor in problem.getSuccessors(state):
            #print "Successor: ", successor
            if (not successor[0] in explored):
                #print("State[1] ", state[1])
                cost = successor[2] + heuristic(state, problem) #cost = stepCost +currentNodeCost
                parentChild.append((state, successor[1], successor[0]))
                frontier.push(successor[0], cost)
    return None

def uniformCostSearch(problem):
    "Search the node of least total cost first. "
    "*** YOUR CODE HERE ***"
    queue = PriorityQueue()
    explored = []
    
    queue.push((problem.getStartState(), [], 0), 0) #what in the queue is the (state, path,cost)
    explored.append(problem.getStartState())
    while not queue.isEmpty():
        tuple = queue.pop()
        #print tuple[0], tuple[2]
        currentPath = tuple[1]
        #print currentPath
        currentCost = tuple[2]
                
        if problem.isGoalState(tuple[0]):
            return currentPath
                    
        suc = problem.getSuccessors(tuple[0])
        for triple in suc:
            if explored.__contains__(triple[0]):
                continue
            if not problem.isGoalState(triple[0]):
                explored.append(triple[0])
            path = currentPath + [triple[1]]
            cost = currentCost + triple[2]
            queue.push((triple[0], path, cost), cost)

def aStarSearch(problem, heuristic=nullHeuristic):
    """Search the node that has the lowest combined cost and heuristic first."""
    "*** YOUR CODE HERE ***"
    from util import PriorityQueue
    # Visited
    visited = []
    # Declare priority queue
    priorityQueueNode = PriorityQueue()
    # Start state
    priorityQueueNode.push((problem.getStartState(),[]),0)

    while not priorityQueueNode.isEmpty():
        currentState, actions = priorityQueueNode.pop()

        if currentState not in visited:
            #print "Current node ", currentNode
            if problem.isGoalState(currentState):
                return actions
            
            visited.append(currentState)

            seccessors = problem.getSuccessors(currentState)

            for state, action, cost in seccessors:
                if state not in visited:
                    tempActions = actions + [action]
                    priorityQueueNode.push((state, tempActions), problem.getCostOfActions(tempActions) + heuristic(state, problem))

    return []

def uniformCostSearch(problem):
    """Search the node of least total cost first."""
    "*** YOUR CODE HERE ***"
    
    from util import PriorityQueue

    queueOpen = PriorityQueue()

    rootNode = SearchNode(problem.getStartState(), None, None, 0, 0)

    if problem.isGoalState(rootNode.position):
        return []

    queueOpen.push(rootNode, 0)
    visited = {}

    while not queueOpen.isEmpty():
        currentNode = queueOpen.pop()
        if currentNode.position in visited:
            continue
        if problem.isGoalState(currentNode.position):
            return currentNode.backtrack()
        visited[currentNode.position] = True
        for succ in expand(problem, currentNode):
            if succ.position not in visited:
                temp = SearchNode(succ.position, currentNode, succ.transition, succ.cost, 0)
                queueOpen.push(temp, succ.cost)

def aStarSearch(problem, heuristic=nullHeuristic):
    "Search the node that has the lowest combined cost and heuristic first."
    closed = []
    closedSet = sets.Set(closed)
    fringe = PriorityQueue()
    for child in problem.getSuccessors(problem.getStartState()):
        path = [child[1]]
        fringe.push((child[0], path, child[2]), child[2] + heuristic(child[0], problem))
    closedSet.add(problem.getStartState())
    
    while( not(fringe.isEmpty()) ):
        nodePos, nodeDir, nodeCost = fringe.pop()
        if (problem.isGoalState(nodePos)):
            return nodeDir
        if (nodePos not in closedSet):
            successors = problem.getSuccessors(nodePos)
            closedSet.add(nodePos)
            path = [nodeDir]
            for childNode in successors:
                cost = nodeCost + childNode[2]
                path = nodeDir + [childNode[1]]
                childNode = [childNode[0], path , cost]
                fringe.push(childNode, cost + heuristic(childNode[0], problem))

    return -1

def uniformCostSearch(problem):
    "Search the node of least total cost first. "
    closed = []
    closedSet = sets.Set(closed)
    fringe = PriorityQueue()
    for child in problem.getSuccessors(problem.getStartState()):
        path = [child[1]]
        fringe.push((child[0], path, child[2]), child[2])
    closedSet.add(problem.getStartState())
    
    while( not(fringe.isEmpty()) ):
        nodePos, nodeDir, nodeCost = fringe.pop()
        if (problem.isGoalState(nodePos)):
            return nodeDir
        if (nodePos not in closedSet):
            successors = problem.getSuccessors(nodePos)
            closedSet.add(nodePos)
            path = [nodeDir]
            for childNode in successors:
                cost = nodeCost + childNode[2]
                path = nodeDir + [childNode[1]]
                childNode = [childNode[0], path , cost]
                fringe.push(childNode, cost)

    return -1

def aStarSearch(problem, heuristic=nullHeuristic):
    """Search the node that has the lowest combined cost and heuristic first."""
    "*** YOUR CODE HERE ***"
    from game import Directions
    from util import PriorityQueue
    s = Directions.SOUTH
    w = Directions.WEST
    e = Directions.EAST
    n = Directions.NORTH
    
    visited = [None] # list of states (each state is a position in this case)
    pq = PriorityQueue() # contains pairs. each pair's first elem is a list of actions. second elem is state
    pq.push([[], problem.getStartState()], 0)

    while (not pq.isEmpty()):
        top = pq.pop() # this is the top of the stack
        actions = top[0]
        state = top[1]
        if (state not in visited):
            if problem.isGoalState(state):
                return actions
            
            visited.append(state)
            
            for child in problem.getSuccessors(state):
            
                tempActions = actions[:]
                tempActions.append(child[1])
                totalCostOfActions = problem.getCostOfActions(tempActions) + heuristic(child[0], problem)
                pq.push([tempActions, child[0]], totalCostOfActions)
    return None

def aStarSearch(problem, heuristic=nullHeuristic):
  "Search the node that has the lowest combined cost and heuristic first."
  "*** YOUR CODE HERE ***"

  #A* is just UCS with g(n) + h(n)
  from util import PriorityQueue
  
  pathConstructDict = {}
  priorityQueue = PriorityQueue()
  costToNode = {}
  
  for successor in problem.getSuccessors(problem.getStartState()):
    priorityQueue.push((successor, None), successor[2] + heuristic(successor[0], problem)) #push successor and its priority is its path cost for a* it is g(n) + h(n)

  costToNode[problem.getStartState()] = 0

  while(not priorityQueue.isEmpty()):
    expansion = priorityQueue.pop()
    pathConstructDict[expansion[0]] = expansion[1]

    if(expansion[1] != None):
      costToNode[expansion[0][0]] = costToNode[expansion[1][0]] + expansion[0][2]
    else:
      costToNode[expansion[0][0]] = expansion[0][2]

    if(problem.isGoalState(expansion[0][0])):
      return pathConstructor(expansion[0], pathConstructDict)

    for successor in problem.getSuccessors(expansion[0][0]):
      if(successor[0] not in costToNode):
        priorityQueue.push((successor, expansion[0]), costToNode[expansion[0][0]] + successor[2] + heuristic(successor[0], problem))

  return None

def uniformCostSearch(problem):
  "Search the node of least total cost first. "
  "*** YOUR CODE HERE ***"
  from util import PriorityQueue
  
  pathConstructDict = {}
  priorityQueue = PriorityQueue()
  costToNode = {}
  
  for successor in problem.getSuccessors(problem.getStartState()):
    priorityQueue.push((successor, None), successor[2]) #push successor and its priority is its path cost

  costToNode[problem.getStartState()] = 0

  while(not priorityQueue.isEmpty()):
    expansion = priorityQueue.pop()
    pathConstructDict[expansion[0]] = expansion[1]

    if(expansion[1] != None):
      costToNode[expansion[0][0]] = costToNode[expansion[1][0]] + expansion[0][2]
    else:
      costToNode[expansion[0][0]] = expansion[0][2]

    if(problem.isGoalState(expansion[0][0])):
      return pathConstructor(expansion[0], pathConstructDict)

    for successor in problem.getSuccessors(expansion[0][0]):
      if(successor[0] not in costToNode):
        priorityQueue.push((successor, expansion[0]), costToNode[expansion[0][0]] + successor[2])

  return None