def breadthFirstSearch(problem):
  "Search the shallowest nodes in the search tree first. [p 74]"
  "*** YOUR CODE HERE ***"
  from util import Queue
    
  currentState = problem.getStartState()
  
  setOfExploredNodes = set([currentState])
  listofMoves = []
  successorStack = Queue()
  
  while not problem.isGoalState(currentState):
      for successor in problem.getSuccessors(currentState):          
          if successor[0] not in setOfExploredNodes:              
              if problem.isGoalState(successor[0]):  
                #print listofMoves + [successor[1]]              
                return listofMoves + [successor[1]]
                
              setOfExploredNodes.add(successor[0])
              successorStack.push((successor[0], listofMoves + [successor[1]]))              
      
      currentState = successorStack.pop()
            
      listofMoves = currentState[1]
      currentState = currentState[0]
      

        

  return None

def breadthFirstSearch(problem):
  "Search the shallowest nodes in the search tree first. [p 74]"
  "*** YOUR CODE HERE ***"
  from util import Queue
  
  #util.raiseNotDefined()
  currentState = problem.getStartState()
  
  setOfExploredNodes = set([currentState])
  listofMoves = []
  successorStack = Queue()
  counter = 0;
  
  while not problem.isGoalState(currentState):
      for successor in problem.getSuccessors(currentState):          
          if successor[0] not in setOfExploredNodes:
              #print successor
              setOfExploredNodes.add(successor[0])
              successorStack.push((successor[0], listofMoves + [successor[1]]))              
      
      currentState = successorStack.pop()
      
      #setOfExploredNodes.add(currentState[0])
      listofMoves = currentState[1]
      currentState = currentState[0]
      
      counter += 1
      if counter % 100 == 0:
        print counter
        

  print listofMoves  
  return listofMoves

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

    MAX_ITER = int(20000)
    stateQ = Queue()
    actQ = Queue() # queues action for backtracking
    visitedSet = set()

    curState = problem.getStartState()
    visitedSet.add(curState)

    actionList = [] # add actions to get to the state, use pop to remove last one

    for it in range(MAX_ITER):
        
        if problem.isGoalState(curState):
            return actionList
        
        successors = problem.getSuccessors(curState)

        for node in successors:
            if node[successor_idx['state']] not in visitedSet:
                stateQ.push(node[successor_idx['state']])
                actQ.push(actionList + [node[successor_idx['dir']]])
                visitedSet.add(node[successor_idx['state']])

        actionList = actQ.pop()
        curState = stateQ.pop()

    return []

def breadthFirstSearch(problem):
    """
    Search the shallowest nodes in the search tree first.
    """
    "*** YOUR CODE HERE ***"
    queue = Queue()
    explored = []
    
    queue.push((problem.getStartState(), [])) #what in the queue is the (state, path)
    explored.append(problem.getStartState())
    while not queue.isEmpty():
        tuple = queue.pop()
        currentPath = tuple[1]
        
        
        if problem.isGoalState(tuple[0]):
            return currentPath
                    
        suc = problem.getSuccessors(tuple[0])
        for triple in suc:
            if explored.__contains__(triple[0]):
                continue
            explored.append(triple[0])
            path = currentPath + [triple[1]]
            queue.push((triple[0], path))

def buildMazeDistanceMap(position, gameState):
    """
      Use BFS to build a map that stores maze distances between position and each point in the layout

    """
    x, y = position
    walls = gameState.getWalls()
    assert not walls[x][y], 'position is a wall: ' + str(position)

    # initialization
    distanceMap = {}
    queue = Queue()
    distance = 0
    queue.push(position)

    while not queue.isEmpty():
    	currPos = queue.pop()

    	if currPos not in distanceMap:
    		distanceMap[currPos] = distance

    		for pos in Actions.getLegalNeighbors(currPos, walls):
    			queue.push(pos)

    	distance += 1

    return distanceMap

def breadthFirstSearch(problem):
  """
  Search the shallowest nodes in the search tree first.
  [2nd Edition: p 73, 3rd Edition: p 82]
  """
  "*** YOUR CODE HERE ***"
  from util import Queue

  fila = Queue()
  colorir = []
  acoes = []
  inicial = (problem.getStartState(),None,None,0)
  "Tupla formada por (Estado atual, Estado Pai, Aчуo, Custo)"
  fila.push(inicial)
  colorir.append(inicial)

  while fila.isEmpty() == False:
    noAtual = fila.pop()
    
    if problem.isGoalState(noAtual[0]):
      return gerarCaminho(noAtual)
    else:
      for item in problem.getSuccessors(noAtual[0]):
        est = item[0]
        aco = item[1]
        cus = item[2]
        if est not in colorir:
          colorir.append(est)
          tupla = (est,noAtual,aco,cus)
          fila.push(tupla)

def breadthFirstSearchNormal2(problem):
    """Search the shallowest nodes in the search tree first."""
    "*** YOUR CODE HERE ***"
    from util import Queue
    actionListMap = []
    visitedList = {}
    startState = problem.getStartState()
    q = Queue()
    q.push(startState)
    visitedList[startState] = [None, None]
    while not q.isEmpty():
        targetState = q.pop()
        isGoal = problem.isGoalState(targetState)
        if isGoal == None:
            actionList = buildActionListFromBFSResult(visitedList,startState, targetState)
            actionListMap.append((targetState, actionList))
        elif isGoal:
            actionList = buildActionListFromBFSResult(visitedList,startState, targetState)
            actionListMap.append((targetState, actionList))
            print "Meet Goal"
            return actionListMap
        else:
            for successor in problem.getSuccessors(targetState):
                state = successor[0]
                action = successor[1]
                if state not in visitedList.keys():
                    visitedList[state] = [targetState, action]
                    q.push(state)

def breadthFirstSearch(problem):
    """
    Search the shallowest nodes in the search tree first.
    """

    from util import Queue
    from game import Directions
    
    actions = []
    
    frontier = Queue()
    frontier.push((problem.getStartState(), [], 0))
    visited = []

    while (frontier.isEmpty() == False):
        (currentS, currentP, currentC) = frontier.pop()
        if (problem.isGoalState(currentS) == True):
            actions = currentP
            break
        if (visited.count(currentS) == 0):
            visited.append(currentS)
            successors = problem.getSuccessors(currentS)
            for i in range(0,len(successors)):
                (neighbor, direction, cost) = successors[i]
                if (visited.count(neighbor) == 0):
                    frontier.push((neighbor, (currentP +[direction]), (currentC + cost)))
    print actions
    return actions

    util.raiseNotDefined()

def breadthFirstSearch(problem):
  "Search the shallowest nodes in the search tree first. [p 81]"
  "*** YOUR CODE HERE ***"
  from util import Queue
  from game import Directions

  state = 0
  action = 1

  cstate = problem.getStartState()
  if (problem.isGoalState(cstate)):
          return []

  q = Queue()
  visited = []
  q.push((cstate, []))

  while(not q.isEmpty()):
      cstate, path = q.pop()
      visited.append(cstate)

      for x in problem.getSuccessors(cstate):
          npath = path + [x[action]]

          if(visited.count(x[state]) != 0):
              continue

          if (problem.isGoalState(x[state])):
              return npath

          nstate = x[state]
          visited.append(x[state])
          q.push((nstate, npath))

  print("Path is not found")

def breadthFirstSearch(problem):
    "Search the shallowest nodes in the search tree first. [p 81]"
    from util import Queue
    BFS1 = Queue()
    Moves = []
    Visited = []
    Final = []
    NewState = (0, (problem.getStartState(), 'Start', 0))
    #print CurrentState
    BFS1.push([NewState])
    while not BFS1.isEmpty():
        NewState = BFS1.pop()
        if problem.isGoalState(NewState[0][1][0]):
            Final = NewState
            break
        if Visited.count(NewState[0][1][0]) == 0:
            #print NewState
            for item in enumerate(problem.getSuccessors(NewState[0][1][0])):
                #print item
                BFS1.push([item] + NewState)
        Visited.append(NewState[0][1][0])
    for nodes in Final:
        Moves.append(nodes[1][1])
    Moves.reverse()
    Moves.remove('Start')
    #print Moves
    return Moves


    """def breadthFirstSearch(problem):

def breadthFirstSearch(problem):    

    """
    Search the shallowest nodes in the search tree first.
    """
    "*** YOUR CODE HERE ***"
    from util import Queue

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

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

    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]
            for successor in problem.getSuccessors(node[0]):
                newActions = actionsSoFar[:]
                newActions.append(successor[1])
                fringe.push((successor[0], newActions))

def breadthFirstSearch(problem):
  "Search the shallowest nodes in the search tree first. [p 74]"
  "*** YOUR CODE HERE ***"
  from util import Queue
  successorsExplored = {}
  statesExplored = set()
  bfsQueue = Queue()

  for successor in problem.getSuccessors(problem.getStartState()):
    bfsQueue.push((successor, None))

  statesExplored.add(problem.getStartState())
  if(problem.isGoalState(problem.getStartState())):
    return []

  while(not bfsQueue.isEmpty()):
    currentSuccessorPair = bfsQueue.pop()
    if(currentSuccessorPair[0][0] in statesExplored):
      continue

    successorsExplored[currentSuccessorPair[0]] = currentSuccessorPair[1]
    statesExplored.add(currentSuccessorPair[0][0])
    if(problem.isGoalState(currentSuccessorPair[0][0])):
      return reconstructPath(successorsExplored, currentSuccessorPair[0])

    for successor in problem.getSuccessors(currentSuccessorPair[0][0]):
      if(successor[0] not in statesExplored):
        bfsQueue.push((successor, currentSuccessorPair[0]))

  return None

def breadthFirstSearch(problem):
    """Search the shallowest nodes in the search tree first."""
    "*** YOUR CODE HERE ***"

    # Imports for tools
    from util import Queue
    from game import Directions
    import copy

    path = Queue()
    path.push((problem.getStartState(), []))
    visited = set([problem.getStartState()])

    while not path.isEmpty():
        info = path.pop()
        currentState = info[0]
        actions = info[1]
        if problem.isGoalState(currentState):
            return actions
        for successor in problem.getSuccessors(currentState):
            if not successor[0] in visited:
                visited.add(successor[0])
                if ("North" in successor):
                    actions.append(Directions.NORTH)
                elif ("East" in successor):
                    actions.append(Directions.EAST)
                elif ("South" in successor):
                    actions.append(Directions.SOUTH)
                elif ("West" in successor):
                    actions.append(Directions.WEST)
                path.push((successor[0], copy.copy(actions)))
                actions.pop()

def breadthFirstSearch(problem):
    """
    Search the shallowest nodes in the search tree first.
    """
    closed = []
    closedSet = sets.Set(closed)
    fringe = Queue()
    for child in problem.getSuccessors(problem.getStartState()):
        path = [child[1]]
        fringe.push((child[0], path, 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 , child[2]]
                fringe.push(childNode)

    return -1

def breadthFirstSearch(problem):
    """Search the shallowest nodes in the search tree first."""
    "*** YOUR CODE HERE ***"
    from util import Queue

    queueOpen = Queue()

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

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

    queueOpen.push(rootNode)
    visited = {}
    #visited;
    #cvorovi = [pocetni];

    while not queueOpen.isEmpty():
        #ako je cvor u visited -> continue
        #stavi cvor u visited
        #za svakog sljedbenika: ako nije u visited, dodaj ga u cvorove
        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)

def breadthFirstSearch(problem):
    """Search the shallowest nodes in the search tree first."""
    "*** YOUR CODE HERE ***"
    from util import Queue
    startState = problem.getStartState()
    # Visited
    visited = []
    # Declare variable for queue node
    queueNode = Queue()
    # Push start state into queue
    queueNode.push((startState,[]))

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

        #if currentState not in visited:
            #visited.append(currentState)

        seccessors = problem.getSuccessors(currentState)
        print "\t" , seccessors
        for state,action,cost in seccessors:
            if state not in visited:
                if problem.isGoalState(currentState):
                    print actions
                    return actions
                visited.append(state)
                queueNode.push((state,actions + [action]))
    return []

def breadthFirstSearch(problem):
  "Search the shallowest nodes in the search tree first. [p 81]"
  "*** YOUR CODE HERE ***"
  from util import Queue
  
  queue = Queue()
  exploredSet = set()
  pathConstructDict = {}
  
  #print "Start's successors:", problem.getSuccessors(problem.getStartState())

  #add start to exploredSet, and check if it is the goal 
  exploredSet.add(problem.getStartState())
  if(problem.isGoalState(problem.getStartState())):
    return []
    
  for successor in problem.getSuccessors(problem.getStartState()):
    #print successor
    queue.push((successor, None))

  while(not queue.isEmpty()):
    expansion = queue.pop()
    if(expansion[0][0] in exploredSet):
      continue

    exploredSet.add(expansion[0][0])
    pathConstructDict[expansion[0]] = expansion[1]

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

    for successor in problem.getSuccessors(expansion[0][0]):
      queue.push((successor, expansion[0]))

def breadthFirstSearch(problem):
    """Search the shallowest nodes in the search tree first."""
    "*** YOUR CODE HERE ***"   
    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 = Queue()
    frontier.push(problem.getStartState())
    while (not frontier.isEmpty()):
        state = frontier.pop()
        #print "Current state: ", state
        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):
                parentChild.append((state, successor[1], successor[0]))
               
                frontier.push(successor[0])
    return None

 def __init__(self, position, scaredTimer, game_state, n):
     self.ds = Queue()
     self.position = position
     self.scaredTimer = scaredTimer
     self.game_state = game_state
     self.n = n
     self.visited = {}
     for i in range(n):
         self.visited[i] = []

def breadthFirstSearch(problem):
    from collections import defaultdict
    from util import Queue
    initial_node=problem.getStartState()
    
    current_node=defaultdict(list)
    current_node[initial_node].append("No parent")
    current_node[initial_node].append("No direction")
    queue=Queue()
    queue.push(current_node)
    current_node=queue.pop()
    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=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])
                queue.push(temp_dict)

            current_node=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]
    print "The path is",path
    while (path!= problem.getStartState()):
        backtracking.append(direction_list[path])
        path = visited_list[path]
    backtracking.reverse()

    return backtracking
    


    util.raiseNotDefined()