From 4416734de8428706b102d092387e293d4a1f7c9d Mon Sep 17 00:00:00 2001
From: s473593 <timbar@st.amu.edu.pl>
Date: Fri, 5 May 2023 15:12:48 +0200
Subject: [PATCH] fixed current node cost

---
 AI_brain/rotate_and_go_a_star.py | 26 ++++++++++++++++------
 domain/world.py                  |  2 +-
 main.py                          | 37 ++++++++++++++++++++++++--------
 3 files changed, 49 insertions(+), 16 deletions(-)

diff --git a/AI_brain/rotate_and_go_a_star.py b/AI_brain/rotate_and_go_a_star.py
index 33e9736..34ad92f 100644
--- a/AI_brain/rotate_and_go_a_star.py
+++ b/AI_brain/rotate_and_go_a_star.py
@@ -25,8 +25,8 @@ class Node:
         self.state = state
         self.parent = None
         self.action = None
-        self.g = None
-        self.h = None
+        self.g = 0
+        self.h = 0
 
 
 @dataclass(order=True)
@@ -78,10 +78,14 @@ class RotateAndGoAStar:
                 next_node = Node(state)
                 next_node.action = action
                 next_node.parent = elem
-                next_node.g = abs(elem.state.x - state.x) + abs(elem.state.y - state.y) + self.world.get_cost(state.x, state.y)
-                if next_node.g > 100:
-                    print(str(state.x) + ":" + str(state.y))
-                next_node.h = abs(state.x - self.goal_state.x) ** 2 + abs(state.y - self.goal_state.y) ** 2
+                next_node.g = (elem.g
+                        + abs(elem.state.x - state.x)
+                        + abs(elem.state.y - state.y)
+                        + self.world.get_cost(state.x, state.y)
+                               )
+
+                next_node.h = abs(state.x - self.goal_state.x) + abs(state.y - self.goal_state.y)
+                self.print_fringe()
                 f = next_node.g + next_node.h
 
                 if state not in self.enqueued_states and state not in self.explored:
@@ -94,6 +98,16 @@ class RotateAndGoAStar:
 
         return False
 
+    def print_fringe(self):
+        elems = []
+        while not self.fringe.empty():
+            e = self.fringe.get()
+            elems.append(e)
+            print(str(e.item.state.x) + ":" + str(e.item.state.x))
+
+        for e in elems:
+            self.fringe.put(e)
+
     def add_existed(self, node: Node, f: int):
         old = []
         while not self.fringe.empty():
diff --git a/domain/world.py b/domain/world.py
index 1324bf8..426943a 100644
--- a/domain/world.py
+++ b/domain/world.py
@@ -3,7 +3,7 @@ from domain.entities.entity import Entity
 
 class World:
     def __init__(self, width: int, height: int) -> object:
-        self.costs = [[0 for j in range(height)] for i in range(width)]
+        self.costs = [[1000 for j in range(height)] for i in range(width)]
         self.width = width
         self.height = height
         self.dust = [[[] for j in range(height)] for i in range(width)]
diff --git a/main.py b/main.py
index b950899..b7757d4 100644
--- a/main.py
+++ b/main.py
@@ -134,11 +134,11 @@ class Main:
 
 def generate_world(tiles_x: int, tiles_y: int) -> World:
     world = World(tiles_x, tiles_y)
-    for _ in range(10):
+    for _ in range(20):
         temp_x = randint(0, tiles_x - 1)
         temp_y = randint(0, tiles_y - 1)
         world.add_entity(Entity(temp_x, temp_y, "PEEL"))
-    world.vacuum = Vacuum(1, 1)
+    world.vacuum = Vacuum(0, 0)
     world.doc_station = Doc_Station(9, 8)
     if config.getboolean("APP", "cat"):
         world.cat = Cat(7, 8)
@@ -150,13 +150,32 @@ def generate_world(tiles_x: int, tiles_y: int) -> World:
     world.add_entity(Entity(9, 3, "PLANT3"))
 
     # TEST
-    world.costs[9][3] = 1000
-    world.costs[8][3] = 1000
-    world.costs[7][3] = 1000
-    world.costs[6][3] = 1000
-    world.costs[5][3] = 1000
-    world.costs[4][3] = 1000
-    world.costs[3][3] = 1000
+    # world.costs[9][3] = 1000
+    # world.costs[8][3] = 1000
+    # world.costs[7][3] = 1000
+    # world.costs[6][3] = 1000
+    # world.costs[5][3] = 1000
+    # world.costs[4][3] = 1000
+    # world.costs[3][3] = 1000
+    # world.costs[2][3] = 1000
+    # # world.costs[1][3] = 1000
+    # world.costs[0][3] = 1000
+    #
+    # world.costs[2][4] = 1000
+    # world.costs[1][5] = 1000
+    # world.costs[5][4] = 1000
+    # # world.costs[5][5] = 1000
+    # world.costs[5][6] = 1000
+    # world.costs[5][7] = 1000
+    # world.costs[5][8] = 1000
+    # world.costs[5][9] = 1000
+
+    for x in range(world.width):
+        for y in range(world.height):
+            if world.is_garbage_at(x, y):
+                world.costs[x][y] = 1;
+
+    # world.costs[0][0] = -1000
 
     return world