from machine import Pin, ADC, PWM, time_pulse_us
from time import sleep, sleep_us

# Joystick
joy_x = ADC(Pin(26))
joy_y = ADC(Pin(27))
joy_sw = Pin(15, Pin.IN, Pin.PULL_UP)

# LED
led = Pin(20, Pin.OUT)
ultimo_estado = 1
estado_led = 0

# Ultrasonic
trig = Pin(18, Pin.OUT)
echo = Pin(19, Pin.IN)

# Servos
servo_horizontal = PWM(Pin(16))
servo_vertical = PWM(Pin(17))
servo_horizontal.freq(50)
servo_vertical.freq(50)

# Initial servo positions
horizontal_angle = 90
vertical_angle = 90

# Joystick center and dead zone
CENTER = 32768
DEADZONE = 6000
STEP = 3   # bigger number = faster movement


def map_value(value, in_min, in_max, out_min, out_max):
    return int((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)


def set_servo_angle(servo, angle):
    angle = max(0, min(180, angle))

    min_duty = 1638   # approx 0.5 ms
    max_duty = 8192   # approx 2.5 ms

    duty = map_value(angle, 0, 180, min_duty, max_duty)
    servo.duty_u16(duty)


def get_distance():
    trig.low()
    sleep_us(2)

    trig.high()
    sleep_us(10)
    trig.low()

    duration = time_pulse_us(echo, 1, 30000)

    if duration < 0:
        return -1

    return duration * 0.034 / 2


while True:
    # -------------------------
    # Button and LED
    # -------------------------
    estado_boton = joy_sw.value()

    if estado_boton == 0 and ultimo_estado == 1:
        estado_led = 1 - estado_led
        led.value(estado_led)
        sleep(0.05)

    ultimo_estado = estado_boton

    # -------------------------
    # Read distance
    # -------------------------
    distance = get_distance()
    print("Distance:", distance, "cm")

    # -------------------------
    # Read joystick
    # -------------------------
    x_value = joy_x.read_u16()
    y_value = joy_y.read_u16()

    # -------------------------
    # Horizontal servo always works
    # -------------------------
    if x_value > CENTER + DEADZONE:
        horizontal_angle += STEP      # joystick right
    elif x_value < CENTER - DEADZONE:
        horizontal_angle -= STEP      # joystick left

    horizontal_angle = max(0, min(180, horizontal_angle))
    set_servo_angle(servo_horizontal, horizontal_angle)

    # -------------------------
    # Vertical servo stops only if object <= 10 cm
    # -------------------------
    if distance > 0 and distance <= 10:
        print("STOP - Object too close! Vertical servo stopped.")

    else:
        if y_value > CENTER + DEADZONE:
            vertical_angle += STEP    # joystick up
        elif y_value < CENTER - DEADZONE:
            vertical_angle -= STEP    # joystick down

        vertical_angle = max(0, min(180, vertical_angle))
        set_servo_angle(servo_vertical, vertical_angle)

    print("Horizontal:", horizontal_angle, "Vertical:", vertical_angle)

    sleep(0.05)