from time import sleep
from machine import Pin

from joystick import Joystick
from servo import Servo
steps_of_servo = 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)



def move_s(horizontal_s, vertical_s, direction):
    if direction == "L":
        horizontal_s.left(steps_of_servo)
    if direction == "R":
        horizontal_s.right(steps_of_servo)
    if direction == "D":
        vertical_s.right(steps_of_servo)
    if direction == "U":
        vertical_s.left(steps_of_servo)
    

def gain():
    while True:
        try:
            userInput = int(input("Input gain value from 1 to 10: "))
            if 1 <= userInput <= 10:
                return userInput
            else:
                print(" Invalid input Please enter a value between 1- 10")
        except ValueError:
            print("Invalid value, please enter an integer")




last_press_state = 0
while True:
    joystick_direction, joystick_button = joystick.read()
    switch_value = switch.value()
    if joystick_button and not last_press_state:
        steps_of_servo = gain()
    last_press_state = joystick_button

    if switch_value == 1:
        led1.on()
        led2.off()
        move_s(servo3, servo1, joystick_direction)
    else:
        led1.off()
        led2.on()
        move_s(servo4, servo2, joystick_direction)

    sleep(0.05)