Added BFS algorithm for pathfinding

main.py

@@ -3,8 +3,6 @@ import random
 import time
 import numpy
 import threading
-import collections
-from collections import deque
 
 kuchnia_xy = 0
 pozycja_startowa = 0
@@ -14,45 +12,28 @@ blockSize = 60
 rows = 14
 columns = 24
 
-
 #-----------------------------Inicjacja klas
-class Stan:
-    def __init__(self,position, kierunek):
-        self.position = position
-        self.kierunek = kierunek
-
-    toVisit = deque()
-    visited = deque()
-
-    def succ(self):
-        newStan: Stan(x+1, y, 1)
-        queue = deque([x, y, self.kierunek])
-        print(queue[0])
-
-#def BFS:
-
-
 class Kelner:
     def __init__(self, x, y):
         self.x = x
         self.y = y
         self.speed = 70 # od 0 do 100, preferowane 70
+        self.stanPrzestrzeni = [0,0,0]
         self.stan = "stoi"  # Stan kelnera: stoi, odbiera lub wraca
         self.stolik_docelowy = None  # Stolik, do którego idzie kelner
         self.chodzi = True
         self.cel_x = x
         self.cel_y = y
         self.kierunek = 0   # 0 - północ, 1 - wschód, 2 - południe, 3 - zachód
+        self.indexRuchu = 0
 
     def wklej(self):
         kelnerRotated = pygame.transform.rotate(kelnerImg, -90 * kelner.kierunek)
         screen.blit(kelnerRotated, (self.x * blockSize, self.y * blockSize))
 
-    #def planuj_ruch(self):
+    # def idz_do_stolika(self):
-
+    #     self.cel_x, self.cel_y = self.stolik_docelowy.x, self.stolik_docelowy.y
-    def idz_do_stolika(self):
+    #     kelner.stan = "odbiera"
-        self.cel_x, self.cel_y = self.stolik_docelowy.x, self.stolik_docelowy.y
-        kelner.stan = "odbiera"
 
     def idz_do_kuchni(self):
         self.cel_x, self.cel_y = kuchnia_xy, kuchnia_xy
@@ -60,26 +41,40 @@ class Kelner:
         kelner.stan = "wraca"
 
     def obrot_w_lewo(self):
-        if self.kierunek == 0:
+        self.kierunek = (self.kierunek - 1) % 4
-            self.kierunek = 3
+        self.stanPrzestrzeni[2] = (self.stanPrzestrzeni[2] - 1) % 4  
-        else:
-            self.kierunek -= 1
 
     def obrot_w_prawo(self):
-        if self.kierunek == 3:
+        self.kierunek = (self.kierunek + 1) % 4
-            self.kierunek = 0
+        self.stanPrzestrzeni[2] = (self.stanPrzestrzeni[2] + 1) % 4
-        else:
-            self.kierunek += 1
 
     def idz_do_przodu(self):
         if self.kierunek == 0:
             self.y -=  1
+            self.stanPrzestrzeni[1] -= 1
         elif self.kierunek == 1:
             self.x +=  1
+            self.stanPrzestrzeni[0] += 1
         elif self.kierunek == 2:
             self.y +=  1
+            self.stanPrzestrzeni[1] += 1
         elif self.kierunek == 3:
             self.x -=  1
+            self.stanPrzestrzeni[0] -= 1
+        
+
+    def wykonajAkcje(self, ruchy):
+        if self.indexRuchu < len(ruchy):
+            akcja = ruchy[self.indexRuchu]
+            if akcja == 'F':
+                self.idz_do_przodu()
+            elif akcja == 'L':
+                self.obrot_w_lewo()
+            elif akcja == 'R':
+                self.obrot_w_prawo()
+            self.indexRuchu += 1
+            if self.indexRuchu >= len(ruchy):  # Reset po zakończeniu wszystkich ruchów
+                self.indexRuchu = 0
 
 
 class Stolik:
@@ -92,11 +87,81 @@ class Stolik:
         screen.blit(stolikImg, (self.x * blockSize, self.y * blockSize))
 
 
+#-----------------Przeszukiwanie przestrzeni stanów
+
+from collections import deque
+
+def bfs(start, cel, stoliki):
+    queue = deque([start])
+    odwiedzone = set([start])
+    poprzednik = {start: (None, None)}  # Słownik ścieżek (stan, ruch)
+
+    while queue:
+        obecny = queue.popleft()
+
+        if obecny[:2] == cel:
+            return odtworz_ruchy(poprzednik, obecny)
+
+        for sasiad, ruch in generuj_nastepniki_i_ruchy(obecny, stoliki):
+            if sasiad not in odwiedzone:
+                odwiedzone.add(sasiad)
+                queue.append(sasiad)
+                poprzednik[sasiad] = (obecny, ruch)
+
+    return []  # Cel nie został znaleziony
+
+
+#----------Funkcja generowania następników dla poszczególnych stanów
+def generuj_nastepniki_i_ruchy(stan, stoliki):
+    x, y, kierunek = stan
+    ruchy = []
+
+    # Obrot w lewo
+    nowy_kierunek = (kierunek - 1) % 4
+    ruchy.append(((x, y, nowy_kierunek), 'L'))
+
+    # Obrot w prawo
+    nowy_kierunek = (kierunek + 1) % 4
+    ruchy.append(((x, y, nowy_kierunek), 'R'))
+
+    # Krok do przodu
+    if kierunek == 0:
+        nowy_x, nowy_y = x, y - 1
+    elif kierunek == 1:
+        nowy_x, nowy_y = x + 1, y
+    elif kierunek == 2:
+        nowy_x, nowy_y = x, y + 1
+    elif kierunek == 3:
+        nowy_x, nowy_y = x - 1, y
+
+    #sprawdzamy, czy następny stan jest w granicach planszy
+    if 0 <= nowy_x < columns and 0 <= nowy_y < rows:
+        #sprawdzamy, czy następny stan nie wchodzi w stolik
+        if (nowy_x, nowy_y) not in stoliki:
+            ruchy.append(((nowy_x, nowy_y, kierunek), 'F'))
+
+    return ruchy
+
+#-----Funkcja tworząca listę kroków potrzebnych do uzyskania celu
+def odtworz_ruchy(poprzednicy, cel):
+    ruchy = []
+    krok = cel
+    while poprzednicy[krok][0] is not None:
+        ruchy.append(poprzednicy[krok][1])
+        krok = poprzednicy[krok][0]
+    ruchy.reverse()
+    return ruchy
+
+
+start = (0, 0, 0)  # Początkowy stan
+cel = (0, 0)  # Docelowe współrzędne
+
+
 #--------------Inicjacja obiektów
 kelner = Kelner(pozycja_startowa,pozycja_startowa)
 
 #-----------wspolrzedne stolikow
-coords = ["8 8", "4 12", "16 8", "12 12", "20 12"]
+coords = ["8 10", "4 12", "16 10", "12 12", "20 12", "12 9", "0 6", "8 4", "16 4", "23 6"]
 
 #Tworzenie listy stolikow
 stoliki = []
@@ -122,15 +187,12 @@ menuImg = pygame.transform.scale(menuImg, (blockSize / 2, blockSize / 2))
 kitchenImg = pygame.image.load("kitchen.png")
 kitchenImg = pygame.transform.scale(kitchenImg, (blockSize * 2, blockSize * 2))
 
-
 def kuchnia(x, y):
     screen.blit(kitchenImg, (x * blockSize, y * blockSize))
 
-
 def menu(x, y):
     screen.blit(menuImg, (x * blockSize, y * blockSize))
 
-
 def wypiszOkno():
     screen.fill((0, 0, 0))
     for x in range(0, width, blockSize):
@@ -138,14 +200,19 @@ def wypiszOkno():
             rect = pygame.Rect(x, y, blockSize, blockSize)
             pygame.draw.rect(screen, (200, 200, 200), rect, 1) #-------------Wypisz kratę -TA
 
-
 run = True
 
 # czcionka = pygame.font.SysFont('Arial',50)
 
 licznik = 0
+ruchy = []
+cel2 = []
 
 while run:
+    stoliki_pozycje = [(stolik.x, stolik.y) for stolik in stoliki]
+    cel2 = list(cel)
+
+    #print(f"{kelner.stanPrzestrzeni}, {cel2}, {kelner.indexRuchu} {kelner.stan}")
     wypiszOkno()
     kuchnia(kuchnia_xy, kuchnia_xy)
 
@@ -165,8 +232,16 @@ while run:
             menu(stolik.x, stolik.y)
             if kelner.stan == "stoi":
                 kelner.stolik_docelowy = stolik
-                #kelner.planuj_ruch()
+                kelner.cel_x, kelner.cel_y = kelner.stolik_docelowy.x, kelner.stolik_docelowy.y - 1
-                kelner.idz_do_stolika()
+                cel = (kelner.cel_x, kelner.cel_y)
+                print("Szukam ścieżki do stolika...")
+                ruchy = bfs(tuple(kelner.stanPrzestrzeni), cel, stoliki_pozycje)
+                kelner.stan = "odbiera"
+                if ruchy:
+                    print("Znaleziono ścieżkę ruchów: ", ruchy)
+                else:
+                    print("Nie znaleziono ścieżki do celu.")
+                
 
     #----------Losuje stoliki, które dokonają zamówienia
     if kelner.stan == "stoi":
@@ -180,31 +255,26 @@ while run:
     #print(kelner.stan)--------------------------Wypisuje stan kelnera
     #print(f"{kelner.x} {kelner.y}")-------------Wypisuje wspolrzedne kelnera
 
-    # --------------Zmiana kierunku kelnera
-    if kelner.x != kelner.cel_x:
-        if kelner.x < kelner.cel_x:
-            kelner.kierunek = 1
-        else:
-            kelner.kierunek = 3
-    else:
-        if kelner.y < kelner.cel_y:  # zmieniamy pozycje Y dopiero, gdy pozycja X bedzie prawidlowa
-            kelner.kierunek = 2
-        else:
-            kelner.kierunek = 0
-
     #----------------Zmiana pozycji kelnera
-    if kelner.chodzi == True and licznik % (101 - kelner.speed) == 0:  # ograniczenie prędkości
+    if kelner.chodzi == True and licznik % (101 - kelner.speed) == 0 and kelner.stanPrzestrzeni[:2] != cel2: #ograniczenie prędkości
-        kelner.idz_do_przodu()
+        kelner.wykonajAkcje(ruchy)
 
-    # ----------------Sprawdzenie, czy kelner jest u celu
+
-    if kelner.x == kelner.cel_x and kelner.y == kelner.cel_y:
+    if kelner.stanPrzestrzeni[:2] == cel2:
-        if kelner.stan == "odbiera":
+        if kelner.stan == "odbiera" and kelner.x == kelner.stolik_docelowy.x and kelner.y == kelner.stolik_docelowy.y - 1:
             kelner.stolik_docelowy.zamowione = False
             kelner.idz_do_kuchni()
-
+            cel = (kelner.cel_x, kelner.cel_y)
-        elif kelner.stan == "wraca":
+            print("Szukam ścieżki do kuchni...")
+            ruchy = bfs(tuple(kelner.stanPrzestrzeni), cel, stoliki_pozycje)
+            if ruchy:
+                print("Znaleziono ścieżkę ruchów: ", ruchy)
+            else:
+                print("Nie znaleziono ścieżki do celu.")
+        elif kelner.stan == "wraca" and kelner.x == kuchnia_xy and kelner.y == kuchnia_xy:
             kelner.stan = "stoi"
 
+
     time.sleep(0.001)
 
     key = pygame.key.get_pressed()
@@ -212,5 +282,4 @@ while run:
     for event in pygame.event.get():
         if event.type == pygame.QUIT:
             run = False
-
 pygame.quit()