import pygame
import numpy as np
import math
import pickle

# Colors:
# Define some colors
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
GREEN = (0, 255, 0)
RED = (255, 0, 0)
BLUE = (0, 0, 240)
YELLOW = (255, 255, 0)
#Width and Height of each square:
WIDTH = 20
HEIGHT = 20

#Margin:
MARGIN = 5
grid = [[0 for x in range(16)] for y in range(16)]

def change_value(i, j, width, n):
    for r in range (i, i+width):
        for c in range (j, j+width):
            grid[r][c] = n
            
class Table:
    def __init__(self, coordinate_i, coordinate_j):
        self.coordinate_i = coordinate_i
        self.coordinate_j = coordinate_j
        change_value(coordinate_i, coordinate_j, 2, 1)
    def get_destination_coor(self):
        return [self.coordinate_i, self.coordinate_j-1]
        
class Kitchen:
    def __init__(self, coordinate_i, coordinate_j):
        self.coordinate_i = coordinate_i
        self.coordinate_j = coordinate_j
        change_value(coordinate_i, coordinate_j, 3, 2)
        
class Agent:
    def __init__(self,orig_coordinate_i, orig_coordinate_j):
        self.orig_coordinate_i = orig_coordinate_i
        self.orig_coordinate_j = orig_coordinate_j
        self.state = np.array([orig_coordinate_i,orig_coordinate_j])
        change_value(orig_coordinate_j, orig_coordinate_j, 1, 3)
        self.state_update(orig_coordinate_i, orig_coordinate_j)
        
    def state_update(self, c1, c2):
        self.state[0] = c1
        self.state[1] = c2
        
    def leave(self):
        change_value(self.state[0], self.state[1], 1, 0)
    
    
    def move_to(self, nextPos):
        self.leave()
        nextPos_x, nextPos_y = nextPos
        self.state_update(nextPos_x, nextPos_y)
        change_value(self.state[0], self.state[1], 1, 3)
    

def check_done():
    for event in pygame.event.get():        # Checking for the event
        if event.type == pygame.QUIT:       # If the program is closed:
            return True                     # To exit the loop

def draw_grid(visited):
    for row in range(16):                   # Drawing the grid
        for column in range(16):
            color = WHITE
            if grid[row][column] == 1:
                color = GREEN
            if grid[row][column] == 2:
                color = RED
            if grid[row][column] == 3:
                color = BLUE
            if (row, column) in visited or (row, column) in table_targets:
                color = YELLOW
            pygame.draw.rect(screen,
                             color,
                             [(MARGIN + WIDTH) * column + MARGIN,
                              (MARGIN + HEIGHT) * row + MARGIN,
                              WIDTH,
                              HEIGHT]) 


## default positions of the agent:
x = 12
y = 12
agent = Agent(x, y)

table1 = Table(2, 2)
table2 = Table (2,7)
table3 = Table(2, 12)
table4 = Table(7, 2)
table5 = Table(7, 7)
table6 = Table(7, 12)
table7 = Table(12, 2)
table8 = Table(12, 7)

#class Kitchen:
kitchen = Kitchen(13, 13)

pygame.init()
WINDOW_SIZE = [405, 405]
screen = pygame.display.set_mode(WINDOW_SIZE)

pygame.display.set_caption("Waiter_Grid3")

done = False

clock = pygame.time.Clock()
 
with open('res_targets_4-1.data', 'rb') as filehandle:
    # read the data as binary data stream
    trained_route = pickle.load(filehandle)

print(trained_route)
destination = (9, 4)
trained_route.append(destination)

table_targets = [(4, 4),(4, 9),(4, 14),(9, 4)]

# -------- Main Program Loop -----------
while not done:
    visited = set()
    screen.fill(BLACK)                      # Background color
    draw_grid(visited)
    done = check_done()
    new_route = trained_route[:]
    
    while len(new_route) != 0:
        x = agent.state[0]
        y = agent.state[1]
        
        agent.move_to(new_route[0])
        new_route = new_route[1:]
        
        
        pygame.time.delay(150)
        screen.fill(BLACK)  
        visited.add((x,y))
        draw_grid(visited)
                        # Drawing the grid
        clock.tick(100)                           # Limit to 60 frames per second
        pygame.display.flip()                    # Updating the screen
    
    
pygame.quit()