import sys
from queue import PriorityQueue
import numpy as np

import pygame

screen = []
objectArray = []
collisionsMap = []


class Position:
    def __init__(self, x, y):
        self.x = x
        self.y = y

    def get_moved(self, delta_x, delta_y):
        return Position(self.x + delta_x, self.y + delta_y)


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

    def draw(self, square):
        leftTopX, leftTopY = 50 + self.pos.x * square, 10 + self.pos.y * square
        pygame.draw.rect(screen, (0, 0, 0), (leftTopX, leftTopY, square, square))


def detect_collision(newPos):
    return collisionsMap[newPos.x][newPos.y]


def position_in_grid(pos, gridLength):
    return 0 <= pos.x < gridLength and 0 <= pos.y < gridLength


def movement_allowed(newPos, gridLength):
    return position_in_grid(newPos, gridLength) and not detect_collision(newPos)


class Agent(Object):
    def __init__(self, name, pos):
        super().__init__(name, pos)

    def draw(self, square):
        ## RYSUJEMY AGENTA
        circleX = 52 + self.pos.x * square
        circleY = 12 + self.pos.y * square

        truck = pygame.image.load("car.png").convert_alpha()  # tu ścieżka do zdjęcia w tle
        truck = pygame.transform.scale(truck, (square, square))
        screen.blit(truck, (circleX, circleY))

    def move(self, event, gridLength):
        if event.key == pygame.K_LEFT:
            newPos = self.pos.get_moved(-1, 0)
            self.move_if_possible(newPos, gridLength)
        if event.key == pygame.K_RIGHT:
            newPos = self.pos.get_moved(1, 0)
            self.move_if_possible(newPos, gridLength)
        if event.key == pygame.K_UP:
            newPos = self.pos.get_moved(0, -1)
            self.move_if_possible(newPos, gridLength)
        if event.key == pygame.K_DOWN:
            newPos = self.pos.get_moved(0, 1)
            self.move_if_possible(newPos, gridLength)

    def move_if_possible(self, newPos, gridLength):
        if movement_allowed(newPos, gridLength):
            self.pos = newPos


class House(Object):
    def __init__(self, name, pos):
        super().__init__(name, pos)
        self.trash_cans = {
            "paper": False,
            "glass": False,
            "plastic": False,
            "bio": False
        }

    def draw(self, square):
        x = 52 + self.pos.x * square
        y = 12 + self.pos.y * square

        house = pygame.image.load("house.png").convert_alpha()  # tu ścieżka do zdjęcia w tle
        house = pygame.transform.scale(house, (square, square))
        screen.blit(house, (x, y))


class Junkyard(Object):
    def __init__(self, name, pos):
        super().__init__(name, pos)
        self.heaps = {
            "paper": True,
            "glass": True,
            "plastic": True,
            "bio": True
        }

    def draw(self, square):
        x = 52 + self.pos.x * square
        y = 12 + self.pos.y * square

        junkyard = pygame.image.load("junkyard.png").convert_alpha()  # tu ścieżka do zdjęcia w tle
        junkyard = pygame.transform.scale(junkyard, (square, square))
        screen.blit(junkyard, (x, y))


class Hole(Object):
    def __init__(self, name, pos):
        super().__init__(name, pos)

    def draw(self, square):
        x = 52 + self.pos.x * square
        y = 12 + self.pos.y * square

        hole = pygame.image.load("hole.png").convert_alpha()  # tu ścieżka do zdjęcia w tle
        hole = pygame.transform.scale(hole, (square, square))
        screen.blit(hole, (x, y))


def draw(square_num, objectArr):
    # następne dwie linijki do odkomentowania, jak będzie wgrane zdjęcie do tła
    # background = pygame.image.load("ścieżka do pliku").convert() #tu ścieżka do zdjęcia w tle
    # screen.blit(background, (0, 0))
    grid_color = (0, 0, 0)  # kolor czarny

    grid_size = 510  # rozmiar kraty
    square = grid_size // square_num  # rozmiar pojedyńczego kwadracika

    a = 50
    b = 10  # odległości kraty od krawędzi okna

    for o in objectArr:
        o.draw(square)

    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)


def kb_listen(objectArray, gridLength):
    agent = objectArray[0]
    for event in pygame.event.get():
        if event.type == pygame.KEYDOWN:
            agent.move(event, gridLength)
        if event.type == pygame.QUIT:
            sys.exit()


#moje

def manhattan(node1, node2):
    x1, y1 = node1.state[0], node1.state[1]
    x2, y2 = node2.state[0], node2.state[1]
    distance = abs(x1 - x2) + abs(y1 - y2)
    return distance


class Node:  #prawie jak Field w bfs
    def __init__(self, state, parent, action):
        self.state = state  #position - (x, y, direction)
        self.parent = parent
        self.action = action

    def __eq__(self, other):
        return True

    def __lt__(self, other):
        return True


def f(state):#tablica z losowymi wagami(kosztami) pól, w astar trzeba zsumować wagę pola z heurystyką - f + manhattan
    weights = np.array([
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 8],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 4],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
        [9, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2]
    ])
    pos_x = state[0]
    pos_y = state[1]
    
    return weights[pos_x][pos_y]


def succ1(state):
    successors = []#-90 obrót w lewo, +90 obrót w prawo
    print(state[0])#operujemy na 0, 90, 180, 270
    print(state[1])
    right = state[2] + 90
    left = state[2] - 90
    if right == 360:
        right = 0
    successors.append((("turn right"), (state[0], state[1], right)))
    if left == -90:
        left = 270
    successors.append((("turn left"), (state[0], state[1], left)))
    if (state[0], state[1]) not in black_list:#działa
        if state[2] == 0 and state[0] < 14:
            new_x = state[0]+1
            successors.append((("move forward"), (new_x, state[1], state[2])))
        elif state[2] == 90 and state[1] > 0:
            new_y = state[1]-1
            successors.append((("move forward"), (state[0], new_y, state[2])))
        elif state[2] == 180 and state[0] > 0:
            new_x = state[0]-1
            successors.append((("move forward"), (new_x, state[1], state[2])))
        elif state[2] == 270 and state[1] < 14:
            new_y = state[1]+1
            successors.append((("move forward"), (state[0], new_y, state[2])))
    return successors


def algorithm():
    opened = PriorityQueue()#może być też lista
    closed = []#już odwiedzone, odrzucone wierzchołki
    list_of_actions = []

    first_state = (0, 0, 0)#x, y, kierunek
    final_state = (14, 14, 0)
    starting_point = Node(first_state, False, False)
    ending_point = Node(final_state, False, False)

    pos1 = (starting_point.state[0], starting_point.state[1])
    pos2 = (ending_point.state[0], ending_point.state[1])

    opened.put((1, starting_point))

    while not opened.empty():
        elem = opened.get()[1]#[1] bo inaczej elem nie ma state
        if elem.state[0] == ending_point.state[0] and elem.state[1] == ending_point.state[1]:
            while elem.action is not False:
                list_of_actions.insert(0, elem.action)
                elem = elem.parent
            return list_of_actions
        if elem.state not in closed:
            closed.append(elem.state)
            for (action, state) in succ1(elem.state):
                point = Node(state, elem, action)
                score = manhattan(point, ending_point) + f(point.state)
                print(score)
                if state not in opened.queue:
                    opened.put((score, point))
                if state in opened.queue:
                    opened.queue.remove(elem)
                    opened.put((score, point))


if __name__ == '__main__':
    f((1, 1, 0))
    pygame.init()  # inicjalizacja modułów, na razie niepotrzebna
    gridSize = 15

    # Tworzymy nowego playera, czy tam agenta
    agent = Agent("smieciarka", Position(0, 0))
    junkyard = Junkyard("wysypisko", Position(10, 10))
    houses = [House(f'dom-{i}', pos) for i, pos in enumerate([Position(x, y) for x, y in [
        (7, 4), (3, 10), (8, 10), (4, 5), (1, 2), (10, 4), (13, 14), (6, 9)
    ]])]
    holes = [Hole(f'dziura-{i}', pos) for i, pos in enumerate([Position(x, y) for x, y in [
        (4, 9), (5, 11), (11, 7), (13, 8)
    ]])]
    objectArray.append(agent)
    objectArray.append(junkyard)
    objectArray += houses
    objectArray += holes

    collisionsMap = [[False] * gridSize for _ in range(gridSize)]
    for object in objectArray[1:]:
        collisionsMap[object.pos.x][object.pos.y] = True

    black_list = [(10, 10), (7, 4), (3, 10), (8, 10), (4, 5), (1, 2), (10, 4), (13, 14), (6, 9), (4, 9), (5, 11), (11, 7), (13, 8)]
    c = algorithm()
    print(c)

    width = 610
    height = 530
    screen = pygame.display.set_mode((width, height))  # ustalanie rozmiarów okna

    while 1:
        c = (255, 255, 255)  # tymczasowy kolor tła - do usunięcia, jak już będzie zdjęcie
        screen.fill(c)
        draw(gridSize, objectArray)
        kb_listen(objectArray, gridSize)
        pygame.display.update()  # by krata pojawiła się w okienku - update powierzc