# search.py
# ---------
# Licensing Information:  You are free to use or extend these projects for
# educational purposes provided that (1) you do not distribute or publish
# solutions, (2) you retain this notice, and (3) you provide clear
# attribution to UC Berkeley, including a link to http://ai.berkeley.edu.
# 
# Attribution Information: The Pacman AI projects were developed at UC Berkeley.
# The core projects and autograders were primarily created by John DeNero
# (denero@cs.berkeley.edu) and Dan Klein (klein@cs.berkeley.edu).
# Student side autograding was added by Brad Miller, Nick Hay, and
# Pieter Abbeel (pabbeel@cs.berkeley.edu).


"""
In search.py, you will implement generic search algorithms which are called by
Pacman agents (in searchAgents.py).
"""

import util

class SearchProblem:
    """
    This class outlines the structure of a search problem, but doesn't implement
    any of the methods (in object-oriented terminology: an abstract class).

    You do not need to change anything in this class, ever.
    """

    def getStartState(self):
        """
        Returns the start state for the search problem.
        """
        util.raiseNotDefined()

    def isGoalState(self, state):
        """
          state: Search state

        Returns True if and only if the state is a valid goal state.
        """
        util.raiseNotDefined()

    def getSuccessors(self, state):
        """
          state: Search state

        For a given state, this should return a list of triples, (successor,
        action, stepCost), where 'successor' is a successor to the current
        state, 'action' is the action required to get there, and 'stepCost' is
        the incremental cost of expanding to that successor.
        """
        util.raiseNotDefined()

    def getCostOfActions(self, actions):
        """
         actions: A list of actions to take

        This method returns the total cost of a particular sequence of actions.
        The sequence must be composed of legal moves.
        """
        util.raiseNotDefined()


def tinyMazeSearch(problem):
    """
    Returns a sequence of moves that solves tinyMaze.  For any other maze, the
    sequence of moves will be incorrect, so only use this for tinyMaze.
    """
    from game import Directions
    s = Directions.SOUTH
    w = Directions.WEST
    return  [s, s, w, s, w, w, s, w]


class Node():
    def __init__(self, position, totalPath, totalCost):
        self.pos = position
        self.cost = totalCost
        self.path = totalPath
    def getPos(self):
        return self.pos
    def getPath(self):
        return self.path
    def getCost(self):
        return self.cost


def depthFirstSearch(problem):
    if(problem.isGoalState(problem.getStartState())):
        return problem.getStartState()
    frontier = util.Stack()
    visited = {}
    initialNode = Node(problem.getStartState(),[], 0)
    frontier.push(initialNode)
    while 1:
        if(frontier.isEmpty()):
            return list()
        node = frontier.pop()
        if node.getPos() in visited:
            continue
        visited[node.getPos()] = True
        if problem.isGoalState(node.getPos()):
            return node.getPath()
        succ = problem.getSuccessors(node.getPos())
        for succNode in succ:
            newPath = node.getPath()[:]
            newPath.append(succNode[1])
            tempNode = Node(succNode[0],newPath, node.getCost()+succNode[2])
            frontier.push(tempNode)

def breadthFirstSearch(problem):
    if(problem.isGoalState(problem.getStartState())):
        return problem.getStartState()
    frontier = util.Queue()
    visited = {}
    initialNode = Node(problem.getStartState(),[], 0)
    frontier.push(initialNode)
    while 1:
        if(frontier.isEmpty()):
            return list()
        node = frontier.pop()
        if node.getPos() in visited:
            continue
        visited[node.getPos()] = True
        if problem.isGoalState(node.getPos()):
            return node.getPath()
        succ = problem.getSuccessors(node.getPos())
        for succNode in succ:
            newPath = node.getPath()[:]
            newPath.append(succNode[1])
            tempNode = Node(succNode[0],newPath, node.getCost()+succNode[2])
            tempStack = util.Queue()
            insertTemp = True
            while not frontier.isEmpty():
                nPrime = frontier.pop()
                if nPrime.getPos()==tempNode.getPos():
                    if tempNode.getCost()<nPrime.getCost():
                        continue
                    else:
                        insertTemp = False
                tempStack.push(nPrime)
            while not tempStack.isEmpty():
                frontier.push(tempStack.pop())
            if insertTemp:
                frontier.push(tempNode)

def uniformCostSearch(problem):
    if(problem.isGoalState(problem.getStartState())):
        return problem.getStartState()
    frontier = util.PriorityQueue()
    visited = {}
    initialNode = Node(problem.getStartState(),[], 0)
    frontier.push(initialNode, 0)
    while 1:
        if(frontier.isEmpty()):
            return list()
        node = frontier.pop()
        if node.getPos() in visited:
            continue
        visited[node.getPos()] = True
        if problem.isGoalState(node.getPos()):
            return node.getPath()
        succ = problem.getSuccessors(node.getPos())
        for succNode in succ:
            newPath = node.getPath()[:]
            newPath.append(succNode[1])
            tempNode = Node(succNode[0],newPath, node.getCost()+succNode[2])
            tempStack = util.PriorityQueue()
            insertTemp = True
            while not frontier.isEmpty():
                nPrime = frontier.pop()
                if nPrime.getPos()==tempNode.getPos():
                    if tempNode.getCost()<nPrime.getCost():
                        continue
                    else:
                        insertTemp = False
                tempStack.push(nPrime, nPrime.getCost())
            while not tempStack.isEmpty():
                bestNode = tempStack.pop()
                frontier.push(bestNode, bestNode.getCost())
            if insertTemp:
                frontier.push(tempNode, tempNode.getCost())

def nullHeuristic(state, problem=None):
    """
    A heuristic function estimates the cost from the current state to the nearest
    goal in the provided SearchProblem.  This heuristic is trivial.
    """
    return 0

def getFCost(problem, heuristic, node):
    return node.getCost()+heuristic(node.getPos(), problem)

def getHeuristicCost(problem, heuristic, node):
    return heuristic(node.getPos(), problem)


def aStarSearch(problem, heuristic=nullHeuristic):
    """Search the node that has the lowest combined cost and heuristic first."""
    if(problem.isGoalState(problem.getStartState())):
        return problem.getStartState()
    frontier = util.PriorityQueue()
    visited = {}
    initialNode = Node(problem.getStartState(),[], 0)
    frontier.push(initialNode, getFCost(problem, heuristic, initialNode))
    while 1:
        if(frontier.isEmpty()):
            return list()
        node = frontier.pop()
        if node.getPos() in visited:
            continue
        visited[node.getPos()] = True
        if problem.isGoalState(node.getPos()):
            return node.getPath()
        succ = problem.getSuccessors(node.getPos())
        for succNode in succ:
            newPath = node.getPath()[:]
            newPath.append(succNode[1])
            tempNode = Node(succNode[0], newPath, node.getCost() + succNode[2])
            #Test for heuristic consistency
            #if succNode[2]+getHeuristicCost(problem, heuristic, tempNode)<getHeuristicCost(problem, heuristic, node):
                #print "Not consistent, rip :'(", succNode[2], getFCost(problem, heuristic, tempNode), getFCost(problem, heuristic, node)
            tempStack = util.PriorityQueue()
            insertTemp = True
            while not frontier.isEmpty():
                nPrime = frontier.pop()
                if nPrime.getPos()==tempNode.getPos():
                    if getFCost(problem, heuristic, tempNode)<getFCost(problem, heuristic, nPrime):
                        continue
                    else:
                        insertTemp = False
                tempStack.push(nPrime, getFCost(problem, heuristic, nPrime))
            while not tempStack.isEmpty():
                bestNode = tempStack.pop()
                frontier.push(bestNode, getFCost(problem, heuristic, bestNode))
            if insertTemp:
                frontier.push(tempNode, getFCost(problem, heuristic, tempNode))


# Abbreviations
bfs = breadthFirstSearch
dfs = depthFirstSearch
astar = aStarSearch
ucs = uniformCostSearch
