#!/usr/bin/env python

import time
import PicoBorgRev3 as PicoBorgRev
import pygame
import os
import UltraBorg3 as Ultraborg
 
# Settings
servoMin = -1.0                 # Smallest servo position to use
servoMax = +1.0                 # Largest servo position to use
startupDelay = 0.5              # Delay before making the initial move
stepDelay = 0.1                 # Delay between steps
rateStart = 0.05                # Step distance for all servos during initial move
rateServo1 = 0.01               # Step distance for servo #1
rateServo2 = 0.01               # Step distance for servo #2
 
# Start the UltraBorg
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Start the PicoBorg Reverse
PBR = PicoBorgRev.PicoBorgRev()
PBR.Init()
PBR.ResetEpo()

# Start pygame & define pygame screen size
pygame.init()
screen = pygame.display.set_mode ((480,480))

print('Move to central')
# Initial settings
global servo1
global servo2
servo1 = 0.0
servo2 = 0.0
# Set our initial servo positions
UB.SetServoPosition1(servo1)
UB.SetServoPosition2(servo2)
# Wait a while to be sure the servos have caught up
time.sleep(startupDelay)

# Define servo variables
def servo_left():
    global servo1
    servo1 += rateServo1
def servo_right():
    global servo1
    servo1 -= rateServo1
def servo_up():
    global servo2
    servo2 += rateServo2
def servo_down():
    global servo2
    servo2 -= rateServo2

# Check if any servos exceed limits and constrain to limit
if servo1 > servoMax:
    global servo1
    servo1 = servoMax
if servo1 < servoMax:
    global servo1
    servo1 = servoMax
if servo2 > servoMax:
    global servo2
    servo2 = servoMax
if servo2 < servoMin:
    global servo2
    servo2 = servoMin

# Set our new servo positions
    UB.SetServoPosition1(servo1)
    UB.SetServoPosition2(servo2)

# Define motor variables
def forwards():
    PBR.SetMotor1(+0.8)
    PBR.SetMotor2(+0.7)

def back():
    PBR.SetMotor1(-0.8)
    PBR.SetMotor2(-0.7)

def left():
    PBR.SetMotor1(-0.6)
    PBR.SetMotor2(+0.6)
    
def right():
    PBR.SetMotor1(+0.6)
    PBR.SetMotor2(-0.6)
    
def stop():
    PBR.MotorsOff()

def shutdown():
    os.system("sudo shutdown -P now")
      
while True:
    pygame.display.flip()
    keystate=pygame.key.get_pressed()
    
    if keystate[pygame.K_DOWN]:
        print('backwards')
        back()

    elif keystate[pygame.K_UP]:
        print('forwards')
        forwards()

    elif keystate[pygame.K_RIGHT]:
        print('right')
        right()

    elif keystate[pygame.K_LEFT]:
        print('left')
        left()

    elif keystate[pygame.K_a]:
        print('servo left')
        servo_left()

    elif keystate[pygame.K_d]:
        print('servo right')
        servo_right()

    elif keystate[pygame.K_w]:
        print('servo up')
        servo_up()

    elif keystate[pygame.K_x]:
        print('servo down')
        servo_down()

    elif keystate[pygame.K_SPACE]:
        print('shutdown')
        shutdown()

    else:
        stop()

    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            stop()
            exit(0)
            
    time.sleep(0.01)