From 9406038dcbcd23a0e418c8a096002fb5753dae4b Mon Sep 17 00:00:00 2001
From: Mateusz <mtszlachetka@gmail.com>
Date: Wed, 19 Apr 2023 23:51:24 +0200
Subject: [PATCH] implemented movement planning

---
 bfs.py  | 14 ++++++++++----
 main.py | 55 +++++++++++++++++++++++++++++++++++++++++--------------
 succ.py |  1 -
 3 files changed, 51 insertions(+), 19 deletions(-)

diff --git a/bfs.py b/bfs.py
index b0d3da5..0e29f49 100644
--- a/bfs.py
+++ b/bfs.py
@@ -1,15 +1,21 @@
 from succ import succ as successors
 
-def bfs(istate, goalstate):
+def bfs(istate, goalx, goaly):
     fringe = [istate]
     explored = []
+    steps = []
     while(fringe):
         state = fringe.pop(0)
-        if state == goalstate : 
-                return 
+        if state.xpos == goalx and state.ypos == goaly: 
+                steps.insert(0, state)
+                while(state.parent != None):
+                    state = state.parent
+                    steps.insert(0, state)
+                return steps
+        
         element = successors(state)
         explored.append(state)
         for value in element :
-            if value not in explored :
+            if value not in explored and value not in fringe:
                 fringe.append(value)
     return False
diff --git a/main.py b/main.py
index 2beb086..73ebbd7 100644
--- a/main.py
+++ b/main.py
@@ -1,5 +1,8 @@
 import pygame
 import random
+from bfs import bfs
+from state import State
+import time
 
 pygame.init()
 WIDTH, HEIGHT = 800, 800
@@ -17,6 +20,10 @@ COBBLE_IMG = pygame.image.load("cobble.jpeg")
 COBBLE = pygame.transform.scale(COBBLE_IMG, (50, 50))
 FPS = 10
 
+class Agent:
+    def __init__(self, rect, direction):
+        self.rect = rect
+        self.direction = direction
 
 def randomize_map():        # tworzenie mapy z losowymi polami
     fields_list = [DIRT, GRASS, SAND, COBBLE]
@@ -34,37 +41,57 @@ def draw_window(agent, fields):
     for i in range(16):
         for j in range(16):
             window.blit(fields[i][j], (i * 50, j * 50))
-    window.blit(AGENT, (agent.x, agent.y))      # wyswietlanie agenta
+    window.blit(AGENT, (agent.rect.x, agent.rect.y))      # wyswietlanie agenta
     pygame.display.update()
 
 
-def agent_movement(keys_pressed, agent):    # sterowanie
-    if keys_pressed[pygame.K_LEFT] and agent.x > 0:
-        agent.x -= 50
-    if keys_pressed[pygame.K_RIGHT] and agent.x < 750:
-        agent.x += 50
-    if keys_pressed[pygame.K_UP] and agent.y > 0:
-        agent.y -= 50
-    if keys_pressed[pygame.K_DOWN] and agent.y < 750:
-        agent.y += 50
+#def agent_movement(keys_pressed, agent):    # sterowanie
+#    if keys_pressed[pygame.K_LEFT] and agent.x > 0:
+#        agent.x -= 50
+#    if keys_pressed[pygame.K_RIGHT] and agent.x < 750:
+#        agent.x += 50
+#    if keys_pressed[pygame.K_UP] and agent.y > 0:
+#        agent.y -= 50
+#    if keys_pressed[pygame.K_DOWN] and agent.y < 750:
+#        agent.y += 50
 
 
 def main():
     clock = pygame.time.Clock()
     run = True
-    agent = pygame.Rect(0, 0, 50, 50)   # tworzenie pola dla agenta
+    agent = Agent(pygame.Rect(0, 0, 50, 50), 0)   # tworzenie pola dla agenta
     fields = randomize_map()
     while run:
         clock.tick(FPS)
         for event in pygame.event.get():    # przechwycanie zamknięcia okna
             if event.type == pygame.QUIT:
                 run = False
-        keys_pressed = pygame.key.get_pressed()
-        draw_window(agent, fields)
-        agent_movement(keys_pressed, agent)
+        #keys_pressed = pygame.key.get_pressed()
+        
+        steps = bfs(State(None, None, 0, 0, 'E'), 450, 100)
+        for interm in steps:
+            if interm.action == 'LEFT':
+                agent.direction = (agent.direction - 1) % 4
+            if interm.action == 'RIGHT':
+                agent.direction = (agent.direction + 1) % 4
+            if interm.action == 'FORWARD':
+                if agent.direction == 0:
+                    agent.rect.x += 50
+                elif agent.direction == 1:
+                    agent.rect.y += 50
+                elif agent.direction == 2:
+                    agent.rect.x -= 50
+                else:
+                    agent.rect.y -= 50
+            draw_window(agent, fields)
+            time.sleep(1)
+    
+        while True:
+            pass  
 
     pygame.quit()
 
 
+
 if __name__ == "__main__":
     main()
diff --git a/succ.py b/succ.py
index 70b5f0f..b7309dd 100644
--- a/succ.py
+++ b/succ.py
@@ -26,5 +26,4 @@ def succ(st: State):
         successors.append(State(st, 'RIGHT', st.xpos, st.ypos, 'S'))
         if st.xpos < 750:
             successors.append(State(st, 'FORWARD', st.xpos + 50, st.ypos, 'E'))
-
     return successors