import cwiid 
import time
import os
import RPi.GPIO as GPIO
from time import sleep


GPIO.cleanup()

#assign GPIO pins 
LeftMotorTrackA = 3
LeftMotorTrackB = 5
RightMotorTrackA = 7
RightMotorTrackB = 8
MotorGripperA = 10
MotorGripperB = 11
GripperLED=12
CameraServo = 13
LeftMotorTrackEnable=31
RightMotorTrackEnable=33
GripperEnable=35

#set GPIO pins to output
GPIO.setmode(GPIO.BOARD)
GPIO.setup(LeftMotorTrackA,GPIO.OUT)
GPIO.setup(LeftMotorTrackB,GPIO.OUT)
GPIO.setup(RightMotorTrackA,GPIO.OUT)
GPIO.setup(RightMotorTrackB,GPIO.OUT)
GPIO.setup(MotorGripperA,GPIO.OUT)
GPIO.setup(MotorGripperB,GPIO.OUT)
GPIO.setup(GripperLED,GPIO.OUT)
GPIO.setup(LeftMotorTrackEnable,GPIO.OUT)
GPIO.setup(RightMotorTrackEnable,GPIO.OUT)
GPIO.setup(GripperEnable,GPIO.OUT)
GPIO.setup(CameraServo,GPIO.OUT)

def TrackDriveForward():
    GPIO.output(LeftMotorTrackA,GPIO.HIGH)
    GPIO.output(LeftMotorTrackB,GPIO.LOW)
    GPIO.output(RightMotorTrackA,GPIO.HIGH)
    GPIO.output(RightMotorTrackB,GPIO.LOW)

def TrackDriveReverse():
    GPIO.output(LeftMotorTrackA,GPIO.LOW)
    GPIO.output(LeftMotorTrackB,GPIO.HIGH)
    GPIO.output(RightMotorTrackA,GPIO.LOW)
    GPIO.output(RightMotorTrackB,GPIO.HIGH)

def TrackDriveLeft():
    GPIO.output(LeftMotorTrackA,GPIO.LOW)
    GPIO.output(LeftMotorTrackB,GPIO.HIGH)
    GPIO.output(RightMotorTrackA,GPIO.HIGH)
    GPIO.output(RightMotorTrackB,GPIO.LOW)

def TrackDriveRight():
    GPIO.output(LeftMotorTrackA,GPIO.HIGH)
    GPIO.output(LeftMotorTrackB,GPIO.LOW)
    GPIO.output(RightMotorTrackA,GPIO.LOW)
    GPIO.output(RightMotorTrackB,GPIO.HIGH)

def TrackDriveStop():
    GPIO.output(LeftMotorTrackA,GPIO.LOW)
    GPIO.output(LeftMotorTrackB,GPIO.LOW)
    GPIO.output(RightMotorTrackA,GPIO.LOW)
    GPIO.output(RightMotorTrackB,GPIO.LOW)

def GripperClose():
    GPIO.output(MotorGripperA,GPIO.LOW)
    GPIO.output(MotorGripperB,GPIO.HIGH)

def GripperStop():
    GPIO.output(MotorGripperA,GPIO.LOW)
    GPIO.output(MotorGripperB,GPIO.LOW)


    
#set pulse width modulation to 50 hz
LMpw = GPIO.PWM(LeftMotorTrackEnable, 50)
RMpw = GPIO.PWM(RightMotorTrackEnable, 50)
GRpw = GPIO.PWM(GripperEnable, 50)
CAMpw = GPIO.PWM(CameraServo, 50)

#start pwm at 50%
LMpw.start(50)
RMpw.start(50)        
GRpw.start(30)
CAMpw.start(7.5)



#Make the Bluetooth dongle discoverable
os.system("sudo hciconfig hci0 piscan")

#connecting to the Wiimote. This allows several attempts 
# as first few often fail. 
print 'Press 1+2 on your Wiimote now...' 

wm = None 
i=2 
while not wm: 
  try: 
    wm=cwiid.Wiimote() 
  except RuntimeError: 
    if (i>10): 
      print "Giving up connecting"
      quit() 
      break 
    print "Error opening wiimote connection" 
    print "attempt " + str(i) 
    i +=1 
    #Pause for a bit
    time.sleep(0.2)

#Got here, tell the user
print "Success - we have connected!"

#set up the inputs we are looking for
wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC | cwiid.RPT_EXT

#Wait a bit
time.sleep(0.5)

#Do a rumble
wm.rumble = True
time.sleep(0.5)
wm.rumble = False

#Now start checking for button presses
print "Ready to receive button presses and accelerometer input"

#Loop for ever detecting
isTurning = False;
lightOn = False;
try:
  while True:
    if (lightOn):
       GPIO.output(GripperLED,GPIO.HIGH)
    else:
       GPIO.output(GripperLED,GPIO.LOW)

    #Set up a button object to check
    buttons = wm.state['buttons']
    if (buttons & cwiid.BTN_MINUS):
      print 'Minus pressed'
      GripperClose()
    else:
      GripperStop()  

    if (buttons & cwiid.BTN_A):
       if (lightOn): 
         lightOn = False;
       else:
         lightOn = True;
    #stop everthing by pressing button B -- panic stop
    if (buttons & cwiid.BTN_B):
      print 'b pressed'
      GPIO.cleanup()

      #Here we handle the nunchuk, along with the joystick and the buttons 
    while(1): 
        if wm.state.has_key('nunchuk'): 
            try: 
                #Here is the data for the nunchuk stick: 
                #X axis:LeftMax = 25, Middle = 125, RightMax = 225 
                NunchukStickX = (wm.state['nunchuk']['stick'][cwiid.X]) 
                #Y axis:DownMax = 30, Middle = 125, UpMax = 225 
                NunchukStickY = (wm.state['nunchuk']['stick'][cwiid.Y]) 
                #The 'NunchukStickX' and the 'NunchukStickY' variables now store the stick values 
                #print NunchukStickX 
                #print NunchukStickY
                
                if ((NunchukStickX > 180) | (NunchukStickX < 60)):
                    isTurning = True;
                else:
                    isTurning = False;
                    
                if ((NunchukStickY > 140) & (NunchukStickY < 200) & (not isTurning)): 
                    print "forward" 
                    LMpw.ChangeDutyCycle(20)
                    RMpw.ChangeDutyCycle(20)
                    TrackDriveForward()
                elif ((NunchukStickY < 120) & (not isTurning)): 
                    print "Backward" 
                    LMpw.ChangeDutyCycle(25)
                    RMpw.ChangeDutyCycle(25)
                    TrackDriveReverse()
                elif ((NunchukStickY > 200) & (not isTurning)): 
                    print "fast forward" 
                    LMpw.ChangeDutyCycle(30)
                    RMpw.ChangeDutyCycle(30)
                    TrackDriveForward()
                elif (NunchukStickX >180): 
                    print "right" 
                    LMpw.ChangeDutyCycle(25)
                    RMpw.ChangeDutyCycle(25)
                    TrackDriveRight()
                elif (NunchukStickX < 60):
                    print "left" 
                    LMpw.ChangeDutyCycle(25)
                    RMpw.ChangeDutyCycle(25)
                    TrackDriveLeft()
                else: 
                    TrackDriveStop()
                #    print "ystop"    
                    
                ChukBtn = wm.state['nunchuk']['buttons'] 
                if (ChukBtn == 1):
                    CAMpw.ChangeDutyCycle(2.5)
                elif (ChukBtn == 2):
                    CAMpw.ChangeDutyCycle(4.5)
                                
                break 
            except KeyError: 
                print 'No nunchuk detected.' 
        else: 
            if (Counter == 10000): 
                print 'No nunchuk detected.' 
                Counter = Counter/10000 
                break 
            Counter = Counter + 1 
            break 
    #Chill for a bit 
    time.sleep(0.3) 

                
except KeyboardInterrupt:
    pass

print "Goodby"