from time import sleep
from machine import Pin
from joystick import Joystick
from servo import Servo
#pins definition\ constants
servo_increment = 1
switch = Pin(2, Pin.IN)
led1, led2 = Pin(16, Pin.OUT, value = 0),  Pin(4, Pin.OUT, value = 0)


joystick = Joystick(33, 32, 'interrupt', 35) #joystick object definition

#servo objects 
servo1, servo2, servo3, servo4 = Servo(5), Servo(17), Servo(21), Servo(12)

#function to control the motion of servo motors based on direction of joystick
def control_servo(servo_h, servo_v, direction):
    moving_directions = {
        "L": lambda : servo_h.left(servo_increment),
        "R": lambda : servo_h.right(servo_increment),
        "D": lambda : servo_v.right(servo_increment),
        "U": lambda : servo_v.left(servo_increment)


    }
    moving_directions.get(direction, lambda: None)() 
    """
    returns none if direction key isnt found 
    and if found it returns the compatible lambda function with direction
    """


#Function to get input from user and check if the input is within the range of 1-10
def gain():
    while True:
        try:
            userInput = int(input("Please enter gain value from 1 to 10: "))
            if 1 <= userInput <= 10:
                return userInput
            else:
                print("Invalid input, enter a value between 1-10")
        except ValueError:
            print("Invalid value, please enter an integer")

last_press_state = 0 # variable to store last button press state in order not to ask for input twice or more
while True:
    joystick_direction, joystick_btn = joystick.read()
    sw_val = switch.value()
    if joystick_btn and not last_press_state:
        servo_increment = gain()
    last_press_state = joystick_btn

    # control leds based on switch reading
    led1_state, led2_state = (led1.on, led2.off) if sw_val ==1 else (led1.off, led2.on)
    led1_state()
    led2_state()
    # determine servos to move based on switch reading
    activated_servos = (servo3, servo1) if sw_val ==1 else( servo4, servo2)
    control_servo(*activated_servos, joystick_direction) 
    """
    unpack the tuple(activated_servos) and assign its values to the function control_servos
    """
    sleep(0.05)