from machine import Pin
from time import sleep
from joystick import Joystick
from servo import Servo



# l = left, r = right, d = down, u = up
l_d_servo = Servo(25)
l_u_servo = Servo(26)
r_d_servo = Servo(16)
r_u_servo = Servo(17)

switch = Pin(22, Pin.IN)

#       Right LED       , Left LED
leds = [Pin(19, Pin.OUT), Pin(32, Pin.OUT)]

l_d_servo.goto(90)
l_u_servo.goto(90)
r_d_servo.goto(90)
r_u_servo.goto(90)

print("Program has started.")


# Choose a mode: polling or interrupt
mode = ""

while mode != "interrupt" and mode != "polling":
    mode = input("Please choose a mode: polling or interrupt: ")

js = Joystick(33, 34, 12, mode)

while True:

    leds[switch.value()].on()
    leds[1-switch.value()].off()

    # js.read()[0] reads vertical state, js.read()[1] reads horizontal state, js.read()[2] reads gain

    if switch.value() == 1:
        if js.read()[0] == "L":
            l_u_servo.left(js.read()[2])
        elif js.read()[0] == "R":
            l_u_servo.right(js.read()[2])
        if js.read()[1] == "D":
            l_d_servo.left(js.read()[2])
        elif js.read()[1] == "U":
            l_d_servo.right(js.read()[2])
    else:
        if js.read()[0] == "L":
            r_u_servo.left(js.read()[2])
        elif js.read()[0] == "R":
            r_u_servo.right(js.read()[2])
        if js.read()[1] == "D":
            r_d_servo.left(js.read()[2])
        elif js.read()[1] == "U":
            r_d_servo.right(js.read()[2])

    sleep(0.03)