Traktor/astar.py

import pygame
from board import Board
from constant import width, height, rows, cols
from tractor import Tractor
import heapq

fps = 2
WIN = pygame.display.set_mode((width, height))
pygame.display.set_caption('Inteligenty Traktor')


class Node:
    def __init__(self, state, parent=None, action=None, cost=0):
        self.state = state  # Stan reprezentowany przez węzeł
        self.parent = parent  # Węzeł rodzica
        self.action = action  # Akcja prowadząca do tego stanu
        self.cost = cost  # Koszt przejścia do tego stanu
        self.f = 0  # Wartość funkcji priorytetowej
        self.tie_breaker = 0  # Wartość używana do rozwiązywania konfliktów priorytetów

    def __lt__(self, other):
        # Porównanie węzłów w celu ustalenia kolejności w kolejce priorytetowej
        if self.f == other.f:
            return self.tie_breaker > other.tie_breaker  # Większy tie_breaker ma wyższy priorytet
        return self.f < other.f
        # Jesli pare wezlow ma taie same f, to tilebreaker ustawia
        # prorytety akcje right i down maja wyzszy priorytet


def manhattan(pos0, pos1):
    # Heurystyka odległości Manhattan
    d1 = abs(pos1[0] - pos0[0])
    d2 = abs(pos1[1] - pos0[1])
    return d1 + d2


def nastepnik(state, board):
    # Funkcja generująca możliwe następne stany (akcje)
    x, y = state
    successors = []
    actions = [('right', (x+1, y), 1), ('down', (x, y+1), 1), ('up', (x, y-1), 0), ('left', (x-1, y), 0)]
    for action, next_state, tie_breaker in actions:
        if 0 <= next_state[0] < cols and 0 <= next_state[1] < rows:
            cost = board.get_cost(next_state[0], next_state[1])
            successors.append((action, next_state, cost, tie_breaker))
    return successors


def goal_test(state, goal):
    #  Czy dany stan jest stanem docelowym
    return state == goal


def graphsearch(istate, goal, board, heuristic=manhattan):
    # Algorytm przeszukiwania grafu
    fringe = []  # Kolejka priorytetowa przechowująca węzły do odwiedzenia
    explored = set()  # Zbiór odwiedzonych stanów
    start_node = Node(istate)
    start_node.f = heuristic(istate, goal)  # Obliczenie wartości heurystycznej dla stanu początkowego
    start_node.tie_breaker = 0  # Ustawienie tie_breaker dla węzła startowego,
    heapq.heappush(fringe, start_node)

    while fringe:
        elem = heapq.heappop(fringe)
        if goal_test(elem.state, goal):
            path = []
            total_cost = elem.cost  # Zapisanie całkowitego kosztu
            while elem:
                path.append((elem.state, elem.action))
                elem = elem.parent
            return path[::-1], total_cost  # Zwrócenie ścieżki i kosztu

        explored.add(elem.state)
        for action_index, (action, state, cost, tie_breaker) in enumerate(nastepnik(elem.state, board)):
            x = Node(state, parent=elem, action=action, cost=elem.cost + cost)
            x.f = x.cost + heuristic(state, goal)  # Obliczenie wartości funkcji priorytetowej
            x.tie_breaker = elem.tie_breaker * 4 + action_index  # Obliczanie tie_breaker na podstawie akcji

            if state not in explored and not any(node.state == state for node in fringe):
                heapq.heappush(fringe, x)
            else:
                for i, node in enumerate(fringe):
                    if node.state == state and (node.f > x.f or (node.f == x.f and node.tie_breaker < x.tie_breaker)):
                        fringe[i] = x
                        heapq.heapify(fringe)
                        break

    print("Nie znaleziono ścieżki.")
    return None, 0  # Zwrócenie ścieżki jako None i kosztu jako 0 w przypadku braku ścieżki

def main():
    run = True
    clock = pygame.time.Clock()
    board = Board()
    board.load_images()

    start_state = (9, 9)  # Stan początkowy
    goal_state = (0, 0)  # Stan docelowy
    tractor = Tractor(start_state[1], start_state[0])
    board.set_grass(start_state[0], start_state[1])  # Ustawienie startowego pola jako trawę
    board.set_grass(goal_state[0], goal_state[1])  # Ustawienie docelowego pola jako trawę

    path, total_cost = graphsearch(start_state, goal_state, board)

    while run:
        clock.tick(fps)

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

        if not path:
            run = False
            continue

        next_state, action = path.pop(0) if path else (start_state, None)
        print(next_state)  # Wypisanie następnego stanu
        tractor.row, tractor.col = next_state[1], next_state[0]

        if action == "right":
            tractor.direction = "right"
        elif action == "left":
            tractor.direction = "left"
        elif action == "down":
            tractor.direction = "down"
        elif action == "up":
            tractor.direction = "up"

        # Aktualizacja planszy na podstawie położenia traktora
        if board.is_weed(tractor.col, tractor.row):
            board.set_grass(tractor.col, tractor.row)
        elif board.is_dirt(tractor.col, tractor.row):
            board.set_soil(tractor.col, tractor.row)
        elif board.is_soil(tractor.col, tractor.row):
            board.set_carrot(tractor.col, tractor.row)

        board.draw_cubes(WIN)
        tractor.draw(WIN)
        pygame.display.update()

    print(f"Całkowity koszt trasy: {total_cost}")

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


main()