SI_InteligentnyWozekWidlowy/pathfinding/PathfinderOnStates.py

from typing import List
from typing import Tuple

from data.GameConstants import GameConstants
from data.enum.Direction import Direction
from decision.Action import Action
from decision.ActionType import ActionType
from pathfinding.PathFinderState import PathFinderState
from pathfinding.PrioritizedItem import PrioritizedItem
from util.PathDefinitions import GridLocation
from util.PriorityQueue import PriorityQueue


class PathFinderOnStates:
    def __init__(self, game_constants: GameConstants, goal: GridLocation, root_state: PathFinderState):
        super().__init__()
        self.game_constants = game_constants
        self.goal = goal
        self.queue = PriorityQueue()
        self.queue.put(PrioritizedItem(root_state.cost, root_state), root_state.cost)

    def heuristic(self, a: Tuple[int, int], b: Tuple[int, int]) -> float:
        # tutaj mozna uzyc heury np. manhatan distance (zmodyfikowany bo masz obroty a to zmienia oplacalnosc)
        (x1, y1) = a
        (x2, y2) = b
        return abs(x1 - x2) + abs(y1 - y2)

    def evaluate(self, curr_state: PathFinderState) -> float:
        # koszt dojscia do danego stanu+ heura
        return curr_state.cost + self.heuristic(curr_state.agent_position, self.goal)

    def get_position_after_move(self, curr_state: PathFinderState) -> GridLocation:
        if curr_state.agent_direction == Direction.top:
            return curr_state.agent_position[0], curr_state.agent_position[1] + 1

        elif curr_state.agent_direction == Direction.down:
            return curr_state.agent_position[0], curr_state.agent_position[1] - 1

        elif curr_state.agent_direction == Direction.right:
            return curr_state.agent_position[0] + 1, curr_state.agent_position[1]

        elif curr_state.agent_direction == Direction.left:
            return curr_state.agent_position[0] - 1, curr_state.agent_position[1]

    def is_move_possible(self, curr_state: PathFinderState) -> bool:
        position_after_move = self.get_position_after_move(curr_state)
        if position_after_move in self.game_constants.walls:
            return False
        elif position_after_move[0] < 0 or position_after_move[0] > self.game_constants.grid_width:
            return False
        elif position_after_move[1] < 0 or position_after_move[1] > self.game_constants.grid_height:
            return False
        else:
            return True

    def create_state(self, curr_state: PathFinderState, action: Action) -> PathFinderState:

        if action == action.action_type.MOVE:
            if curr_state.agent_position in self.game_constants.diffTerrain:
                cost = curr_state.cost + 45
            # tutaj koszt kaluzy
            else:
                cost = curr_state.cost + 1
        # TODO: jezeli bedziemy rozpatrywac rozne stany to nalezy rozbic na kazdy mozliwy obrot
        else:
            cost = curr_state.cost + 10

        last_action = action
        action_taken: List[Action] = []
        action_taken.extend(curr_state.action_taken)
        action_taken.append(last_action)
        agent_position = curr_state.agent_position
        agent_direction = curr_state.agent_direction

        if action.action_type == ActionType.ROTATE_UP:
            agent_direction = Direction.top
        elif action.action_type == ActionType.ROTATE_DOWN:
            agent_direction = Direction.down
        elif action.action_type == ActionType.ROTATE_LEFT:
            agent_direction = Direction.left
        elif action.action_type == ActionType.ROTATE_RIGHT:
            agent_direction = Direction.right
        elif action.action_type == ActionType.MOVE:
            agent_position = self.get_position_after_move(curr_state)

        return PathFinderState(agent_position, agent_direction, cost, last_action, action_taken)

    # Funkcja następnika
    def expansion(self, curr_state: PathFinderState) -> List[PathFinderState]:
        # dla stanu sprawdzamy jakie akcje z tego miejsca mozemy podjac (ActionType)
        # reprezentacja kazdego stanu co moge podjac z tego miejsca
        # generowanie stanu
        # sprawdz w ktorym kierunku obrocony
        possible_next_states: List[PathFinderState] = []
        if self.is_move_possible(curr_state):
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.MOVE)))

        if curr_state.agent_direction == Direction.top:
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_RIGHT)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_LEFT)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_DOWN)))

        elif curr_state.agent_direction == Direction.down:
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_RIGHT)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_LEFT)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_UP)))

        elif curr_state.agent_direction == Direction.left:
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_RIGHT)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_UP)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_DOWN)))

        elif curr_state.agent_direction == Direction.right:
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_UP)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_LEFT)))
            possible_next_states.append(self.create_state(curr_state, Action(ActionType.ROTATE_DOWN)))

        return possible_next_states

    def get_action_list(self) -> List[Action]:
        already_visited = {}

        while not self.queue.empty():
            item: PrioritizedItem = self.queue.get()
            best_state: PathFinderState = item.item

            if best_state.agent_position == self.goal or (self.heuristic(best_state.agent_position, self.goal) == 1
                                                          and self.goal in self.game_constants.walls):
                break

            for state in self.expansion(best_state):
                s_tuple = (state.agent_position[0], state.agent_position[1], state.agent_direction)

                if s_tuple not in already_visited:
                    priority = self.evaluate(state)
                    self.queue.put(PrioritizedItem(priority, state), priority)
                    already_visited[s_tuple] = state

        return best_state.action_taken