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

# Joystick Setup
joystick = Joystick(34, 35, 32, "interrupt")

# Slide Switch and LED Setup
switch = Pin(21, Pin.IN)
led1 = Pin(22, Pin.OUT)
led2 = Pin(23, Pin.OUT)

# Servo Setup
servo1 = Servo(25)  # Camera 1 - Pan
servo2 = Servo(26)  # Camera 1 - Tilt
servo3 = Servo(27)  # Camera 2 - Pan
servo4 = Servo(14)  # Camera 2 - Tilt

gain = 1

def get_gain():
    while True:
        try:
            value = int(input("Enter gain (1-10): "))
            if 1 <= value <= 10:
                return value
            else:
                print("Invalid input. Enter a number between 1 and 10.")
        except ValueError:
            print("Invalid input. Enter a number between 1 and 10.")

while True:
    direction = joystick.read()
    cam_selected = switch.value()

    if cam_selected:
        led1.on()
        led2.off()
    else:
        led1.off()
        led2.on()

    if joystick.buttonPressed():
        gain = get_gain()

    if cam_selected:
        if direction == 'L':
            print("1")
            servo1.left(gain)
        elif direction == 'R':
            print("2")
            servo1.right(gain)
        elif direction == 'U':
            print("3")
            servo2.left(gain)
        elif direction == 'D':
            print("4")
            servo2.right(gain)
    else:
        if direction == 'L':
            servo3.left(gain)
        elif direction == 'R':
            servo3.right(gain)
        elif direction == 'U':
            servo4.left(gain)
        elif direction == 'D':
            servo4.right(gain)

    time.sleep(0.1)