#! /usr/bin/env python

import pygame
import math
import serial 
import time

START_BYTE = '\x80'
ID = '\0x7'
SET_MOTORS = '\0xD0'

bgcolor = 0, 128, 0
linecolor = 255, 128, 255
gridcolour = 255, 255, 255

#ser = serial.Serial("com3", 57600, timeout=1)

x = y = 0
running = 1
screen = pygame.display.set_mode((512, 512))

def turnwheels(left,right,leftdir,rightdir):
        #turnmotors = '%c%c' % ( chr(194), chr(velocity))
        #ser.write(turnmotors)
        print "left(%f) right (%f) leftdir (%f) rightdir (%f)" % (left,right,leftdir,rightdir)
        
while running:
    event = pygame.event.poll()
    if event.type == pygame.QUIT:
        running = 0
    elif event.type == pygame.MOUSEMOTION:
        x, y = event.pos
        velocity = 255 - y
        if velocity > 0:
                if x < 256:
                    rightdir = 1
                    leftdir = 1
                    leftrate = 256 - x # 0 is closer to center, 256 is far to left
                    # Turning left, subtract left value from forwards to slow left wheel
                    leftspeed = velocity - leftrate
                    if leftspeed < 0: # Left wheel may and up wanting to go backwards
                            leftdir = 0
                            leftspeed = leftspeed - leftspeed - leftspeed   #htbabw  
                    turnwheels(leftspeed,velocity,leftdir,rightdir)
                if x > 256:
                    rightdir = 1
                    leftdir = 1   
                    # Turning right, subtract "right" value from forwards value to slow right wheel
                    rightrate  = x - 256 # 0 closer, 256 far to right
                    rightspeed = velocity - rightrate
                    if rightspeed < 0: #Right wheel may want to spin backwards
                                rightdir = 0
                                rightspeed = rightspeed - rightspeed - rightspeed #htbabw                                    
                    turnwheels(velocity,rightspeed,leftdir,rightdir)
                if x == 256:
                        leftdir = 1
                        rightdir = 1
                        turnwheels(velocity,velocity,leftdir,rightdir)
        if velocity < 0:
                leftdir = 0
                rightdir = 0
                #going backwards, things get inverted.
                reverse = 0 - velocity #Flip negative velocity to positive
                if x < 256:
                        leftdir = 0
                        leftrate = 256 - x # 0 is closer to center, 256 is far to left
                        # Turning left, subtract left value from forwards to slow left wheel
                        leftspeed = reverse - leftrate
                        if leftspeed < 0: # Left wheel may and up wanting to go forwards
                                leftdir = 1
                                leftspeed = leftspeed - leftspeed - leftspeed   #htbabw  
                        
                        turnwheels(leftspeed,reverse,leftdir,rightdir)
                if x > 256:
                        rightdir = 0
                        # Turning right, subtract "right" value from forwards value to slow right wheel
                        rightrate  = x - 256 # 0 closer, 256 far to right
                        rightspeed = reverse - rightrate
                        if rightspeed < 0: #Right wheel may want to spin forwards
                                rightdir = 1
                                rightspeed = rightspeed - rightspeed - rightspeed #htbabw                     
                        turnwheels(reverse,rightspeed,leftdir,rightdir)
                if x == 256:
                    turnwheels(reverse,reverse,leftdir,rightdir)                
                

            
    screen.fill(bgcolor)
    pygame.draw.line(screen, gridcolour, (0, 0), (511, 511))
    pygame.draw.line(screen, gridcolour, (255, 0), (255, 511))
    pygame.draw.line(screen, gridcolour, (0, 255), (511, 255))
    pygame.draw.line(screen, gridcolour, (0, 511), (511,0))    
    pygame.draw.line(screen, linecolor, (x, 0), (x, 511))
    pygame.draw.line(screen, linecolor, (0, y), (511, y))
    pygame.draw.line(screen, linecolor, (255, 255), (x, y))
    pygame.display.flip()

pygame.quit ()