from machine import Pin, PWM
import utime

# ---------------- CONFIG ----------------

led = Pin(15, Pin.OUT)
buzzer = Pin(14, Pin.OUT)

servo = PWM(Pin(13))
servo.freq(50)

trigger = Pin(2, Pin.OUT)
echo = Pin(1, Pin.IN)

# Posiciones del servo
POS_SMALL = 2000
POS_MEDIUM = 4000
POS_LARGE = 6000
POS_HOME = 4000

current_position = POS_HOME
servo.duty_u16(current_position)

# ---------------- FUNCIONES ----------------

def mover_suave(destino):
    global current_position
    
    paso = 50
    
    if destino > current_position:
        while current_position < destino:
            current_position += paso
            servo.duty_u16(current_position)
            utime.sleep_ms(10)
    else:
        while current_position > destino:
            current_position -= paso
            servo.duty_u16(current_position)
            utime.sleep_ms(10)

def beep():
    buzzer.value(1)
    utime.sleep_ms(100)
    buzzer.value(0)

def obtener_distancia():
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(10)
    trigger.low()

    timeout = utime.ticks_us()
    while echo.value() == 0:
        if utime.ticks_diff(utime.ticks_us(), timeout) > 30000:
            return -1

    inicio = utime.ticks_us()
    timeout = utime.ticks_us()

    while echo.value() == 1:
        if utime.ticks_diff(utime.ticks_us(), timeout) > 30000:
            return -1

    fin = utime.ticks_us()

    duracion = utime.ticks_diff(fin, inicio)
    distancia = (duracion * 0.0343) / 2
    return distancia

# ---------------- SISTEMA PRINCIPAL ----------------

print("Sistema de Clasificación Automática Iniciado")

while True:
    d = obtener_distancia()

    if d > 0 and d < 30:  # Solo actúa si hay objeto cercano
        print("Distancia:", round(d,2), "cm")

        led.value(1)
        beep()

        if d < 10:
            print("Objeto PEQUEÑO")
            mover_suave(POS_SMALL)

        elif d < 20:
            print("Objeto MEDIANO")
            mover_suave(POS_MEDIUM)

        else:
            print("Objeto GRANDE")
            mover_suave(POS_LARGE)

        # Espera antes de regresar
        utime.sleep(2)

        print("Regresando a posición inicial...")
        mover_suave(POS_HOME)

        led.value(0)

        utime.sleep(1)  # Pequeña pausa antes de siguiente lectura

    utime.sleep(0.2)