from machine import Pin, PWM
import machine
import time
import sys
import uselect

# ── Configuración del servo ──────────────────────────────────────────────────
PIN_SERVO     = 14
PULSO_CERRADO = 1500   # duty_u16 ≈ 0.46 ms — calibra si tu servo no llega al tope
PULSO_ABIERTO = 8000   # duty_u16 ≈ 2.44 ms — calibra si tu servo no llega al tope
PASO_VELOCIDAD = 50    # incremento por paso (más pequeño = movimiento más suave)
TIEMPO_ESPERA  = 0.02  # segundos entre pasos

# ── Configuración del sensor ultrasónico HC-SR04 ─────────────────────────────
PIN_TRIG        = 16
PIN_ECHO        = 17
DISTANCIA_MIN   = 20.0   # cm — detiene el cierre si hay algo más cerca
DISTANCIA_ERROR = 400.0  # cm — valor de retorno cuando el pulso falla

# ── Configuración del botón ──────────────────────────────────────────────────
PIN_BOTON   = 15
DEBOUNCE_MS = 50

# ── Inicialización del hardware ──────────────────────────────────────────────
servo = PWM(Pin(PIN_SERVO))
servo.freq(50)

trig  = Pin(PIN_TRIG, Pin.OUT)
echo  = Pin(PIN_ECHO, Pin.IN)
boton = Pin(PIN_BOTON, Pin.IN, Pin.PULL_UP)

# ── Estado global ────────────────────────────────────────────────────────────
posicion_actual = PULSO_CERRADO
servo.duty_u16(posicion_actual)
estado_porton   = "CERRADO"   # CERRADO | ABIERTO | ABRIENDO | CERRANDO
poll_stdin = uselect.poll()
poll_stdin.register(sys.stdin, uselect.POLLIN)


# ── Funciones ────────────────────────────────────────────────────────────────

def medir_distancia() -> float:
    """Dispara el HC-SR04 y devuelve la distancia en cm."""
    trig.value(0)
    time.sleep_us(2)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    duracion = machine.time_pulse_us(echo, 1, 30_000)  # timeout 30 ms ≈ 5 m

    if duracion < 0:
        return DISTANCIA_ERROR

    return (duracion * 0.0343) / 2


def abrir_porton() -> None:
    """Mueve el servo desde la posición actual hasta PULSO_ABIERTO."""
    global estado_porton, posicion_actual

    print("Abriendo portón...")
    estado_porton = "ABRIENDO"

    for pulso in range(posicion_actual, PULSO_ABIERTO + 1, PASO_VELOCIDAD):
        servo.duty_u16(pulso)
        posicion_actual = pulso
        time.sleep(TIEMPO_ESPERA)

    # Asegura que llega exactamente al valor final sin importar el paso
    posicion_actual = PULSO_ABIERTO
    servo.duty_u16(posicion_actual)

    estado_porton = "ABIERTO"
    print("Portón ABIERTO.")


def cerrar_porton() -> None:
    """
    Mueve el servo hacia PULSO_CERRADO.
    Si detecta un obstáculo a menos de DISTANCIA_MIN cm, reabre el portón.
    """
    global estado_porton, posicion_actual

    print("Cerrando portón...")
    estado_porton = "CERRANDO"

    for pulso in range(posicion_actual, PULSO_CERRADO - 1, -PASO_VELOCIDAD):
        distancia = medir_distancia()

        if distancia < DISTANCIA_MIN:
            print(f"¡ALERTA! Obstáculo a {distancia:.1f} cm — reabriendo portón.")
            time.sleep(0.5)
            abrir_porton()
            return

        servo.duty_u16(pulso)
        posicion_actual = pulso
        time.sleep(TIEMPO_ESPERA)

    # Asegura que llega exactamente al valor final sin importar el paso
    posicion_actual = PULSO_CERRADO
    servo.duty_u16(posicion_actual)

    estado_porton = "CERRADO"
    print("Portón CERRADO.")


def procesar_comando(cmd: str) -> None:
    """
    Interpreta un comando recibido por UART/USB.
    Comandos válidos: ABRIR | CERRAR | ESTADO
    """
    cmd = cmd.upper()

    if cmd == "ABRIR":
        if estado_porton == "CERRADO":
            abrir_porton()
        else:
            print(f"Ignorado: el portón ya está en estado '{estado_porton}'.")

    elif cmd == "CERRAR":
        if estado_porton == "ABIERTO":
            cerrar_porton()
        else:
            print(f"Ignorado: el portón ya está en estado '{estado_porton}'.")

    elif cmd == "ESTADO":
        distancia = medir_distancia()
        print(
            f"Estado: {estado_porton} | "
            f"Distancia: {distancia:.1f} cm | "
            f"Posición servo: {posicion_actual}"
        )

    else:
        print(f"Comando desconocido: '{cmd}'. Usa ABRIR, CERRAR o ESTADO.")


# ── Programa principal ────────────────────────────────────────────────────────

print("Sistema de garage listo. Esperando órdenes...")

while True:
    # — Botón físico —
    if boton.value() == 0:
        time.sleep_ms(DEBOUNCE_MS)           # debounce: filtra rebotes
        if boton.value() == 0:               # confirmar pulsación real
            if estado_porton == "CERRADO":
                abrir_porton()
            elif estado_porton == "ABIERTO":
                cerrar_porton()

            while boton.value() == 0:        # esperar a que lo suelte
                time.sleep_ms(DEBOUNCE_MS)
                # — Comandos por UART/USB —
    if poll_stdin.poll(0):          # 0 = no bloquea, revisa y sigue
        cmd = sys.stdin.readline().strip()
        if cmd:
            procesar_comando(cmd)
    time.sleep_ms(10)   # cede CPU y evita busy-loop