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

slide_switch = Pin(12,Pin.IN)
led1 = Pin(16,Pin.OUT,value = 0)
led2 = Pin(17,Pin.OUT,value = 0)

servo1 = Servo(4)
servo2 = Servo(0)
servo3 = Servo(2)
servo4 = Servo(15)

servo_motion_step = 1
my_joystick = Joystick(35,33,'interrupt',14)

def validate_message(message: str):
    try:
        num = int(message)
        if 1 <= num <= 10:
            return num
    except ValueError:
        pass
    return None


while True:
    direction,press = my_joystick.read()
    print('Current Direction and button state is ',(direction,press))
    if press:
        message = ''
        while validate_message(message) is None:
            message = input('Please Enter Servo gain 1-10 :')
        servo_motion_step = validate_message(message)
    if slide_switch.value() == 1:
        led1.on()
        led2.off()
        if direction == 'U':
            servo2.left(servo_motion_step)
        elif direction == 'D':
            servo2.right(servo_motion_step)
        elif direction == 'L':
            servo1.left(servo_motion_step)
        elif direction == 'R':
            servo1.right(servo_motion_step)
    else:
        led1.off()
        led2.on()
        if direction == 'U':
            servo4.left(servo_motion_step)
        elif direction == 'D':
            servo4.right(servo_motion_step)
        elif direction == 'L':
            servo3.left(servo_motion_step)
        elif direction == 'R':
            servo3.right(servo_motion_step)
    sleep(0.1)