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

PWM_FREQ = 50  

camera_1pan = Servo(25)
camera_1tilt = Servo(26)
camera_2pan = Servo(27)
camera_2tilt = Servo(14)

joystick = Joystick(34, 35, 13, mode="interrupt")  

slide_switch = Pin(32, Pin.IN, Pin.PULL_UP)  
led_cam1 = Pin(15, Pin.OUT)  
led_cam2 = Pin(33, Pin.OUT)  

gain = 1  
current_angle_x = 90  
current_angle_y = 90 
DEAD_ZONE = 0.1  

previous_switch_state = slide_switch.value()  

def get_gain_from_user():
    global gain
    while True:
        try:
            print("Enter a gain value (1 Min. or 10 Max.):")
            user_input = input().strip()  
            gain = int(user_input)  
            
            if 1 <= gain <= 10:
                print(f"Gain set to {gain}.")
                return  
            else:
                print("Invalid gain. Please enter a value between 1 and 10.")
        except ValueError:
            print("Invalid input. Please enter a numeric value between 1 and 10.")

def move_servos():
    global gain, current_angle_x, current_angle_y, previous_switch_state
    
    joystick_state, button_state = joystick.read() 

    if button_state == 0:  
        print("Joystick button pressed. Requesting gain input...")
        get_gain_from_user() 

    switch_state = slide_switch.value()

    if switch_state != previous_switch_state:
        print("Camera switched! Resetting angles.")
        current_angle_x = 90
        current_angle_y = 90
        previous_switch_state = switch_state  

    x_pos = joystick.joystick_x.read()  
    y_pos = joystick.joystick_y.read() 

    delta_x = (x_pos - 2047) / 2047  
    delta_y = (y_pos - 2047) / 2047  

    if abs(delta_x) < DEAD_ZONE:
        delta_x = 0
    if abs(delta_y) < DEAD_ZONE:
        delta_y = 0

    current_angle_x += delta_x * gain
    current_angle_y += delta_y * gain

    current_angle_x = max(0, min(180, current_angle_x))
    current_angle_y = max(0, min(180, current_angle_y))

    if switch_state == 1:  # Camera 1 selected
        camera_1pan.goto(int(current_angle_x))
        camera_1tilt.goto(int(current_angle_y))
        led_cam1.value(1)
        led_cam2.value(0)
    else:  # Camera 2 selected
        camera_2pan.goto(int(current_angle_x))
        camera_2tilt.goto(int(current_angle_y))
        led_cam1.value(0)
        led_cam2.value(1)

while True:
    move_servos()
    sleep(0.05)