import serial
import time
import pygame
import re
import sys

def text_objects(text, font):
    textSurface = font.render(text, True, WHITE)
    return textSurface, textSurface.get_rect()

class Controller:
        def __init__(self, com, speed):
            self.comport = com
            self.comspeed= speed

        def opencom(self):
            self.serial = serial.Serial(port=self.comport,baudrate=self.comspeed,timeout=5)

        def write(self, str):
            self.serial.write(str)
            print('scrivo : {}'.format(str))

        def readline(self):
            return self.serial.readline()

        def grbl_init(self):
            print "init grbl"
            self.serial.write("\r\n\r\n")
            time.sleep(2)
            self.serial.flushInput()
            print "grbl READY!"

        def closecom(self):
            self.serial.close()
            print "chiusura..."

        def send_wait(self, str):
            l_block = str.strip()
            c.write(l_block + '\n')
            grbl_out = c.readline().strip()
            print (grbl_out)

class Axis:
        def __init__(self, name, var, var2, var3, controller):
                self.name = name
                self.var = var
                self.var2 = var2
                self.var3 = var3
                self.position = 0
                self.targetPosition= 0
                self.step_mm = 160   # (step/mm)
                self.max_rate = 14000 # (max speed mm/min)
                self.accel = 1000     # acceleration
                self.headPosition = 0
                self.controller = controller

        def setup(self):
                line = ('{}={}{}'.format(self.var,self.step_mm, "\n"))
                c.send_wait(line)
                line2 = ('{}={}{}'.format(self.var2,self.max_rate, "\n"))
                c.send_wait(line2)
                line3 = ('{}={}{}'.format(self.var3,self.accel, "\n"))
                c.send_wait(line3)
                print "Axis Initialized"
                line4 = ('{}{}'.format("G21", "\n"))
                c.send_wait(line4)
                print "using mm as unit"
                line5 = ('{}{}'.format("G90", "\n"))
                c.send_wait(line5)
                print "using absolute coordinates"

        
class Commands:

            def __init__(self):
                pass

            def move_X(self,vel,posX,feed):
                line = ('{}{}{}{}{}{}'.format(vel,"X",posX,"F",feed,"\n"))
                c.send_wait(line)
            def move_both(self, vel, posX, posY, feed):
                line = ('{}{}{}{}{}{}{}{}'.format(vel,"X",posX,"Y",posY,"F",feed,"\n"))
                c.send_wait(line)

            def sol_on(self):
                line = ('{}{}'.format("M8", "\n"))
                time.sleep(0.5)
                c.send_wait(line)

            def sol_off(self):
                line = ('{}{}'.format("M9", "\n"))
                c.send_wait(line)
                
                
                
            
        


# Define some colors
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
GREEN = (0, 255, 0)
RED = (255, 0, 0)

# Define offset
OFFSET = 30

# This sets the WIDTH and HEIGHT of each grid location
WIDTH = 20
HEIGHT = 20
 
# This sets the margin between each cell
MARGIN = 5
 
# Create a 2 dimensional array. A two dimensional
# array is simply a list of lists.
grid = []
for row in range(12):
    # Add an empty array that will hold each cell
    # in this row
    grid.append([])
    for column in range(12):
        grid[row].append(0)  # Append a cell

# Initialize pygame
pygame.init()
 
# Set the HEIGHT and WIDTH of the screen
WINDOW_SIZE = [305, 365]
screen = pygame.display.set_mode(WINDOW_SIZE)
 
# Set title of screen
pygame.display.set_caption("Steam Machine v1.0")
 
# Loop until the user clicks the close button.
done = False
 
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
 
if __name__ == '__main__':

    c = Controller("COM7", 115200)
    c.opencom()
    c.grbl_init()
    #time.sleep(5)
    #grbl_out = c.readline().strip()
    #print(grbl_out)
    x_axis = Axis('X','$100','$110','$120',c)
    x_axis.setup()
    y_axis = Axis('Y','$101','$111','$121',c)
    y_axis.setup()
    d = Commands()
    
    
# -------- Main Program Loop -----------
    while not done:
        for event in pygame.event.get():  # User did something
            if event.type == pygame.QUIT:  # If user clicked close
                done = True  # Flag that we are done so we exit this loop
            elif event.type == pygame.MOUSEBUTTONDOWN:
                # User clicks the mouse. Get the position
                pos = pygame.mouse.get_pos()
                # Change the x/y screen coordinates to grid coordinates
                column = pos[0] // (WIDTH + MARGIN)
                row = pos[1] // (HEIGHT + MARGIN)
                print("Click ", pos, "Grid coordinates: ", row, column)
                # Set that location to one
                try:
                    if grid[row][column] == 1:
                        grid[row][column] = 0
                        
                    elif grid[row][column] == 0:
                        grid[row][column] = 1
                        d.move_both('G0',OFFSET*column,OFFSET*row,'500')
                        d.sol_on()
                        time.sleep(.3)
                        d.sol_off()
                        
                        
                except:
                    pass
            
        # Set the screen background
        screen.fill(BLACK)
        #pygame.draw.circle(screen, BLUE, (300, 50), 20, 0)
        #pygame.draw.rect(gameDisplay, red,(550,450,100,50))

        #draw the send button
        width, height = pygame.display.Info().current_w, pygame.display.Info().current_h
        pygame.draw.rect(screen, GREEN, [(width/2)-50, height-60, 100, 50])

        #text inside the button
        smallText = pygame.font.Font("freesansbold.ttf",20)
        textSurf, textRect = text_objects("G-CODE!", smallText)
        textRect.center = ( (width/2), height-30 )
        screen.blit(textSurf, textRect)
        # Draw the grid
        for row in range(12):
            for column in range(12):
                color = WHITE
                if grid[row][column] == 1:
                    color = GREEN
                pygame.draw.rect(screen,
                                 color,
                                 [(MARGIN + WIDTH) * column + MARGIN,
                                  (MARGIN + HEIGHT) * row + MARGIN,
                                  WIDTH,
                                  HEIGHT])
     
        # Limit to 60 frames per second
        clock.tick(60)
     
        # Go ahead and update the screen with what we've drawn.
        pygame.display.flip()
     
    # Be IDLE friendly. If you forget this line, the program will 'hang'
    # on exit.
    pygame.quit()
    c.closecom()