from machine import Pin, PWM, ADC, Timer
import time

# PWM frequency and duty cycle ranges
freq = 50
duty_center = 77  # Center position of the servos
duty_min = 40     # Minimum position (left)
duty_max = 115    # Maximum position (right)

# Create PWM objects and set frequency
servo_x = PWM(Pin(14), freq=freq)  # Servo controlled by X axis
servo_y = PWM(Pin(18), freq=freq)  # Servo controlled by Y axis (adjust pin number as needed)

# Create ADC objects for X and Y axes
vrx = ADC(Pin(34))
vry = ADC(Pin(35))
vrx.atten(ADC.ATTN_11DB)
vry.atten(ADC.ATTN_11DB)

# Define global condition flag for system activity
system_active = True

# Define ISR for button press
def button_isr(pin):
    global system_active
    # Toggle the system_active flag
    system_active = not system_active

# Create a Pin object for the button with a pull-down resistor
button_pin = Pin(2, Pin.IN, Pin.PULL_DOWN)
# Attach an interrupt to the button pin for the falling edge (button press)
button_pin.irq(trigger=Pin.IRQ_FALLING, handler=button_isr)

# Create Pin objects for LEDs
led_left = Pin(12, Pin.OUT)  # LED for left movement (adjust pin number as needed)
led_right = Pin(13, Pin.OUT)  # LED for right movement (adjust pin number as needed)

# Define timers for blinking LEDs
timer_left = Timer(0)  # Timer for left LED
timer_right = Timer(1)  # Timer for right LED

# Define LED blink interval (in milliseconds)
blink_interval = 100

# Function to toggle the state of the left LED
def blink_left(timer):
    led_left.value(not led_left.value())

# Function to toggle the state of the right LED
def blink_right(timer):
    led_right.value(not led_right.value())

# Define function to adjust servo duty cycle incrementally
def adjust_servo_duty(servo, current_duty, target_duty, step=1):
    if current_duty < target_duty:
        current_duty = min(current_duty + step, target_duty)
    elif current_duty > target_duty:
        current_duty = max(current_duty - step, target_duty)
    servo.duty(current_duty)
    return current_duty

# Initialize current duty cycle variables for the servos
current_duty_x = duty_center
current_duty_y = duty_center

while True:
    # Read ADC values
    valorx = vrx.read()
    valory = vry.read()
    
    # If the system is active, control the servos and LEDs based on ADC values
    if system_active:
        # Determine target duty cycles for the servos
        if valorx < 1900:
            # Move servo to the left
            target_duty_x = duty_min
            timer_left.init(period=blink_interval, mode=Timer.PERIODIC, callback=blink_left)
            timer_right.deinit()
        elif valorx > 2200:
            # Move servo to the right
            target_duty_x = duty_max
            timer_right.init(period=blink_interval, mode=Timer.PERIODIC, callback=blink_right)
            timer_left.deinit()
        else:
            # Servo is centered
            target_duty_x = current_duty_x  # Keep last known position
            timer_left.deinit()
            timer_right.deinit()
            led_left.off()
            led_right.off()

        if valory < 1900:
            # Move servo down
            target_duty_y = duty_min
        elif valory > 2200:
            # Move servo up
            target_duty_y = duty_max
        else:
            # Servo is centered
            target_duty_y = current_duty_y  # Keep last known position

        # Adjust the duty cycles of the servos incrementally
        current_duty_x = adjust_servo_duty(servo_x, current_duty_x, target_duty_x, step=2)
        current_duty_y = adjust_servo_duty(servo_y, current_duty_y, target_duty_y, step=2)
    else:
        # System is inactive; keep the servos in their last known positions
        # No adjustments are made here, allowing the servos to maintain their last positions
        # Servo is centered
        servo_x.duty(duty_center)
        servo_y.duty(duty_center)
        # Stop blinking both LEDs
        timer_left.deinit()
        timer_right.deinit()
        led_left.off()
        led_right.off()
    # Delay
    time.sleep_ms(100)