import time
from machine import Pin, ADC, Timer
from joystick import Joystick
from servo import Servo
gain = 1
prev_angle1 = 90
prev_angle2 = 90
prev_angle3 = 90
prev_angle4 = 90
servo1 = Servo(23)
servo2 = Servo(22)
servo3 = Servo(21)
servo4 = Servo(19)
switch_pin = Pin(34, Pin.IN)
led1 = Pin(26, Pin.OUT, Pin.PULL_DOWN)
led2 = Pin(27, Pin.OUT, Pin.PULL_DOWN)
while True:
  controller = Joystick(12, 13, "polling", 14)
  lista = controller.read()
  controller_state = lista[0]
  button_state = lista[1]
  print("Controller state: ",controller_state, " || Button State:",button_state)
  if button_state == 0:
    resp = input("Please type a number from 1 to 10 to set as the gain, the current gain is " + str(gain) + "\n")
    while True:
        try:
            if 1 <= int(resp) <= 10:
                gain = int(resp)
                break
            else:
                print("Incorrect input, please type a number from 1 to 10")
        except ValueError:
            print("Incorrect input, please type a number from 1 to 10")
        resp = input()
  if switch_pin.value() == 1:
    ident = "first"
    led1.on()
    led2.off()
    updownservo = servo1
    leftrightservo = servo2
  else:
    ident = "second"
    led1.off()
    led2.on()
    updownservo = servo3
    leftrightservo = servo4

  if controller_state == "L":
    if ident  == "first":
        prev_angle2 = leftrightservo.left(prev_angle2, gain)
    else:
        prev_angle4 = leftrightservo.left(prev_angle4, gain)
  elif controller_state == "R":
    if ident  == "first":
        prev_angle2 = leftrightservo.right(prev_angle2, gain)
    else:
        prev_angle4 = leftrightservo.right(prev_angle4, gain)
  elif controller_state == "U":
    if ident == "first":
        prev_angle1 = updownservo.left(prev_angle1, gain)
    else:
        prev_angle3 = updownservo.left(prev_angle3, gain)
  elif controller_state == "D":
    if ident == "first":
        prev_angle1 = updownservo.right(prev_angle1, gain)
    else:
        prev_angle3 = updownservo.right(prev_angle3, gain)
  else:
    pass
  time.sleep(0.1)