implement bfs algorythm

agentOrientation.py

@@ -0,0 +1,7 @@
+from enum import Enum
+
+class AgentOrientation (Enum):
+    UP = 0
+    RIGHT = 1
+    DOWN = 2
+    LEFT = 3

bfs.py

@@ -0,0 +1,94 @@
+from agentState import AgentState
+from typing import Dict, Tuple
+from gridCellType import GridCellType
+from agentActionType import AgentActionType
+from agentOrientation import AgentOrientation
+from queue import Queue
+
+class Succ:
+    state: AgentState
+    action: AgentActionType
+
+    def __init__(self, state: AgentState, action: AgentActionType) -> None:
+        self.state = state
+        self.action = action
+
+def find_path_to_nearest_can(startState: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> list[AgentActionType]:
+    q: Queue[list[Succ]] = Queue()
+    visited: list[Tuple[int, int]] = []
+    startStates: list[Succ] = [Succ(startState, AgentActionType.UNKNOWN)]
+    q.put(startStates)
+    while not q.empty():
+        currently_checked = q.get()
+        visited.append(currently_checked[-1].state.position)
+        if is_state_success(currently_checked[-1].state, grid):
+            return extract_actions(currently_checked)
+        successors = succ(currently_checked[-1].state)
+        for s in successors:
+            already_visited = False
+            for v in visited:
+                if v[0] == s.state.position[0] and v[1] == s.state.position[1]:
+                    already_visited = True
+                    break
+            if already_visited:
+                continue
+            if is_state_valid(s.state, grid):
+                new_list = currently_checked.copy()
+                new_list.append(s)
+                q.put(new_list)
+    return []
+
+        
+        
+def extract_actions(successors: list[Succ]) -> list[AgentActionType]:
+    output: list[AgentActionType] = []
+    for s in successors:
+        if s.action != AgentActionType.UNKNOWN:
+            output.append(s.action)
+    return output
+
+def succ(state: AgentState) -> list[Succ]:
+    result: list[Succ] = []
+    result.append(Succ(AgentState(state.position, turn_left_orientation(state.orientation)), AgentActionType.TURN_LEFT))
+    result.append(Succ(AgentState(state.position, turn_right_orientation(state.orientation)), AgentActionType.TURN_RIGHT))
+    state_succ = move_forward_succ(state)
+    if state_succ != None:
+        result.append(move_forward_succ(state))
+    return result
+
+def turn_left_orientation(orientation: AgentOrientation) -> AgentOrientation:
+    return (orientation - 1) % 4
+
+def turn_right_orientation(orientation: AgentOrientation) -> AgentOrientation:
+    return (orientation + 1) % 4
+
+def move_forward_succ(state: AgentState) -> Succ:
+    position = get_next_cell(state)
+    if position == None:
+        return None
+    return Succ(AgentState(position, state.orientation), AgentActionType.MOVE_FORWARD)
+
+
+def get_next_cell(state: AgentState) -> Tuple[int, int]:
+    if state.orientation == AgentOrientation.UP:
+        if state.position[1] - 1 < 1:
+            return None
+        return (state.position[0], state.position[1] - 1)
+    if state.orientation == AgentOrientation.DOWN:
+        if state.position[1] + 1 > 27:
+            return None
+        return (state.position[0], state.position[1] + 1)
+    if state.orientation == AgentOrientation.LEFT:
+        if state.position[0] - 1 < 1:
+            return None
+        return (state.position[0] - 1, state.position[1])
+    if state.position[0] + 1 > 27:
+        return None
+    return (state.position[0] + 1, state.position[1])
+
+def is_state_success(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool:
+    next_cell = get_next_cell(state)
+    return grid[next_cell] == GridCellType.GARBAGE_CAN
+
+def is_state_valid(state: AgentState, grid: Dict[Tuple[int, int], GridCellType]) -> bool:
+    return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[state.position] == GridCellType.STREET_VERTICAL