#!/usr/bin/env python
# coding: Latin-1

# Load library functions we want
import time
import os
import sys
import pygame
import PicoBorgRev

# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr

# Setup the first PicoBorg Reverse
PBR1 = PicoBorgRev.PicoBorgRev()
PBR1.i2cAddress = 10
PBR1.Init()
if not PBR1.foundChip:
    boards = PicoBorgRev.ScanForPicoBorgReverse()
    if len(boards) == 0:
        print 'No PicoBorg Reverse found, check you are attached :)'
    else:
        print 'No PicoBorg Reverse at address %02X, but we did find boards:' % (PBR1.i2cAddress)
        for board in boards:
            print '    %02X (%d)' % (board, board)
        print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
        print 'PBR1.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#PBR1.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
PBR1.ResetEpo()

# Setup the second PicoBorg Reverse
PBR2 = PicoBorgRev.PicoBorgRev()
PBR2.i2cAddress = 11
PBR2.Init()
if not PBR2.foundChip:
    boards = PicoBorgRev.ScanForPicoBorgReverse()
    if len(boards) == 0:
        print 'No PicoBorg Reverse found, check you are attached :)'
    else:
        print 'No PicoBorg Reverse at address %02X, but we did find boards:' % (PBR2.i2cAddress)
        for board in boards:
            print '    %02X (%d)' % (board, board)
        print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
        print 'PBR2.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#PBR2.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
PBR2.ResetEpo()

# Settings for the joystick
axisUpDown = 1                          # Joystick axis to read for up / down position
axisUpDownInverted = False              # Set this to True if up and down appear to be swapped
axisLeftRight = 2                       # Joystick axis to read for left / right position
axisLeftRightInverted = False           # Set this to True if left and right appear to be swapped
buttonResetEpo = 3                      # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8                          # Joystick button number for driving slowly whilst held (L2)
slowFactor = 0.5                        # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed
buttonFastTurn = 9                      # Joystick button number for turning fast (R2)
buttonThirdMinus = 7                    # Joystick button number to make the third drive run at -100% (D-Pad left)
buttonThirdZero = 6                     # Joystick button number to make the third drive run at 0% (D-Pad down)
buttonThirdPlus = 5                     # Joystick button number to make the third drive run at +100% (D-Pad right)
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time

# Setup pygame
os.environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()

try:
    print 'Press CTRL+C to quit'
    driveLeft = 0.0
    driveRight = 0.0
    driveThird = 0.0
    running = True
    hadEvent = False
    upDown = 0.0
    leftRight = 0.0
    # Loop indefinitely
    while running:
        # Get the latest events from the system
        hadEvent = False
        events = pygame.event.get()
        # Handle each event individually
        for event in events:
            if event.type == pygame.QUIT:
                # User exit
                running = False
            elif event.type == pygame.JOYBUTTONDOWN:
                # A button on the joystick just got pushed down
                hadEvent = True                    
            elif event.type == pygame.JOYAXISMOTION:
                # A joystick has been moved
                hadEvent = True
            if hadEvent:
                # Read axis positions (-1 to +1)
                if axisUpDownInverted:
                    upDown = -joystick.get_axis(axisUpDown)
                else:
                    upDown = joystick.get_axis(axisUpDown)
                if axisLeftRightInverted:
                    leftRight = -joystick.get_axis(axisLeftRight)
                else:
                    leftRight = joystick.get_axis(axisLeftRight)
                # Apply steering speeds
                if not joystick.get_button(buttonFastTurn):
                    leftRight *= 0.5
                # Determine the drive power levels
                driveLeft = -upDown
                driveRight = -upDown
                if leftRight < -0.05:
                    # Turning left
                    driveLeft *= 1.0 + (2.0 * leftRight)
                elif leftRight > 0.05:
                    # Turning right
                    driveRight *= 1.0 - (2.0 * leftRight)
                # Check for button presses
                if joystick.get_button(buttonResetEpo):
                    PBR1.ResetEpo()
                    PBR2.ResetEpo()
                if joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                if joystick.get_button(buttonThirdMinus):
                    driveThird = -1.0
                if joystick.get_button(buttonThirdZero):
                    driveThird = 0.0
                if joystick.get_button(buttonThirdPlus):
                    driveThird = 1.0
                # Set the motors to the new speeds
                PBR1.SetMotor1(driveLeft)
                PBR1.SetMotor2(driveRight)
                PBR2.SetMotor1(driveThird)
        # Change the LED to reflect the status of the EPO latch
        PBR1.SetLed(PBR1.GetEpo())
        PBR2.SetLed(PBR2.GetEpo())
        # Wait for the interval period
        time.sleep(interval)
    # Disable all drives
    PBR1.MotorsOff()
    PBR2.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    PBR1.MotorsOff()
    PBR2.MotorsOff()
print