#!/usr/bin/env python
# coding: utf-8

# Load the gamepad and time libraries
import Gamepad
import time

# Gamepad settings
gamepadType = Gamepad.PS5
buttonHappy = 'CROSS'
buttonBeep = 'CIRCLE'
buttonExit = 'PS'
joystickSpeed = 'LEFT-Y'
joystickSteering = 'RIGHT-X'
dpadX = 'DPAD-X'
dpadY = 'DPAD-Y'
pollInterval = 0.2

# Wait for a connection
if not Gamepad.available():
    print('Please connect your gamepad...')
    while not Gamepad.available():
        time.sleep(1.0)
gamepad = gamepadType()
print('Gamepad connected')

# Set some initial state
global running
global beepOn
global speed
global steering
global direction
global dirChange
running = True
beepOn = False
speed = 0.0
steering = 0.0
direction = ""
dirChange = False

# Create some callback functions
def happyButtonPressed():
    print(':)')

def happyButtonReleased():
    print(':(')

def beepButtonChanged(isPressed):
    global beepOn
    beepOn = isPressed

def exitButtonPressed():
    global running
    print('EXIT')
    running = False

def speedAxisMoved(position):
    global speed
    speed = -position   # Inverted

def steeringAxisMoved(position):
    global steering
    steering = position # Non-inverted

def dpadXMoved(dir):
    global direction
    global dirChange
    print("X direction: ", dir)
    match dir:
        case -1.0:
            direction = 'LEFT'
            dirChange = True
        case 1.0:
            direction = 'RIGHT'
            dirChange = True
            
def dpadYMoved(dir):
    global direction
    global dirChange
    print("Y direction: ", dir)
    match dir:
        case -1.0:
            direction = 'UP'
            dirChange = True
        case 1.0:
            direction = 'DOWN'
            dirChange = True

# Start the background updating
gamepad.startBackgroundUpdates()

# Register the callback functions
gamepad.addButtonPressedHandler(buttonHappy, happyButtonPressed)
gamepad.addButtonReleasedHandler(buttonHappy, happyButtonReleased)
gamepad.addButtonChangedHandler(buttonBeep, beepButtonChanged)
gamepad.addButtonPressedHandler(buttonExit, exitButtonPressed)
gamepad.addAxisMovedHandler(joystickSpeed, speedAxisMoved)
gamepad.addAxisMovedHandler(joystickSteering, steeringAxisMoved)
gamepad.addAxisMovedHandler(dpadX, dpadXMoved)
gamepad.addAxisMovedHandler(dpadY, dpadYMoved)

# Keep running while joystick updates are handled by the callbacks
try:
    while running and gamepad.isConnected():
        # Show the current speed and steering
        # print('%+.1f %% speed, %+.1f %% steering' % (speed * 100, steering * 100))

        # Display the beep if held
        if beepOn:
            print('BEEP')

        if dirChange:
            print(direction)
            dirChange = False
            
        # Sleep for our polling interval
        time.sleep(pollInterval)
finally:
    # Ensure the background thread is always terminated when we are done
    gamepad.disconnect()