AI_PROJECT/AStar.py

"""
f(n) = g(n) + h(n)
g(n) = dotychczasowy koszt -> dodać currentCost w Node lub brać koszt na nowo przy oddtawrzaniu ścieżki
h(n) = abs(state['x'] - goalTreassure[0]) + abs(state['y'] - goalTreassure[1]) -> odległość Manhatan -> można zrobić jeszcze drugą wersje gdzie mnoży się razy 5.5 ze wzgledu na średni koszt przejścia
Należy zaimplementować kolejkę priorytetową oraz zaimplementować algorytm przeszukiwania grafu stanów z uwzględnieniem kosztu za pomocą przerobienia algorytmu przeszukiwania grafu stanów
"""
import random
import pygame
import Node
import BFS
from displayControler import NUM_X, NUM_Y
from Pole import stoneList
from queue import PriorityQueue


def getRandomGoalTreasure():
    while True:
        goalTreasure = (random.randint(0, NUM_X - 1), random.randint(0, NUM_Y - 1)) # Współrzędne celu
        if goalTreasure not in stoneList:
            break
    return goalTreasure


def heuristic(state, goal):
    # Oblicz odległość Manhattanowską między aktualnym stanem a celem
    manhattan_distance = abs(state['x'] - goal[0]) + abs(state['y'] - goal[1])
    return manhattan_distance


'''def get_cost_for_plant(plant_name):
    plant_costs = {
        "pszenica": 7,
        "kukurydza": 9,
        "ziemniak": 2,
        "slonecznik": 5,
        "borowka": 3,
        "winogrono": 4,
        "mud": 15,
        "dirt": 0,
    }
    if plant_name in plant_costs:
        return plant_costs[plant_name]
    else:
        # Jeśli nazwa rośliny nie istnieje w słowniku, zwróć domyślną wartość
        return 0
'''

def A_star(istate, pole, goalTreasure):
    # goalTreasure = (random.randint(0,NUM_X-1), random.randint(0,NUM_Y-1))
    # #jeśli chcemy używać random musimy wykreslić sloty z kamieniami, ponieważ tez mogą się wylosować i wtedy traktor w ogóle nie rusza
    #lub zrobić to jakoś inaczej, np. funkcja szukająca najmniej nawodnionej rośliny
    
    # przeniesione wyżej do funkcji getRandomGoalTreasure, wykorzystywana jest w App.py
    # while True:
    #     goalTreasure = (random.randint(0, NUM_X - 1), random.randint(0, NUM_Y - 1)) # Współrzędne celu
    #     if goalTreasure not in stoneList:
    #         break

    fringe = PriorityQueue()  # Kolejka priorytetowa dla wierzchołków do rozpatrzenia
    explored = []  # Lista odwiedzonych stanów
    obrot = 1

    # Tworzenie węzła początkowego
    x = Node.Node(istate)
    x.g = 0
    x.h = heuristic(x.state, goalTreasure)
    fringe.put((x.g + x.h, x))  # Dodanie węzła do kolejki
    total_cost = 0

    while not fringe.empty():
        _, elem = fringe.get()  # Pobranie węzła z najniższym priorytetem

        if BFS.goalTest3(elem.state, goalTreasure):  # Sprawdzenie, czy osiągnięto cel
            path = []
            cost_list=[]
            while elem.parent is not None:  # Odtworzenie ścieżki
                path.append([elem.parent, elem.action])
                elem = elem.parent
            for node, action in path:
                # Obliczanie kosztu ścieżki dla każdego pola i wyświetlanie
                plant_cost = get_plant_name_and_cost_from_coordinates(node.state['x'],node.state['y'], pole)
                if action == "left" or action == "right":  # Liczenie kosztu tylko dla pól nie będących obrotami
                    total_cost += obrot
                    cost_list.append(obrot)
                else:
                    total_cost += plant_cost
                    cost_list.append(plant_cost)
            return path,cost_list,total_cost

        explored.append(elem.state)

        for resp in succ3A(elem.state):
            child_state = resp[1]
            if child_state not in explored:
                child = Node.Node(child_state)
                child.parent = elem
                child.action = resp[0]

                # Pobranie nazwy rośliny z danego slotu na podstawie współrzędnych
                plant_cost = get_plant_name_and_cost_from_coordinates(child_state['x'], child_state['y'], pole)
                # Pobranie kosztu dla danej rośliny
                #plant_cost = get_cost_for_plant(plant_name)

                if child.action == "left" or child.action == "right":
                    child.g = elem.g + obrot
                else:
                    child.g = elem.g + plant_cost

                # Obliczenie heurystyki dla dziecka
                child.h = heuristic(child.state, goalTreasure)

                in_fringe = False
                for priority, item in fringe.queue:
                    if item.state == child.state:
                        in_fringe = True
                        if priority > child.g + child.h:
                            # Jeśli znaleziono węzeł w kolejce o gorszym priorytecie, zastąp go nowym
                            fringe.queue.remove((priority, item))
                            fringe.put((child.g + child.h, child))
                            break

                if not in_fringe:
                    # Jeśli stan dziecka nie jest w kolejce, dodaj go do kolejki
                    fringe.put((child.g + child.h, child))

        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                quit()

    return False


def get_plant_name_and_cost_from_coordinates(x, y, pole):
    if (x, y) in pole.slot_dict:  # Sprawdzenie, czy podane współrzędne znajdują się na polu
        slot = pole.slot_dict[(x, y)]  # Pobranie slotu na podstawie współrzędnych
        if slot.plant:  # Sprawdzenie, czy na slocie znajduje się roślina
            return slot.plant.stan.koszt  # Zwrócenie nazwy rośliny na slocie
        else:
            return 0  # jeśli na slocie nie ma rośliny
    else:
        return 0  #  jeśli podane współrzędne są poza polem


#to ogólnie identyczna funkcja jak w BFS ale nie chciałam tam ruszać, żeby przypadkiem nie zapsuć do BFS,
#tylko musiałam dodac sprawdzenie kolizji, bo traktor brał sloty z Y których nie ma na planszy
def succ3A(state):
    resp = []
    if state["direction"] == "N":
        if state["y"] > 0 and (state['x'], state["y"] - 1) not in stoneList:
            resp.append(["forward", {'x': state["x"], 'y': state["y"]-1, 'direction': state["direction"]}])
        resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "E"}])
        resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "W"}])
    elif state["direction"] == "S":
        if state["y"] < NUM_Y - 1 and (state['x'], state["y"] + 1) not in stoneList:
            resp.append(["forward", {'x': state["x"], 'y': state["y"]+1, 'direction': state["direction"]}])
        resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "W"}])
        resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "E"}])
    elif state["direction"] == "E":
        if state["x"] < NUM_X - 1 and (state['x'] + 1, state["y"]) not in stoneList:
            resp.append(["forward", {'x': state["x"]+1, 'y': state["y"], 'direction': state["direction"]}])
        resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "S"}])
        resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "N"}])
    else: #state["direction"] == "W"
        if state["x"] > 0 and (state['x'] - 1, state["y"]) not in stoneList:
            resp.append(["forward", {'x': state["x"]-1, 'y': state["y"], 'direction': state["direction"]}])
        resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "N"}])
        resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "S"}])

    return resp




def heuristic2(state, goal):
    # Oblicz odległość Manhattanowską między aktualnym stanem a celem
    manhattan_distance = (abs(state['x'] - goal[0]) + abs(state['y'] - goal[1])) * 2.5
    return manhattan_distance


def A_star2(istate, pole, goalTreasure):
    # goalTreasure = (random.randint(0,NUM_X-1), random.randint(0,NUM_Y-1))
    # #jeśli chcemy używać random musimy wykreslić sloty z kamieniami, ponieważ tez mogą się wylosować i wtedy traktor w ogóle nie rusza
    #lub zrobić to jakoś inaczej, np. funkcja szukająca najmniej nawodnionej rośliny
    
    # przeniesione wyżej do funkcji getRandomGoalTreasure, wykorzystywana jest w App.py
    # while True:
    #     goalTreasure = (random.randint(0, NUM_X - 1), random.randint(0, NUM_Y - 1)) # Współrzędne celu
    #     if goalTreasure not in stoneList:
    #         break
    fringe = PriorityQueue()  # Kolejka priorytetowa dla wierzchołków do rozpatrzenia
    explored = []  # Lista odwiedzonych stanów
    obrot = 1

    # Tworzenie węzła początkowego
    x = Node.Node(istate)
    x.g = 0
    x.h = heuristic2(x.state, goalTreasure)
    fringe.put((x.g + x.h, x))  # Dodanie węzła do kolejki
    total_cost=0

    while not fringe.empty():
        _, elem = fringe.get()  # Pobranie węzła z najniższym priorytetem

        if BFS.goalTest3(elem.state, goalTreasure):  # Sprawdzenie, czy osiągnięto cel
            path = []
            cost_list=[]
            while elem.parent is not None:  # Odtworzenie ścieżki
                path.append([elem.parent, elem.action])
                elem = elem.parent
            for node, action in path:
                # Obliczanie kosztu ścieżki dla każdego pola i wyświetlanie
                plant_cost = get_plant_name_and_cost_from_coordinates(node.state['x'],node.state['y'], pole)
                if action == "left" or action == "right":  # Liczenie kosztu tylko dla pól nie będących obrotami
                    total_cost += obrot
                    cost_list.append(obrot)
                else:
                    total_cost += plant_cost
                    cost_list.append(plant_cost)
            return path,cost_list,total_cost

        explored.append(elem.state)

        for resp in succ3A(elem.state):
            child_state = resp[1]
            if child_state not in explored:
                child = Node.Node(child_state)
                child.parent = elem
                child.action = resp[0]

                # Pobranie nazwy rośliny z danego slotu na podstawie współrzędnych
                plant_cost = get_plant_name_and_cost_from_coordinates(child_state['x'], child_state['y'], pole)

                if child.action == "left" or child.action == "right":
                    child.g = elem.g + obrot
                else:
                    child.g = elem.g + plant_cost
                # Obliczenie heurystyki dla dziecka
                child.h = heuristic2(child.state, goalTreasure)

                in_fringe = False
                for priority, item in fringe.queue:
                    if item.state == child.state:
                        in_fringe = True
                        if priority > child.g + child.h:
                            # Jeśli znaleziono węzeł w kolejce o gorszym priorytecie, zastąp go nowym
                            fringe.queue.remove((priority, item))
                            fringe.put((child.g + child.h, child))
                            break

                if not in_fringe:
                    # Jeśli stan dziecka nie jest w kolejce, dodaj go do kolejki
                    fringe.put((child.g + child.h, child))

        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                quit()

    return False

"""
TO TEST SPEED OF ASTAR

test_speed = False

            if test_speed:
                time1 = 0
                time2 = 0
                cost1 = 0
                cost2 = 0
                for i in range(500):
                    print(i)
                    start = time.time()
                    aStarRoot, cost_list, total_cost = AStar.A_star({'x': 0, 'y': 0, 'direction': "E"}, pole, goalTreasure)
                    end = time.time()
                    time1 += end - start
                    cost1 += total_cost
                    start = time.time()
                    aStarRoot2, cost_list, total_cost = AStar.A_star2({'x': 0, 'y': 0, 'direction': "E"}, pole, goalTreasure)
                    end = time.time()
                    time2 += end - start
                    cost2 += total_cost
                print(time1, time2)
                print(float(cost1 / 1000), float(cost2 / 1000))
"""