import smbus
import time
import serial
import pygame
import time

bus = smbus.SMBus(1)

address = 0x04

pygame.init()
j = pygame.joystick.Joystick(0)
j.init()
global speedr
global speedl
global directionr
global directionl
    
try:
    while True:
        pygame.event.pump()
        time.sleep(0.1)
        if (j.get_axis(2) != -1.0 or j.get_axis(5) != -1.0) and (j.get_button(4) == 0 and j.get_button(5) == 0) :
            if j.get_axis(2) != -1.0 and j.get_axis(5) == -1.0 :
                directionl = 1
                directionr = 1
                print "direction back" , + directionl
                value = j.get_axis(2)
                if value < 0:
                     speedr = (1+value)*127
                     speedl = (1+value)*127
                     print "speed back" , + int(speedl)
                else:
                     speedr = (value*128)+127
                     speedl = (value*128)+127
                     print "speed back" , + int(speedl)
        
            elif j.get_axis(2) == -1.0 and j.get_axis(5) != -1.0 :
                directionl = 0
                directionr = 0
                print "direction front" , + directionl
                value = j.get_axis(5)
                if value < 0:
                     speedr = (1+value)*127
                     speedl = (1+value)*127
                     print "speed front", + int(speedl)
                else:
                     speedr = (value*128)+127
                     speedl = (value*128)+127
                     print "speed front" , + int(speedl)
            else:                 continue
                
        elif j.get_button(4) != 0 or j.get_button(5) != 0 :    
            if j.get_button(4) != 0 and j.get_button(5) == 0:
                
                    if j.get_button(4) == 1:
                        print "left"
                        directionl = 0
                        directionr = 1
                        speedl = 255
                        speedr = 0
                        
            elif j.get_button(5) != 0 and j.get_button(4) == 0:            
               
                    if j.get_button(5) == 1:
                        print "right"
                        directionl = 1
                        directionr = 0
                        speedl = 0
                        speedr = 255
            
            else: 
                continue
              
        else:
            speedr = 0
            speedl = 0
            directionr = 1
            directionl = 0
        
        
        bus.write_i2c_block_data(address,directionr,[directionl,int(speedr),int(speedl)])
        #time.sleep(1)

except KeyboardInterrupt:
     j.quit()
