Inteligentna_smieciarka/astar.py

from succ import succ as successors
from queue import PriorityQueue
from state import State

def astar(istate, goalx, goaly, passedFields):
    fringe = PriorityQueue()
    fringe.put(istate)
    explored = set()
    steps = []
    while not fringe.empty():
        state = fringe.get()
        if state.xpos == goalx and state.ypos == goaly:
            steps.insert(0, state)
            while (state.parent != None):
                state = state.parent
                steps.insert(0, state)
            return steps

        element = successors(state, passedFields, goalx, goaly)
        explored.add((state.xpos, state.ypos, state.orientation))
        for value in element:
            val = (value.xpos, value.ypos, value.orientation)
            if val in explored:
                continue 
            cost = state.priority + value.priority
            succesorState = State(state, value.action, 
                                  value.xpos, value.ypos, 
                                  value.orientation, cost, 
                                  value.heuristic)
            if value not in fringe.queue:
                fringe.put(succesorState)
            else :
                for element in fringe.queue:
                    if (element.xpos==value.xpos and element.ypos==value.ypos) and element > value:
                        element.parent = state
                        element.priority = value.priority
    return False