import pygame
from pygame.locals import *
from astar import Astar
from genetic import genetic
import numpy as np
import time

def drawGrid(w, rows, surface, goals):
    sizeBtwn = w // rows
    x = 0
    y = 0
    for l in range(rows):
        x = x + sizeBtwn
        y = y + sizeBtwn

        pygame.draw.line(surface, (255, 0, 255), (x, 0), (x, width))
        pygame.draw.line(surface, (255, 0, 255), (0, y), (width, y))

def drawHouses(houses):
    for house in houses:
        screen.blit(home, (13 + house[1] * 60, 15 + house[0] * 60))

def drawObstacles(obs):
    for ob in obs:
        screen.blit(xobstacle, (13 + ob[1] * 60, 15 + ob[0] * 60))

def findObstacles(A):
    obstacles = []
    for i in range(10):
        for j in range(10):
            if A[i][j] == 3:
                obstacles.append([i, j])

    return obstacles



pygame.init()

width = 600
height = 600
FPS = 60
running = True
fpsClock = pygame.time.Clock()

screen = pygame.display.set_mode((height, height), 0, 32)

pygame.display.set_caption('InteligentnaSmieciarka2000')
truck = pygame.image.load('GarbageTruck.png')
home = pygame.image.load('Home.png')
xobstacle = pygame.image.load("x.png")

w = 10
rows = 10
A = np.zeros((w, rows), dtype=int)

A[1, 0] = 3
A[0, 2] = 3
A[4, 1] = 3
rows = 10
x = 15
y = 20

sizeBtwn = width // rows

parent = [0, 0]

goals = [[0, 4], [4, 0], [4, 4], [6, 7]]
houses = goals.copy()

goals = genetic(goals, A).genes

start = [0, 0]

obsta = findObstacles(A)

while goals:

    goal = goals.pop(0)
    path = Astar(A, start, goal)
    start = goal
    while path:
        screen.fill((0, 0, 0))
        drawGrid(width, rows, screen, goals)
        drawHouses(houses)
        drawObstacles(obsta)

        pygame.event.get()

        if path:
            destination = path.pop(0)
            temp = destination
            destination = [destination[0] - parent[0], destination[1] - parent[1]]

            if destination[0] == 1:
                y += 60
                time.sleep(0.3)
            elif destination[0] == -1:
                y -= 60
                time.sleep(0.3)
            elif destination[1] == 1:
                x += 60
                time.sleep(0.3)
            elif destination[1] == -1:
                x -= 60
                time.sleep(0.3)

            parent = temp
            screen.blit(truck, (x, y))
            pygame.display.update()

        #screen.blit(truck, (x, y))
        #pygame.display.update()

time.sleep(5)