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

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

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.f = 0
        self.g = 0
        self.h = 0
        self.cost = 1
        self.visited = False
        self.closed = False
        self.parent = None

    def __lt__(self, other):
        return self.f < other.f

    def neighbors(self, grid):
        ret = []
        x, y = self.x, self.y
        if x > 0 and grid[x - 1][y]:
            ret.append(grid[x - 1][y])
        if x < len(grid) - 1 and grid[x + 1][y]:
            ret.append(grid[x + 1][y])
        if y > 0 and grid[x][y - 1]:
            ret.append(grid[x][y - 1])
        if y < len(grid[0]) - 1 and grid[x][y + 1]:
            ret.append(grid[x][y + 1])
        return ret

def init(grid):
    for x in range(len(grid)):
        for y in range(len(grid[x])):
            node = grid[x][y]
            node.f = 0
            node.g = 0
            node.h = 0
            node.cost = 1
            node.visited = False
            node.closed = False
            node.parent = None

def heap():
    return []

def search(grid, start, end, board, heuristic=None):
    init(grid)
    if heuristic is None:
        heuristic = manhattan
    open_heap = heap()
    heapq.heappush(open_heap, start)

    while open_heap:
        current_node = heapq.heappop(open_heap)
        if (current_node.x, current_node.y) == (end.x, end.y):

            ret = []
            while current_node.parent:
                ret.append(current_node)
                current_node = current_node.parent
            ret.append(start)    
            ret_path = ret[::-1]
            for node in ret_path:
                print(f"({node.x}, {node.y}): {node.g}")
            print("Znaleziono ścieżkę [(x,y)jako(kolumna,wiersz)] o koszcie:", ret_path[-1].g)
            return ret_path, start 
        current_node.closed = True
        for neighbor in current_node.neighbors(grid):
            if neighbor.closed:  
                continue
            g_score = current_node.g + board.get_cost(neighbor.x, neighbor.y)  
            been_visited = neighbor.visited
            if not been_visited or g_score < neighbor.g:
                neighbor.visited = True
                neighbor.parent = current_node
                neighbor.h = neighbor.h or heuristic((neighbor.x, neighbor.y), (end.x, end.y))
                neighbor.g = g_score
                neighbor.f = neighbor.g + neighbor.h
                if not been_visited:
                    heapq.heappush(open_heap, neighbor)
    
    print("Nie znaleziono ścieżki.")
    return None


def manhattan(pos0, pos1):
    d1 = abs(pos1[0] - pos0[0])
    d2 = abs(pos1[1] - pos0[1])
    return d1 + d2


def main():
    rotation  = ["left", "up", "right", "down"]  
    run = True
    clock = pygame.time.Clock()
    board = Board()
    board.load_images()
    
    start_row, start_col = 1, 1
    end_row, end_col = 5, 0
    tractor = Tractor(start_row, start_col)  
    board.set_grass(start_row, start_col)
    board.set_grass(end_row, end_col)
   
    grid = [[Node(x, y) for y in range(rows)] for x in range(cols)]
    
    
    start = grid[start_row][start_col]
    end = grid[end_row][end_col]
    
    path, start_node = search(grid, start, end, 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_node = path.pop(0) if path else start_node
        tractor.row, tractor.col = next_node.y, next_node.x
        
        
        #rotation
        



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

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

main()