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

horz = 14 
vert = 12  
btnPin = 27    
btn_pin = Pin(27, Pin.IN)
joystick = Joystick(horz, vert, btnPin, mode="interrupt")

slide_switch = Pin(5, Pin.IN)


led1 = Pin(16, Pin.OUT)
led2 = Pin(4, Pin.OUT)


servo_1_1 = Servo(19)
servo_1_2 = Servo(18)

servo_2_1 = Servo(0)
servo_2_2 = Servo(2)

gain = 1

def get_gain():
    global gain
    
    if btn_pin.value() == 0:  
        
        while True:
            try:
                gain = int(input("Enter gain value (1-10): "))
                if 1 <= gain <= 10:
                    print(f"Gain set to: {gain}")
                    return gain
                    break
                else:
                    print("Invalid input. Please enter a number between 1 and 10.")
            except:
                print("Invalid input. Please enter a number between 1 and 10.")

def move_servo():
    global slide_switch
    switch_val = slide_switch.value()
    if switch_val == 1:  # servo_1_1 (L, R) and servo_1_2 (U, D)
        led1.on()
        led2.off()
        direction = joystick.read()
        if direction == 'L':
            servo_1_1.left(gain)
        elif direction == 'R':
            servo_1_1.right(gain)
        elif direction == 'U':
            servo_1_2.left(gain)
        elif direction == 'D':
            servo_1_2.right(gain)
    elif switch_val == 0:  # servo_2_1 (L, R) and servo_2_2 (U, D)
        led1.off()
        led2.on()
        direction = joystick.read()
        if direction == 'L':
            servo_2_1.left(gain)
        elif direction == 'R':
            servo_2_1.right(gain)
        elif direction == 'U':
            servo_2_2.left(gain)
        elif direction == 'D':
            servo_2_2.right(gain)


print("Program Started")
 
def main():

    while True:
        get_gain()
        move_servo()

if __name__ == main():
    
    print("Program Started")