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

# Initialize components
switch = Pin(2, Pin.IN)  # Slide switch (GPIO 2)
led1 = Pin(16, Pin.OUT)  # LED1 (GPIO 16)
led2 = Pin(4, Pin.OUT)  # LED2 (GPIO 4)
joystick = Joystick(33, 32, 35)  # Joystick (X=33, Y=32, Button=35)
servo1 = Servo(5)  # Servo 1 (GPIO 5)
servo2 = Servo(17)  # Servo 2 (GPIO 17)
servo3 = Servo(21)  # Servo 3 (GPIO 21)
servo4 = Servo(12)  # Servo 4 (GPIO 12)

gain = 1  # Default gain value
servo_increment = 5  # Default increment for servo movement
button_pressed = False  # Flag to track if the button has been pressed

def prompt_gain():
    """
    Prompt the user for a gain value via Serial Terminal.
    """
    global gain, button_pressed
    while True:
        try:
            print("Enter gain value (1-10):")
            input_str = input().strip()
            gain = int(input_str)
            if 1 <= gain <= 10:
                print(f"Gain set to {gain}")
                button_pressed = False  # Reset the button press flag
                break
            else:
                print("Invalid input. Please enter a value between 1 and 10.")
        except ValueError:
            print("Invalid input. Please enter a numeric value.")

def update_leds(switch_value):
    """
    Update LEDs based on the slide switch value.
    """
    led1.value(switch_value)
    led2.value(not switch_value)

def move_servos(direction):
    """
    Move servos based on the joystick direction and gain.
    """
    angle_delta = gain * servo_increment  # Adjust movement speed based on gain
    if direction == 'L':
        servo1.left(angle_delta)
        servo3.left(angle_delta)
    elif direction == 'R':
        servo1.right(angle_delta)
        servo3.right(angle_delta)
    elif direction == 'U':
        servo2.left(angle_delta)
        servo4.left(angle_delta)
    elif direction == 'D':
        servo2.right(angle_delta)
        servo4.right(angle_delta)

# Main loop
while True:
    # Check if the joystick button is pressed
    if joystick.is_button_pressed() and not button_pressed:
        button_pressed = True  # Set the button press flag
        prompt_gain()  # Ask for gain value

    # Read slide switch and update LEDs
    switch_value = switch.value()
    update_leds(switch_value)

    # Read joystick position and move servos
    joystick_position = joystick.read()
    if joystick_position != 'M':  # Ignore middle position
        move_servos(joystick_position)

    time.sleep(0.1)  # Debounce delay