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

# Load library functions we want
import time
import os
import sys
import pygame
import PicoBorgRev
import numpy
import UltraBorg
import datetime
import math
import XLoBorg
import wiringpi2 as wiringpi

wiringpi.wiringPiSetup()
# Setup the LedBorg GPIO pins
PIN_RED = 0
PIN_GREEN = 2
PIN_BLUE = 3 
wiringpi.pinMode(PIN_RED, wiringpi.GPIO.OUTPUT)
wiringpi.pinMode(PIN_GREEN, wiringpi.GPIO.OUTPUT)
wiringpi.pinMode(PIN_BLUE, wiringpi.GPIO.OUTPUT)
wiringpi.pinMode(PIN_BLUE, wiringpi.GPIO.OUTPUT)
   
# A function to set the LedBorg colours
def SetLedBorg(red, green, blue):
    wiringpi.digitalWrite(PIN_RED, red)
    wiringpi.digitalWrite(PIN_GREEN, green)
    wiringpi.digitalWrite(PIN_BLUE, blue)

# A function to turn the LedBorg off
def LedBorgOff():
    SetLedBorg(0, 0, 0)
# Settings for the sequence
delay = 2
# Set the LedBorg to red
SetLedBorg(1, 0, 0)
print 'Start Up'
time.sleep(delay)
# Turn the LedBorg off
LedBorgOff()

# 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
print 'Libraries loaded'

# Global values
global autoMode
global lineMode
global manualMode
autoMode = False
lineMode = False
manualMode = False
pauseMode = False
running = True
controllerLost = False

# Setup the UltraBorg
global UB
UB = UltraBorg.UltraBorg()              # Create a new UltraBorg object
UB.Init()                               # Set the board up (checks the board is connected)

# Setup the PicoBorg Reverse
PBR = PicoBorgRev.PicoBorgRev()
#PBR.i2cAddress = 0x44                  # Uncomment and change the value if you have changed the board address
PBR.Init()
if not PBR.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:' % (PBR.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 'PBR.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#PBR.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
    PBR.SetCommsFailsafe(True)
    failsafe = PBR.GetCommsFailsafe()
    if failsafe:
        break
if not failsafe:
    print 'Board %02X failed to report in failsafe mode!' % (PBR.i2cAddress)
    sys.exit()
PBR.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)
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time
buttonSetAutoMode = 4                   # Joystick button number to enable automatic control (D-Pad UP)
buttonSetLineMode = 7                   # Joystick button number to enable automatic line follow (D-Pad UP)
buttonSetPauseMode = 5                   # Joystick button number to enable pause mode (D-Pad UP)
buttonSetManualMode = 6                 # Joystick button number to enable manual control (D-Pad DOWN)
buttonShutdown = 0                      # Joystick button nuber to get the RPI to shut down

# Power settings
voltageIn = 1.2 * 12                    # Total battery voltage to the PicoBorg Reverse
voltageOut = 12.0                       # Maximum motor voltage

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Setup pygame and wait for the joystick to become available
PBR.MotorsOff()
os.environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
pygame.display.set_mode((1,1))
print 'Waiting for joystick... (press CTRL+C to abort)'
while True:
    try:
        try:
            pygame.joystick.init()
            # Attempt to setup the joystick
            if pygame.joystick.get_count() < 1:
                # No joystick attached, toggle the LED
                PBR.SetLed(not PBR.GetLed())
                pygame.joystick.quit()
                time.sleep(0.5)
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick, toggle the LED
            PBR.SetLed(not PBR.GetLed())
            pygame.joystick.quit()
            time.sleep(0.5)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print '\nUser aborted'
        PBR.SetLed(True)
        sys.exit()
print 'Joystick found'
joystick.init()
PBR.SetLed(False)


try:
    print 'Press CTRL+C to quit'
    driveLeft = 0.0
    driveRight = 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:
                if joystick.get_button(buttonSetAutoMode):
                    autoMode = True
                    lineMode = False
                    manualMode = False
                    pauseMode = False
                if joystick.get_button(buttonSetLineMode):
                    lineMode = True
                    autoMode = False
                    manualMode = False
                    pauseMode = False                    
                if joystick.get_button(buttonSetManualMode):
                    manualMode = True
                    autoMode = False
                    lineMode = False  
                    pauseMode = False          
                if joystick.get_button(buttonSetPauseMode):
                    pauseMode = True
                    manualMode = False
                    autoMode = False
                    lineMode = False            
                           
                if lineMode:
                   print 'Line folow mode'
                   SetLedBorg(0, 0, 0)
                   SetLedBorg(0, 0, 1)                
                   print 'line folow mode'   
                   
                if autoMode:
                   print 'Auto mode'
                   SetLedBorg(0, 0, 0)
                   SetLedBorg(1, 1, 1)
                   # Ultrasonic distance readings
                   usm1 = UB.GetRawDistance1()
                   usm2 = UB.GetRawDistance2()
                   usm3 = UB.GetRawDistance3()
                   usm4 = UB.GetRawDistance4()
                   # Convert to the nearest millimeter
                   usm1 = int(usm1)
                   usm2 = int(usm2)
                   usm3 = int(usm3)
                   usm4 = int(usm4)
                   
                   # Logic for auto mode
                   # set motors drive fward Fast
                   if usm1 >= 400: 
                      driveLeft = 0.6
                      driveRight = 0.6
                      driveLeft *= maxPower
                      driveRight *= maxPower
                      PBR.SetMotor1(driveRight)
                      PBR.SetMotor2(-driveLeft)
                   
                   # set motors drive fward slow
                   if usm1 <= 399:
                      driveLeft = 0.4
                      driveRight = 0.4
                      driveLeft *= maxPower
                      driveRight *= maxPower
                      PBR.SetMotor1(driveRight)
                      PBR.SetMotor2(-driveLeft) 
                      
                   # set motors drive turn left
                   if usm2 <= 399:
                      driveLeft = -0.4
                      driveRight = 0.4
                      driveLeft *= maxPower
                      driveRight *= maxPower
                      PBR.SetMotor1(driveRight)
                      PBR.SetMotor2(-driveLeft) 
                      
                   # set motors drive turn left
                   if usm3 <= 399:
                      driveLeft = 0.4
                      driveRight = -0.4
                      driveLeft *= maxPower
                      driveRight *= maxPower
                      PBR.SetMotor1(driveRight)
                      PBR.SetMotor2(-driveLeft)                         
                     
                if manualMode:
                    print'Manual joystick mode'
                    SetLedBorg(0, 0, 0)
                    SetLedBorg(0, 1, 1)
                    # 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):
                        PBR.ResetEpo()
                    if joystick.get_button(buttonSlow):
                        driveLeft *= slowFactor
                        driveRight *= slowFactor
                    # Set the motors to the new speeds
                    PBR.SetMotor1(driveRight * maxPower)
                    PBR.SetMotor2(-driveLeft * maxPower)
                    
                if pauseMode:
                   print 'Pause mode, use D-pad to shoose mode'
                   SetLedBorg(0, 0, 0)
                   SetLedBorg(1, 0, 0)
                   PBR.MotorsOff()
                   
                if joystick.get_button(buttonShutdown):
                   print 'Shutting Down...'
                   SetLedBorg(0, 0, 0)
                   PBR.MotorsOff()
                   PBR.SetCommsFailsafe(False)
                   PBR.SetLed(True)
                   os.system('sudo halt')
                   break 

        # Change the LED to reflect the status of the EPO latch
        PBR.SetLed(PBR.GetEpo())
        # Wait for the interval period
        time.sleep(interval)
    # Disable all drives
    PBR.MotorsOff()
    SetLedBorg(0, 0, 0)
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    PBR.MotorsOff()
    SetLedBorg(0, 0, 0)
print