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 = Pin(16, Pin.OUT)
led2 = Pin(4, Pin.OUT)

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

servo1 = Servo(5)
servo2 = Servo(17)
servo3 = Servo(21)
servo4 = Servo(12)
#function to control the motion of servo motors based on direction of joystick
def control_servo(servo_h, servo_v, direction):
    if direction == "L":
        servo_h.left(servo_increment)
    elif direction == "R":
        servo_h.right(servo_increment)
    elif direction == "D":
        servo_v.right(servo_increment)
    elif direction == "U":
        servo_v.left(servo_increment)
#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 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

    if sw_val == 1: #activating servos based on switch state
        led1.on()
        led2.off()
        control_servo(servo3, servo1, joystick_direction)
    else:
        led1.off()
        led2.on()
        control_servo(servo4, servo2, joystick_direction)

    sleep(0.05)