From 8dae8a4b20d0e664683aa88dcc6b2829b7e1d9dc Mon Sep 17 00:00:00 2001
From: Mateusz Kantorski <matkan1@st.amu.edu.pl>
Date: Wed, 26 Apr 2023 18:59:38 +0200
Subject: [PATCH] for merge

---
 main.py         |  4 +--
 wyszukiwanie.py | 71 ++++++++++++++++++++++++++++++++++++++++---------
 2 files changed, 60 insertions(+), 15 deletions(-)

diff --git a/main.py b/main.py
index ba6e036..fa0e1e5 100644
--- a/main.py
+++ b/main.py
@@ -26,8 +26,8 @@ def main():
     
         grid = SearchSpace()
         # create start and goal states
-        start_state = Stan(x=0, y=0, kierunek=0)  # for example
-        goal_state = Stan(x=0, y=1, kierunek=0)  # for example
+        start_state = Stan(x=1, y=0, kierunek=0)  # for example
+        goal_state = Stan(x=12, y=0, kierunek=0)  # for example
 
         # perform BFS search
         path = wyszukiwanie_bfs(start_state, goal_state, grid)
diff --git a/wyszukiwanie.py b/wyszukiwanie.py
index bdd5101..abb8664 100644
--- a/wyszukiwanie.py
+++ b/wyszukiwanie.py
@@ -1,31 +1,35 @@
 class SearchSpace:
     def __init__(self):
-        self.grid = [['1']*10 for _ in range(10)]  # create a 10x10 grid of cells
-        for r, c in [(2,2), (2,3), (3,2), (3,3), (8,2), (8,3), (9,2), (9,3),
-                     (2,8), (2,9), (3,8), (3,9), (8,8), (8,9), (9,8), (9,9)]:
+        self.grid = [['1'] * 10 for _ in range(10)]  # create a 10x10 grid of cells
+        for r, c in [(2, 2), (2, 3), (3, 2), (3, 3), (8, 2), (8, 3), (9, 2), (9, 3),
+                     (2, 8), (2, 9), (3, 8), (3, 9), (8, 8), (8, 9), (9, 8), (9, 9)]:
             self.grid[r][c] = 'X'  # set the cells with a shelf to not passable
 
     def is_passable(self, x, y):
         return 0 <= x < 10 and 0 <= y < 10 and self.grid[y][x] != 'X'
-    
+
+
 class Stan:
     def __init__(self, x, y, kierunek):
         self.x = x
         self.y = y
         self.kierunek = kierunek
 
+
 class Wezel:
     def __init__(self, stan):
         self.stan = stan
 
+
 def poprzednik(wezel, search_space):
- # gora -> prawo -> dol -> lewo          | obrot w prawo
+    # gora -> prawo -> dol -> lewo          | obrot w prawo
     # gora -> lewo -> dol -> prawo          | obrot w lewo
     # 0 gora 1 prawo 2 dol 3 lewo
     y = wezel.stan.x
     x = wezel.stan.y
     obrot_w_prawo = Stan(x, y, (wezel.stan.kierunek + 1) % 4)
-    obrot_w_lewo = Stan(x, y, (wezel.stan.kierunek - 1) % 4)
+    # obrot_w_lewo = Stan(x, y, (wezel.stan.kierunek - 1) % 4)
+    obrot_w_lewo = Stan(x, y, 3 if wezel.stan.kierunek == 0 else wezel.stan.kierunek - 1)
 
     if wezel.stan.kierunek == 0:
         y -= 1
@@ -45,23 +49,64 @@ def poprzednik(wezel, search_space):
 
     return wezly
 
+
+# def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_space):
+#     pierwszy_wezel = Wezel(stan_poczatkowy)
+#     fringe = [pierwszy_wezel]
+#
+#     odwiedzone = []
+#     odwiedzone.append(pierwszy_wezel)
+#     while fringe:
+#         wezel = fringe.pop(0)
+#         if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
+#             return odwiedzone
+#
+#         # odwiedzone.add(wezel.stan)
+#
+#         for stan in poprzednik(wezel, search_space):
+#             if stan not in odwiedzone:
+#                 nowy_wezel = Wezel(stan)
+#                 fringe.append(nowy_wezel)
+#                 odwiedzone.append(nowy_wezel)
+#                 if stan_docelowy.x == nowy_wezel.stan.x and stan_docelowy.y == nowy_wezel.stan.y:
+#                     return odwiedzone
+#     return odwiedzone
+
 def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_space):
     pierwszy_wezel = Wezel(stan_poczatkowy)
     fringe = [pierwszy_wezel]
-    
-    odwiedzone = set()
-
+    odl_x = 100
+    odl_y = 100
+    # odleglosci = [[odleglosc_x, odleglosc_y]]
+    odwiedzone = [stan_poczatkowy]
     while fringe:
         wezel = fringe.pop(0)
 
         if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
             return odwiedzone
 
-        odwiedzone.add(wezel.stan)
-
+        # odwiedzone.append(wezel.stan)
         for stan in poprzednik(wezel, search_space):
-            if stan not in odwiedzone:
+            flag = 0
+            for x in odwiedzone:
+                if x.x == stan.x and x.y == stan.y and x.kierunek == stan.kierunek:
+                    flag = 1
+                    break
+            if flag == 1:
+                continue
+            else:
+                odleglosc_x = abs(stan.x - stan_docelowy.x)
+                odleglosc_y = abs(stan.y - stan_docelowy.y)
+                if odl_x + odl_y >= odleglosc_x + odleglosc_y:
+                    odl_x = odleglosc_x
+                    odl_y = odleglosc_y
+                # else:
+                #                 #     # print("JEST")
+                #                 #     pass
+
                 nowy_wezel = Wezel(stan)
                 fringe.append(nowy_wezel)
-                odwiedzone.add(stan)
+                odwiedzone.append(stan)
+                if stan_docelowy.x == stan.x and stan_docelowy.y == stan.y:
+                    return odwiedzone
     return odwiedzone