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

switch = Pin(23, Pin.IN)
firstled = Pin(22, Pin.OUT)
secondled = Pin(21, Pin.OUT)
firstservo = Servo(19, Pin.OUT)
secondservo = Servo(18, Pin.OUT)
thirdservo = Servo(5, Pin.OUT) 
fourthservo = Servo(17, Pin.OUT)

servo_motion_step = 1

joystick = Joystick (32, 35, 'interrupt', 34)

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

while True:
    direction, press = joystick.read()                                                                                                                                                                                                                                                                                                                                                                                                                                     
    print("button state and direction", (direction, press))

    if press:
        message = ""
        while correct_message(message) is None:
            message = input("Enter Servo Speed from 1 ---> 10:")
        servo_motion_step = correct_message(message)

    if switch.value() == 1:
        firstled.on()
        secondled.off()
        if direction == "U":
            secondservo.left(servo_motion_step)
        elif direction == "D":  
            secondservo.right(servo_motion_step)  
        elif direction == "L":  
            firstservo.left(servo_motion_step)  
        elif direction == "R":  
            firstservo.right(servo_motion_step)     

    if switch.value() == 0:
        firstled.off()  
        secondled.on()  
        if direction == "U":  
            fourthservo.left(servo_motion_step)  
        elif direction == "D":  
            fourthservo.right(servo_motion_step)  
        elif direction == "L":  
            thirdservo.left(servo_motion_step)  
        elif direction == "R":  
            thirdservo.right(servo_motion_step)  
sleep(0.1)