zamiana prawdopodobienstwa scian

.

board

@@ -1,78 +1,266 @@
+from asyncio.windows_events import NULL
+from collections import deque
+from lib2to3.pgen2.token import CIRCUMFLEX
+from queue import Queue
 import sys
+from unicodedata import name
 import pygame
+import random
+import time
+import subprocess
+from os import environ
+environ['PYGAME_HIDE_SUPPORT_PROMPT'] = '1'
 
 screen = []
 objectArray = []
+pathToPrint = []
 
-class Agent:
+
+class Object:
     def __init__(self, name, xPos, yPos):
         self.name = name
         self.xPos = xPos
         self.yPos = yPos
 
-def draw(square_num, objectArr):
-    #następne dwie linijki do odkomentowania, jak będzie wgrane zdjęcie do tła
-    #background = pygame.image.load("ścieżka do pliku").convert() #tu ścieżka do zdjęcia w tle
-    #screen.blit(background, (0, 0))
-    grid_color = (0, 0, 0) #kolor czarny
+class Field:
+    def __init__(self, xPos, yPos, isObject):
+        self.xPos = xPos
+        self.yPos = yPos
+        self.visited = False
+        self.parent = None
+        number = random.randint(0, 9)
+        if number > 4 and not isObject:
+            self.isBlock = True
+        else:
+            self.isBlock = False
 
-    grid_size = 500  #rozmiar kraty
-    square = grid_size/square_num #rozmiar pojedyńczego kwadracika
+    def printXandY(self):
+        print("X: " + str(self.xPos) + ". Y: " + str(self.yPos))
 
+
+def draw(square_num, objectArr, pathToTarget, num):
+    grid_color = (0, 0, 0)
+    grid_size = 500 
+    square = grid_size/square_num
     a = 50
-    b = 10  #odległości kraty od krawędzi okna
+    b = 10 
 
     for i in range(square_num):
-        pygame.draw.line(screen, grid_color, (a + i*square, b), (a + i*square, b + grid_size), 2)
+        pygame.draw.line(screen, grid_color, (a + i*square, b),
+                         (a + i*square, b + grid_size), 2)
 
-        pygame.draw.line(screen, grid_color, (a, b + i*square), (a + grid_size, b + i*square), 2)
+        pygame.draw.line(screen, grid_color, (a, b + i*square),
+                         (a + grid_size, b + i*square), 2)
 
     pygame.draw.line(screen, grid_color, (a, b + grid_size), (a + grid_size,
                      b + grid_size), 2)
     pygame.draw.line(screen, grid_color, (a + grid_size, b),
                      (a + grid_size, b + grid_size), 2)
 
-    ## tutaj rysujemy agenta i inne obiekty juz na gotowej mapie
+    circleX = objectArr[0].xPos * square + square + square / 2
+    circleY = objectArr[0].yPos * square + square / 2
+
+    pygame.draw.circle(screen, (0, 0, 255), (circleX + 15, circleY + 15), 8)
+
+    circleX = objectArray[1].xPos * square + square * 2
+    circleY = objectArr[1].yPos * square + square
+    radius = 8
+    grid_color = (255, 0, 0)
+    pygame.draw.circle(screen, grid_color, (circleX, circleY), radius)
 
-    ## RYSUJEMY AGENTA
-    #agent_color = (255, 0, 0)
-    circleX = objectArr[0].xPos * square + square + square/ 2 #dodane jedno +square, by śmieciara nie wychodziła poza kratę
-    circleY = objectArr[0].yPos * square - square / 2
-    #radius = 10
-    #pygame.draw.circle(screen, agent_color, (a + circleX, b + circleY), radius)
-    truck = pygame.image.load("car.png").convert_alpha() #tu ścieżka do zdjęcia w tle
-    truck = pygame.transform.scale(truck, (square, square))
-    screen.blit(truck, (circleX, circleY))
 
 def kb_listen(objectArray, gridLength):
     for event in pygame.event.get():
         if event.type == pygame.KEYDOWN:
             if event.key == pygame.K_LEFT and objectArray[0].xPos > 0:
                 objectArray[0].xPos = objectArray[0].xPos - 1
+                print(objectArray[0].xPos)
             if event.key == pygame.K_RIGHT and objectArray[0].xPos < gridLength - 1:
                 objectArray[0].xPos = objectArray[0].xPos + 1
-            if event.key == pygame.K_UP and objectArray[0].yPos > 1:
+                print(objectArray[0].xPos)
+            if event.key == pygame.K_UP and objectArray[0].yPos > 0:
                 objectArray[0].yPos = objectArray[0].yPos - 1
-            if event.key == pygame.K_DOWN and objectArray[0].yPos < gridLength:
+                print(objectArray[0].yPos)
+            if event.key == pygame.K_DOWN and objectArray[0].yPos < gridLength - 1:
                 objectArray[0].yPos = objectArray[0].yPos + 1
+                print(objectArray[0].yPos)
         if event.type == pygame.QUIT:
             sys.exit()
 
 
-if __name__ == '__main__':
-    pygame.init()  #inicjalizacja modułów, na razie niepotrzebna
+fieldQueue = []
+isGoalAchieved = False
+
+def startQueue(agentX, agentY, fieldList, gridNum):
+    currentX = agentX
+    currentY = agentY
+    currentIndex = currentY * gridNum + currentX
+
+    fieldQueue.append(fieldList[currentIndex])
+
+    checkFlag = False
+
+    while not checkFlag:
+        result = checkGoal(fieldList, gridNum)
+        if result:
+            checkFlag = True
+    return result
+
+
+def checkGoal(fieldList, gridNum):
+    if len(fieldQueue) == 0:
+        print("THE PROGRAM FAILED TO FIND THE PATH FOR A GIVEN WALL/AGENT/TARGET TEMPLATE...")
+        print("========= PLEASE DO NOT TURN THE PROGRAM AGAIN ===========")
+        print("========= THE PROGRAM WILL CREATE A NEW TEMPLATE AUTOMATICALLY ==========")
+        subprocess.Popen(['python', 'board'])
+        sys.exit()
+    currentField = fieldQueue.pop(0)
+
+    currentY = currentField.yPos
+    currentX = currentField.xPos
+    currentIndex = currentY * gridNum + currentX
+
+    # SPRAWDZAMY CZY DANE POLE JEST POLEM W KTORYM ZNAJDUJE SIE NASZ CEL
+    if currentField.xPos == objectArray[1].xPos and currentField.yPos == objectArray[1].yPos:
+        return currentField
+
+    # SPRAWDZAMY CZY DANE POLE ZOSTALO JUZ WCZESNIEJ ODWIEDZONE - JEZELI NIE TO USTAWIAMY STATUS
+    if not currentField.visited:
+        currentField.visited = True
+
+        # JEZELI DANE POLE NIE ZOSTALO ODWIEDZONE, TO DODAJEMY DO KOLEJKI WSZYSTKIE POLA OBOK I LECIMY DALEJ
+        if currentIndex + 15 <= 224 and fieldList[currentIndex + 15].visited == False and fieldList[currentIndex + 15].isBlock == False:
+            fieldQueue.append(fieldList[currentIndex + 15])
+            fieldList[currentIndex + 15].parent = fieldList[currentIndex]
+        if currentIndex - 15 > -1 and fieldList[currentIndex - 15].visited == False and fieldList[currentIndex - 15].isBlock == False:
+            fieldQueue.append(fieldList[currentIndex - 15])
+            fieldList[currentIndex - 15].parent = fieldList[currentIndex]
+        if (currentIndex + 1) % 15 != 0 and fieldList[currentIndex + 1].visited == False and fieldList[currentIndex + 1].isBlock == False:
+            fieldQueue.append(fieldList[currentIndex + 1])
+            fieldList[currentIndex + 1].parent = fieldList[currentIndex]
+        if (currentIndex - 1) % 15 != 14 and not currentIndex - 1 < 0 and fieldList[currentIndex - 1].visited == False and fieldList[currentIndex - 1].isBlock == False:
+            fieldQueue.append(fieldList[currentIndex - 1])
+            fieldList[currentIndex - 1].parent = fieldList[currentIndex]
+
+def generateStartEndPos():
+    startX = random.randint(0, 14)
+    startY = random.randint(0, 14)
+    targetX = None
+    targetY = None
+
+    while targetX == None and targetY == None or targetX == startX and targetY == startY:
+        targetX = random.randint(0, 14)
+        targetY = random.randint(0, 14)
+
+    return [startX, startY, targetX, targetY]
+
+
+
+def drawFull(index):
+    c = (255, 255, 255)
+    screen.fill(c)
+
+    for node in pathToTarget:
+        if node == pathToTarget[0] or node == pathToTarget[-1]:
+            continue
+        if pathToTarget.index(node) <= index:
+            continue
+        green = (0, 255, 0)
+        pygame.draw.circle(screen, green, ((node.xPos + 1)
+                               * 500 / 15 + 30, (node.yPos + 1) * 500 / 15), 5)
+    draw(15, objectArray, pathToTarget, 1)
+
+    for node in fields:
+        if node.isBlock == True:
+            black = (0, 0, 0)
+            pygame.draw.rect(screen, black, pygame.Rect((node.xPos + 1) * 500 / 15 + 30, (node.yPos + 1) * 500 / 15 - 10, 10, 10))
+
+    ##kb_listen(objectArray, 15)
+    pygame.display.update() 
+
+if __name__ == '__main__':
+    [sX, sY, eX, eY] = generateStartEndPos()
+
+    fields = []
+    for y in range(15):
+        for x in range(15):
+            if x == sX and y == sY or x == eX and y == eY:
+                newField = Field(x, y, True)
+            else:
+                newField = Field(x, y, False)
+            fields.append(newField)
+
+    agent = Object("AGENT", sX, sY)
+    target = Object("TARGET", eX, eY)
 
-    #Tworzymy nowego playera, czy tam agenta
-    agent = Agent("smieciarka", 5, 7)
     objectArray.append(agent)
+    objectArray.append(target)
+
+    result = startQueue(objectArray[0].xPos,
+                        objectArray[0].yPos, fields, 15)
+
+    pygame.init()
 
     width = 600
     height = 530
-    screen = pygame.display.set_mode((width, height))  #ustalanie rozmiarów okna
+    screen = pygame.display.set_mode(
+        (width, height))
+
+    print("RED DOT X AND Y POSITION ============================")
+    print("X: " + str(objectArray[1].xPos) + " Y: " + str(objectArray[1].yPos))
+    print("TRUCK X AND Y POSITION ============================")
+    print("X: " + str(objectArray[0].xPos) + " Y: " + str(objectArray[0].yPos))
+
+    result_parent = result.parent
+
+    currentIndex = (objectArray[0].yPos) * 15 + objectArray[0].xPos
+    pathToTarget = deque()
+    pathToTarget.appendleft(result)
+
+    while result_parent.parent != None:
+        pathToTarget.appendleft(result_parent)
+        result_parent = result_parent.parent
+    pathToTarget.appendleft(fields[currentIndex])
+
+    print("PRINTING PATH TO TARGET ============================")
+    for node in pathToTarget:
+        node.printXandY()
+
+    print("============================ SEARCH PROBLEM DIAGRAM ============================")
+    for node in pathToTarget:
+        currentNode = node
+        parentNode = node.parent
+        direction = None
+
+        index = pathToTarget.index(node)
+
+        if parentNode == None:
+            continue
+
+        if currentNode.xPos != parentNode.xPos:
+            if currentNode.xPos > parentNode.xPos:
+                direction = "EAST"
+                objectArray[0].xPos = objectArray[0].xPos + 1
+            else:
+                direction = "WEST"
+                objectArray[0].xPos = objectArray[0].xPos - 1
+        else:
+            if currentNode.yPos > parentNode.yPos:
+                direction = "SOUTH"
+                objectArray[0].yPos = objectArray[0].yPos + 1
+            else:
+                direction = "NORTH"
+                objectArray[0].yPos = objectArray[0].yPos - 1
+        time.sleep(1)
+        print("---- NEXT STEP ---- ")
+        print("AGENT TURNING TO " + direction)
+        print("AGENT MOVING FORWARD")
+        print("ACTUAL AGENT STATE: (X -> " + str(objectArray[0].xPos) + ", Y -> " + str(objectArray[0].yPos) + ", " + direction + ")")
+
+        draw(15, objectArray, pathToTarget, 1)
+        pygame.display.update()
+        drawFull(index)
+    
+    time.sleep(5)
+    quit()
 
-    while 1:
-        c = (255, 255, 255)  #tymczasowy kolor tła - do usunięcia, jak już będzie zdjęcie
-        screen.fill(c)
-        draw(15, objectArray)
-        kb_listen(objectArray, 15)
-        pygame.display.update()  #by krata pojawiła się w okienku - update powierzchni