# xbox one controller
# useful documentation: pygame.org/docs/ref/event.html

import pygame
from math import floor, sqrt, atan, pi, sin, cos
from select import select

from pygame.locals import *

btnCodes = [0, 1, 3, 4, 6, 7, 11, 13, 14]
btnNames = ["A", "B", "X", "Y", "LB", "RB", "home", "Ljoy", "Rjoy"]

joyCodes = [0, 1, 2, 3, 4, 5]
joyNames = ["Ljoy_horz", "Ljoy_vert", "Rjoy_horz", "Rjoy_vert", "RT", "LT"]

dpadCodes = [(0, 0), (0, 1), (1, 0), (0, -1), (-1, 0)]
dpadNames = ["Dpad_Open", "Dpad_N", "Dpad_E", "Dpad_S", "Dpad_W"]

pygame.init()
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()

trigger_pulled = False

def check_buttons():
    for i in range(4):
        if joystick.get_button(i):
            return i
    return -1


def joystick_angle(x, y):
    """ returns angle of joystick as compass bearing 
    North = 0 degrees, East = 90 degrees, etc.
    """
    if y < 0 and x > 0:
        angle = -1 * atan(x/y)*180/pi
    elif y < 0 and x < 0:
        angle = 360 - atan(x/y)*180/pi
    elif y > 0:
        angle = 180 - atan(x/y)*180/pi
    elif y == 0 and x > 0:
        angle = 90
    elif y == 0 and x < 0:
        angle = 270
    else:
        angle = 0  
    return int(angle)

def fix_xy_readings(x, y):
    """ 
    function apologizes for poor hardware
    takes non-linear x-y plane coordinates of joystick, converts to polar coordinates
    if radius in polar coordinates exceeds 100-unit radius circle, rescales 
    """
    angle = joystick_angle(x, y) * pi/180.0
    speed_uncorrected = int(sqrt(x**2 + y**2))
    if speed_uncorrected <= 100:
        x_corrected = x
        y_corrected = y
    else:
        x_corrected = 100 * sin(angle)
        y_corrected = 100 * cos(angle)
    return int(x_corrected), int(y_corrected)

def check_joy(x_axis, y_axis):
    # left joystick = (x_axis = 0, y_axis = 1) 
    # right joystick = (x_axis = 2, y_axis = 3) 
    x_val = joystick.get_axis(x_axis)
    y_val = joystick.get_axis(y_axis)
    pygame.event.clear()  
    speed_uncorrected = int(sqrt(x_val**2 + y_val**2)*100)
    speed = min(speed_uncorrected, 100)   # corrects for hardware that isn't a linear radius circle. Constrains to range [0, 100]
    return joystick_angle(x_val, y_val), speed

def analog_trigger(button):
    global trigger_pulled
    raw = joystick.get_axis(button) # value in range [-1, 1]
    val = int((raw+1)*50)           # value in range [0, 100]
    if(val == 50 and trigger_pulled == False):
        return 0                    # on start up the defualt output is: raw = 0, or val = 50, instead of the desired: raw = -1, or val = 0. This is a work around to ensure the scissor doesn't start immediately and waits for the first true user input to start the scissor motor
    else:
        trigger_pulled = True # as soon as (val != 50) we know user has provided the first input and we can disregard safety. the robot is in the users hands!
        return val