InteligentnySaper/classes/bfs.py

import heapq  # dla utrzymania fringe
from classes import node, minesweeper, system
import time


class BFS:
    window: system.Window
    agent: minesweeper.Minesweeper
    node: node.Node

    def __init__(self, agent, window):
        self.agent = agent
        self.window = window

    def successor(self, current_position):
        new_nodes = []
        neighbours_list = self.agent.sensor(current_position[0], current_position[1])
        #print(f'Current position {current_position}')
        #print(neighbours_list[0])
        #print(neighbours_list[1])
        #print(neighbours_list[2])
        #print(neighbours_list)


        # if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #     new_nodes.append([current_position[0] - 1, current_position[1]])
        # if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #     new_nodes.append([current_position[0], current_position[1] + 1])
        # if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #     new_nodes.append([current_position[0] + 1, current_position[1]])
        # if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #     new_nodes.append([current_position[0],current_position[1] - 1])

        # if current_position[2] == 0:  # jesli patrzy na polnoc
        #     if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('forward', [current_position[0] - 1, current_position[1], 0])
        #         new_nodes.append(tmp)
        #     if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('left', [current_position[0],current_position[1], 270])
        #         new_nodes.append(tmp)
        #     if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('right', [current_position[0], current_position[1], 90])
        #         new_nodes.append(tmp)
        #
        # if current_position[2] == 90:  # jesli patrzy na wschod
        #     if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('forward', [current_position[0], current_position[1] + 1, 90])
        #         new_nodes.append(tmp)
        #     if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('left', [current_position[0], current_position[1], 0])
        #         new_nodes.append(tmp)
        #     if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('right', [current_position[0], current_position[1], 180])
        #         new_nodes.append(tmp)
        #
        # if current_position[2] == 180: # jesli patczy na poludzie
        #     if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('forward', [current_position[0] + 1, current_position[1], 180])
        #         new_nodes.append(tmp)
        #     if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('left', [current_position[0], current_position[1], 90])
        #         new_nodes.append(tmp)
        #     if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('right', [current_position[0],current_position[1], 270])
        #         new_nodes.append(tmp)
        #
        # if current_position[2] == 270:  # jesli patczy na wschod
        #     if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('forward', [current_position[0],current_position[1] - 1, 270])
        #         new_nodes.append(tmp)
        #     if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('left', [current_position[0], current_position[1], 180])
        #         new_nodes.append(tmp)
        #     if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
        #         tmp = ('right', [current_position[0], current_position[1], 0])
        #         new_nodes.append(tmp)

        if current_position[2] == 180:  # jesli patrzy na polnoc
            if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[0][1] == 'grass':
                    cost = 2
                elif neighbours_list[0][1] == 'sand':
                    cost = 1
                elif neighbours_list[0][1] == 'mine':
                    cost = 0
                tmp = ('forward', [current_position[0], current_position[1] - 1, 180], cost)
                new_nodes.append(tmp)
            if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[1][0] == 'grass':
                    cost = 2
                elif neighbours_list[1][0] == 'sand':
                    cost = 1
                elif neighbours_list[1][0] == 'mine':
                    cost = 0
                tmp = ('left', [current_position[0],current_position[1], 270], cost)
                new_nodes.append(tmp)
            if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[1][2] == 'grass':
                    cost = 2
                elif neighbours_list[1][2] == 'sand':
                    cost = 1
                elif neighbours_list[1][2] == 'mine':
                    cost = 0
                tmp = ('right', [current_position[0], current_position[1], 90], cost)
                new_nodes.append(tmp)

        if current_position[2] == 90:  # jesli patrzy na wschod
            if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[1][2] == 'grass':
                    cost = 2
                elif neighbours_list[1][2] == 'sand':
                    cost = 1
                elif neighbours_list[1][2] == 'mine':
                    cost = 0
                tmp = ('forward', [current_position[0] + 1, current_position[1], 90], cost)
                new_nodes.append(tmp)
            if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[0][1] == 'grass':
                    cost = 2
                elif neighbours_list[0][1] == 'sand':
                    cost = 1
                elif neighbours_list[0][1] == 'mine':
                    cost = 0
                tmp = ('left', [current_position[0], current_position[1], 180], cost)
                new_nodes.append(tmp)
            if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[2][1] == 'grass':
                    cost = 2
                elif neighbours_list[2][1] == 'sand':
                    cost = 1
                elif neighbours_list[2][1] == 'mine':
                    cost = 0
                tmp = ('right', [current_position[0], current_position[1], 0], cost)
                new_nodes.append(tmp)

        if current_position[2] == 0: # jesli patczy na poludzie
            if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[2][1] == 'grass':
                    cost = 2
                elif neighbours_list[2][1] == 'sand':
                    cost = 1
                elif neighbours_list[2][1] == 'mine':
                    cost = 0
                tmp = ('forward', [current_position[0], current_position[1] + 1, 0], cost)
                new_nodes.append(tmp)
            if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[1][2] == 'grass':
                    cost = 2
                elif neighbours_list[1][2] == 'sand':
                    cost = 1
                elif neighbours_list[1][2] == 'mine':
                    cost = 0
                tmp = ('left', [current_position[0], current_position[1], 90], cost)
                new_nodes.append(tmp)
            if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[1][0] == 'grass':
                    cost = 2
                elif neighbours_list[1][0] == 'sand':
                    cost = 1
                elif neighbours_list[1][0] == 'mine':
                    cost = 0
                tmp = ('right', [current_position[0],current_position[1], 270], cost)
                new_nodes.append(tmp)

        if current_position[2] == 270:  # jesli patczy na wschod
            if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[1][0] == 'grass':
                    cost = 2
                elif neighbours_list[1][0] == 'sand':
                    cost = 1
                elif neighbours_list[1][0] == 'mine':
                    cost = 0
                tmp = ('forward', [current_position[0] - 1,current_position[1], 270], cost)
                new_nodes.append(tmp)
            if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[2][1] == 'grass':
                    cost = 2
                elif neighbours_list[2][1] == 'sand':
                    cost = 1
                elif neighbours_list[2][1] == 'mine':
                    cost = 0
                tmp = ('left', [current_position[0], current_position[1], 0], cost)
                new_nodes.append(tmp)
            if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
                if neighbours_list[0][1] == 'grass':
                    cost = 2
                elif neighbours_list[0][1] == 'sand':
                    cost = 1
                elif neighbours_list[0][1] == 'mine':
                    cost = 0
                tmp = ('right', [current_position[0], current_position[1], 180], cost)
                new_nodes.append(tmp)

        return new_nodes



    # fringe = struktura danych przeszowyjąca wierchowki do odwiedzenia
    # explored = lista odwiedzonych stanow
    # position_at_beginning = stan poczatkowy
    # succ = funkcja nastempnika
    # goaltest = test spewnienia celu

    def graphsearch(self, fringe, explored, succ, goaltest):

        # def manhattan(state, target_state):
        #     return abs(state.get_x() - target_state.get_x()) + abs(state.get_y() - target_state.get_y())
        self.window.pause(True)

        positiont_at_beginning = [self.agent.position_x, self.agent.position_y, self.agent.rotation_degrees] # x, y, gdzie_patczy
        print(f'Position {positiont_at_beginning}')
        final_action_list = []  # lista co ma robic zeby dojechac do miny
        root = node.Node(None, None, positiont_at_beginning, 0)  # parent, action, position
        counter = 0
        heapq.heappush(fringe, (counter, root))
        #visited_position = []


        while len(fringe) != 0:
            flag = True


            if len(fringe) == 0:
                return False

            tmp_node = heapq.heappop(fringe)  # node
            tmp_node_position = tmp_node[1].get_position() # x, y , gdzie patczy
            #print(f'Position before succ {tmp_node_position}')

            #visited_position.append(tmp_node_position)
            explored.append(tmp_node_position)

            if tmp_node_position[:2] == goaltest:
                 print('Find')

                 while tmp_node[1].get_parent() is not None:
                     final_action_list.append(tmp_node[1].get_action())
                     tmp_node = tmp_node[1].get_parent()
                 final_action_list.reverse()
                 print(final_action_list)
                 self.window.pause(False)
                 return final_action_list

            # nie wiem na razie po co to
            # chyba visited_position lepszy odpowiednik
            # explored.append(tmp_node)


            neighbours_list_of_our_node = self.successor(tmp_node_position)  # lista możliwych akcij
            print(neighbours_list_of_our_node)

            for node_ in neighbours_list_of_our_node:
                # jesli pozucja wezla nie jest w fringe i nie jest w visited
                if [node_[1][0], node_[1][1], node_[1][2]] not in explored and node_[1][0] >= 0 and node_[1][1] >=0:
                    counter += 1
                    x = node.Node(tmp_node, node_[0], node_[1], node_[2])  # action
                    heapq.heappush(fringe, (counter, x))
                    self.window.draw_search([self.agent.position_x, self.agent.position_y], [node_[1][0], node_[1][1]])