import sys
from machine import Pin, ADC, PWM, Timer
from time import sleep
from joystick import Joystick
from servo import Servo

# Initialize components
joystick = Joystick(vert_pin=34, hor_pin=32, button_pin=35, mode="interrupt")
slide_switch = Pin(33, Pin.IN)
led1 = Pin(26, Pin.OUT)
led2 = Pin(27, Pin.OUT)
servo1 = Servo(21)
servo2 = Servo(5)
servo3 = Servo(17)
servo4 = Servo(16)

gain = 1  # Default gain
joystick_status = "M"  # Initialize joystick status

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

def joystick_interrupt(timer):
    global joystick_status
    joystick_status = joystick.read()

# Use interrupt mode for joystick
timer = Timer(-1)
timer.init(period=50, mode=Timer.PERIODIC, callback=joystick_interrupt)
prev_button_state = 0
while True:
    # Check if the button is pressed using the button attribute
    if  joystick.button.value() and not prev_button_state:  # Active low
        get_gain()  # Ask user for gain when button is pressed
        prev_button_state = 1
    elif not joystick.button.value():  # Reset the state when the button is released
        prev_button_state = 0
    
    # Determine which servos to control based on the slide switch
    if slide_switch.value() == 1:
        led1.value(1)  # Indicate first pair of servos
        led2.value(0)
        if joystick_status == "L":
            servo1.left(gain)
        elif joystick_status == "R":
            servo1.right(gain)
        elif joystick_status == "U":
            servo2.left(gain)
        elif joystick_status == "D":
            servo2.right(gain)
    else:
        led1.value(0)
        led2.value(1)  # Indicate second pair of servos
        if joystick_status == "L":
            servo3.left(gain)
        elif joystick_status == "R":
            servo3.right(gain)
        elif joystick_status == "U":
            servo4.left(gain)
        elif joystick_status == "D":
            servo4.right(gain)
    
    sleep(0.1)