automatyczny_kelner/src/StateController.py

from .obj.TemporaryState import TemporaryState
from queue import PriorityQueue


class StateController:
    def __init__(self, istate):
        self.path = []
        self.explored = []
        self.fringe = PriorityQueue()
        self.istate = istate
        self.goal = istate.position

    def reset(self):
        self.path.clear()
        self.explored.clear()
        self.fringe = PriorityQueue()

    def build_path(self, goal_state, engine):
        total_cost = goal_state.cost
        self.path.append(goal_state)
        engine.goals.pop()
        while self.path[-1].parent.agent_role not in ["blank", "waiter"]:
            self.path.append(self.path[-1].parent)
            total_cost += self.path[-1].cost

        print("Total path cost:\t{0}".format(total_cost))
        return self.path

    def graphsearch(self, engine):            # A*
        print("Search path")

        self.goal = list(engine.goals[-1].position)

        self.reset()

        start = TemporaryState(self.istate, 0)

        self.fringe.put(start)

        while self.fringe.queue and not self.path:
            self.explored.append(self.fringe.get())
            if self.goal_test(engine):
                return

            self.succ(self.explored[-1].front(), engine)
            self.succ(self.explored[-1].left(), engine)
            self.succ(self.explored[-1].right(), engine)

            engine.redraw()

        self.reset()
        for o in engine.objects:
            o.compare_pos(engine.goals[-1].position)
            o.agent_role = "block"
        engine.goals.pop()

        print("Not found")

        return False

    def succ(self, state, engine):
        if state.collide_test():
            return
        elif any(e.compare(state) for e in self.explored):
            return
        elif any([o.collide_test(state) for o in engine.objects]):
            return

        for o in engine.objects:
            if state.cost != 1:
                break
            if o.position == state.position:
                state.change_cost(o)

        state.cost_so_far = self.explored[-1].cost_so_far + state.cost

        in_explored = any([state.compare(s) for s in self.explored]
                          )

        in_frige = any([state.compare(f) for f in self.fringe.queue])

        if not in_explored and not in_frige:
            state.heuristic(self.goal)
            self.fringe.put(state)

        elif in_frige:
            fringe = state
            for f in self.fringe.queue:
                if state.compare(f):
                    fringe = f
                    break

            if state.cost_so_far < fringe.cost_so_far:
                fringe.replace(state)

    def goal_test(self, engine) -> bool:
        if self.explored[-1].position == self.goal:
            self.__is_goal__(self.explored[-1], engine)
            return True

        for fringe in self.fringe.queue:
            if fringe.position == self.goal:
                self.__is_goal__(fringe, engine)
                return True

        return False

    def __is_goal__(self, goal_state, engine):
        self.reset()
        self.build_path(goal_state, engine)