SI-projekt-smieciarka/board

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

screen = []
objectArray = []
pathToPrint = []


class Object:
    def __init__(self, name, xPos, yPos):
        self.name = name
        self.xPos = xPos
        self.yPos = yPos

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 > 5 and not isObject:
            self.isBlock = True
        else:
            self.isBlock = False

    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 

    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, 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)

    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)


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
                print(objectArray[0].xPos)
            if event.key == pygame.K_UP and objectArray[0].yPos > 0:
                objectArray[0].yPos = objectArray[0].yPos - 1
                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()


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):
    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__':
    pygame.init()

    width = 600
    height = 530
    screen = pygame.display.set_mode(
        (width, height))

    [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)

    objectArray.append(agent)
    objectArray.append(target)

    result = startQueue(objectArray[0].xPos,
                        objectArray[0].yPos, fields, 15)


    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()