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

# LEDs
ledr = Pin(15, Pin.OUT)   # Rojo
leda = Pin(14, Pin.OUT)   # Amarillo
ledv = Pin(13, Pin.OUT)   # Verde

# Parada de emergencia: botón a GND + Pull-Up interno
emergencia = Pin(12, Pin.IN, Pin.PULL_DOWN)


# Sensor ultrasónico
trig = Pin(11, Pin.OUT)
echo = Pin(10, Pin.IN)


paro_activo = False          # Estado del sistema
estado_anterior = 0


# Servo
servo = PWM(Pin(27))
servo.freq(50)  # 50 Hz


def mover_servo(angulo):
    """Mueve el servo entre 0 y 180 grados."""

    duty_min = 1000
    duty_max = 9000

    duty = int(1638 + (angulo / 2000) * (65535))
    servo.duty_u16(duty)

def medir_distancia():
    """Devuelve distancia en cm, o None si falla/timeout."""
    trig.low()
    sleep_us(2)
    trig.high()
    sleep_us(10)
    trig.low()

    try:
        duracion = time_pulse_us(echo, 1, 30000)  # 30ms timeout
        if duracion < 0:
            return None
        return duracion / 58.0  # cm aprox en HC-SR04
    except:
        return None



while True:


    # -------- EMERGENCIA CON TOGGLE --------
    estado_actual = emergencia.value()

    # Detectar pulsado (flanco ascendente)
    if estado_actual == 1 and estado_anterior == 0:
        paro_activo = not paro_activo    # Cambia de estado
        print("SISTEMA:", "DESACTIVADO" if paro_activo else "ACTIVADO")
        sleep(0.2)  # Antirebote

    estado_anterior = estado_actual

    # -------- PARO ACTIVO --------
    if paro_activo:
        ledr.value(0)
        leda.value(0)
        ledv.value(0)
        mover_servo(0)
        sleep(0.2)
        continue
             


    distancia = medir_distancia()

    if distancia <= 80:
        ledr.value (1)
        leda.value (0)
        ledv.value (0)
        mover_servo(0)

        print ("PELIGRO |", distancia, "cm")

    elif distancia <= 200:
        ledr.value (0)
        leda.value (1)
        ledv.value (0)
        mover_servo(90+10)

        print ("PPRECAUCION |", distancia, "cm")

    else:
        ledr.value (0)
        leda.value (0)
        ledv.value (1)
        mover_servo(180+10)

        print ("SEGURO |", distancia, "cm")
        
    
        
        sleep (0.3)