from camera import CameraController
from machine import Pin, ADC, PWM, UART
from joystick import Joystick
from pins import ReadingMode
from servo import Servo
import utime
DEBOUNCE_DUR = 300


def build_camera_controller(pan_servo_pin, tilt_servo_pin, joystick):
    pan_servo = Servo(pan_servo_pin)
    tilt_servo = Servo(tilt_servo_pin) 
    return CameraController(pan_servo, tilt_servo, joystick)

def get_selected():
    return slide.read() < 4096 // 2


def read_gain():
    
    gain = input("Please enter gain: ")
    while True:
        try:
            gain = int(gain)

            if gain > 10 or gain < 0:
                gain = input("gain must be between 0 and 10, please enter gain: ")
            else : return gain

        except ValueError:
            gain = input("Invalid number please enter gain: ")
gain = 1 

def update_gain():
    global gain
    gain = read_gain()
    print("gain is now {}".format(gain))
    
was_joystick_pressed = False
joystick = Joystick(34,35,18,mode = ReadingMode.INTERRUPT)


camera_controllers = [
    build_camera_controller(16,4,joystick),
    build_camera_controller(5,17,joystick), 
]

leds = [
    Pin(23, Pin.OUT),
    Pin(22, Pin. OUT),
]



slide = ADC(Pin(33, Pin.IN))

print("System started")

while True:
    selected_idx = int(get_selected())
    selected_camera_controller = camera_controllers[selected_idx]
    selected_led = leds[selected_idx]

    is_joystick_pressed = joystick.is_pressed()

    if is_joystick_pressed and not was_joystick_pressed:
        update_gain()
    
    was_joystick_pressed = is_joystick_pressed


    for led in leds: led.value(0)
    selected_led(1)
    selected_camera_controller.update(gain)