import time
import machine
import network
import utime
from machine import Pin, PWM, time_pulse_us
from umqtt.simple import MQTTClient

"HCSR04, ejemplo de biblioteca"
#Fuente: https://github.com/rsc1975/micropython-hcsr04/blob/master/hcsr04.py
class HCSR04:
    def __init__(self, trigger_pin, echo_pin, echo_timeout_us=500*2*30):
        self.echo_timeout_us = echo_timeout_us
        self.trigger = Pin(trigger_pin, mode=Pin.OUT)
        self.trigger.value(0)
        self.echo = Pin(echo_pin, mode=Pin.IN)

    def _send_pulse_and_wait(self):
        self.trigger.value(0)
        utime.sleep_us(5)
        self.trigger.value(1)
        utime.sleep_us(10)
        self.trigger.value(0)

        try:
            pulse_time = time_pulse_us(self.echo, 1, self.echo_timeout_us)
            if pulse_time < 0:
                return None
            return pulse_time
        except OSError:
            return None

    def distance_cm(self):
        pulse_time = self._send_pulse_and_wait()
        if pulse_time is None:
            return None
        return (pulse_time / 2) / 29.1

"MQTT server y adafruit"
ssid = 'Wokwi-GUEST'
wifipassword = ''
mqtt_server = 'io.adafruit.com'
port = 1883 
user = "kiara123" 
password = "aio_himQ38oJbzgMKeDcen8FfIN1yCaD" 

"Topicos y client ID" 
client_ID = "PuertaGarage" 
topic_DISTANCIA = "kiara123/feeds/topic_DISTANCIA" 
topic_SERVO = "kiara123/feeds/servo_CMD"

"Pines" 
sensor = HCSR04(trigger_pin=6, echo_pin=5)
servo_pwm = PWM(Pin(4), freq=50)

led_rojo = Pin(3, Pin.OUT) 
led_azul = Pin(9, Pin.OUT)

"Variables" 
estado_puerta = 0  
distancia_anterior = 400 

ultimo_print = 0
intervalo_print = 2000  

UMBRAL_ABRIR = 90
UMBRAL_CERRAR = 120 

ultimo_envio_mqtt = 0
intervalo_mqtt = 7000  

"Funciones"
def mover_servo(angulo):
    duty = int((angulo * 6554 / 180) + 1638)
    servo_pwm.duty_u16(duty)

def actualizar_leds():
    led_rojo.value(estado_puerta) 
    led_azul.value(not estado_puerta) 

def callback_mqtt(topic, msg): 
    global estado_puerta
    
    comando = msg.decode('utf-8').upper()
    print("Dashboard envió:", comando)

    if "ON" in comando:
        mover_servo(90)
        estado_puerta = 1

    elif "OFF" in comando:
        mover_servo(0)
        estado_puerta = 0

    actualizar_leds()

"Conectando wifi"
sta_if = network.WLAN(network.STA_IF)
sta_if.active(True)
sta_if.connect(ssid, wifipassword)
print("Conectando WiFi", end="")
while not sta_if.isconnected():
    print(".", end="")
    time.sleep(0.2)
print("\nWiFi Conectado!")

"Conexion MQTT"
try:
    client = MQTTClient(client_ID, mqtt_server, user=user, password=password, port=port)
    client.set_callback(callback_mqtt)
    client.connect()
    client.subscribe(topic_SERVO)
    print("Suscrito a Adafruit IO")
except:
    print("Error MQTT. Reiniciando...")
    time.sleep(5)
    machine.reset()

"Estado inicial"
estado_puerta = 0
mover_servo(0)
actualizar_leds() 
print("\n---\nSistema iniciado: Puerta cerrada")

"Main loop"
while True:
    try:
        client.check_msg() 
        dist = sensor.distance_cm()

        if dist is not None: 
            ahora_print = utime.ticks_ms() 

            if utime.ticks_diff(ahora_print, ultimo_print) > intervalo_print: 
                print("Distancia:", dist)
                ultimo_print = ahora_print 

            if distancia_anterior > UMBRAL_ABRIR and dist <= UMBRAL_ABRIR and estado_puerta == 0:
                mover_servo(90)
                estado_puerta = 1
                actualizar_leds()
                client.publish(topic_SERVO, "ON")

            if distancia_anterior < UMBRAL_CERRAR and dist >= UMBRAL_CERRAR and estado_puerta == 1:
                mover_servo(0)
                estado_puerta = 0
                actualizar_leds()
                client.publish(topic_SERVO, "OFF")

            distancia_anterior = dist 
            ahora = utime.ticks_ms()

            if utime.ticks_diff(ahora, ultimo_envio_mqtt) > intervalo_mqtt:
                print("Enviando distancia:", dist)
                client.publish(topic_DISTANCIA, str(dist))
                ultimo_envio_mqtt = ahora

        time.sleep_ms(200) 

    except Exception as e:
        print("Error:", e)
        time.sleep(5)
        machine.reset()