From 2ce57ea190975da62b58f89a0e68d139889b357c Mon Sep 17 00:00:00 2001
From: Pawel Felcyn <pawel.felcyn@onet.pl>
Date: Sun, 23 Apr 2023 16:42:45 +0200
Subject: [PATCH] visit all garbage cans with bfs

---
 bfs.py      | 10 ++++++++--
 main.py     |  4 ++--
 movement.py | 13 +++++++++++++
 3 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/bfs.py b/bfs.py
index 9373df7..2c7c646 100644
--- a/bfs.py
+++ b/bfs.py
@@ -83,7 +83,13 @@ def get_next_cell(state: AgentState) -> Tuple[int, int]:
 
 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
+    try:
+        return grid[next_cell] == GridCellType.GARBAGE_CAN
+    except:
+        return False
 
 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
\ No newline at end of file
+    try: 
+        return grid[state.position] == GridCellType.STREET_HORIZONTAL or grid[state.position] == GridCellType.STREET_VERTICAL
+    except:
+        return False
\ No newline at end of file
diff --git a/main.py b/main.py
index 88a0c48..1a8c608 100644
--- a/main.py
+++ b/main.py
@@ -4,7 +4,7 @@ from gameContext import GameContext
 from startup import startup
 from PIL import Image
 from agentActionType import AgentActionType
-from movement import move_dust_car
+from movement import collect_garbage
 
 pygame.init()
 
@@ -18,7 +18,7 @@ game_context.dust_car_pil = dust_car_pil
 game_context.dust_car_pygame = pygame.image.frombuffer(dust_car_pil.tobytes(), dust_car_pil.size, 'RGB')
 game_context.canvas = canvas
 startup(game_context)
-
+collect_garbage(game_context)
 # test = [AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD,
 #         AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD, AgentActionType.MOVE_FORWARD,
 #         AgentActionType.MOVE_FORWARD, AgentActionType.TURN_RIGHT, AgentActionType.MOVE_FORWARD,
diff --git a/movement.py b/movement.py
index 39f0650..1947a50 100644
--- a/movement.py
+++ b/movement.py
@@ -7,6 +7,19 @@ from gridCellType import GridCellType
 from gameContext import GameContext
 from agentOrientation import AgentOrientation
 import pygame
+from bfs import find_path_to_nearest_can
+from agentState import AgentState
+
+def collect_garbage(game_context: GameContext) -> None:
+    while True:
+        start_agent_state = AgentState(game_context.dust_car.position, game_context.dust_car.orientation)
+        path = find_path_to_nearest_can(start_agent_state, game_context.grid)
+        if path == None or len(path) == 0:
+            break
+        move_dust_car(path, game_context)
+        next_position = calculate_next_position(game_context.dust_car)
+        game_context.grid[next_position] = GridCellType.VISITED_GARBAGE_CAN
+    pass
 
 def move_dust_car(actions: list[AgentActionType], game_context: GameContext) -> None:
     for action in actions: