import time
from machine import Pin, PWM, reset
import network
from simple import MQTTClient, MQTTException
from hcsr04 import HCSR04

SSID = 'Wokwi-GUEST'
PASSWORD = ''
BROKER = 'broker.hivemq.com'
CLIENT_ID = 'smartdustbinumithasan'
TOPIC_FILL = b"smartbinumhas/fill"
TOPIC_INTERACT = b"smartbinumhas/interact"

WIFI_RETRY_DELAY_S = 5
MQTT_RETRY_DELAY_S = 10

wlan = network.WLAN(network.STA_IF)
client = None

bin_sensor = HCSR04(trigger_pin=2, echo_pin=3)
person_sensor = HCSR04(trigger_pin=4, echo_pin=5)
led = Pin(15, Pin.OUT)
servo = PWM(Pin(14))
servo.freq(50)

def set_servo(angle):
    min_duty_u16 = 1638
    max_duty_u16 = 8192
    duty = int(min_duty_u16 + (angle / 180.0) * (max_duty_u16 - min_duty_u16))
    servo.duty_u16(duty)
set_servo(90)

bin_full = False
person_nearby = False
lid_is_actually_open = False
last_person_time = 0
LID_AUTO_CLOSE_DELAY_MS = 3000

def on_message(topic, msg):
    global bin_full, person_nearby
    topic_str, msg_str = "", ""
    try:
        topic_str = topic.decode('utf-8')
        msg_str = msg.decode('utf-8')
    except UnicodeError:
        print(f"UnicodeError. Topic: {topic}, Msg: {msg}")
        return
    print(f"Message on '{topic_str}': '{msg_str}'")
    if topic == TOPIC_FILL:
        try:
            fill = int(msg_str)
            bin_full = fill >= 90
        except ValueError:
            print(f"Invalid fill: '{msg_str}'")
    elif topic == TOPIC_INTERACT:
        try:
            person_nearby = bool(int(msg_str))
        except ValueError:
            print(f"Invalid interact: '{msg_str}'")

def connect_wifi_startup():
    print("Attempting to connect to Wi-Fi (will retry indefinitely)...")
    wlan.active(True)
    attempt = 0
    while not wlan.isconnected():
        attempt += 1
        print(f"Wi-Fi connection attempt {attempt}...")
        try:
            if not wlan.isconnected():
                 wlan.connect(SSID, PASSWORD)
            time.sleep(WIFI_RETRY_DELAY_S)
        except OSError as e:
            print(f"OSError during Wi-Fi connect: {e}")
            wlan.disconnect()
            wlan.active(False)
            time.sleep(1)
            wlan.active(True)
            time.sleep(WIFI_RETRY_DELAY_S) # Extra delay after OSError

    print("Wi-Fi Connected:", wlan.ifconfig())

def connect_mqtt_startup():
    global client
    print("Attempting to connect to MQTT broker (will retry indefinitely)...")
    attempt = 0
    while True:
        attempt += 1
        print(f"MQTT connection attempt {attempt}...")
        led.toggle()
        try:
            client = MQTTClient(CLIENT_ID, BROKER, keepalive=60)
            client.set_callback(on_message)
            client.connect()
            print("Connected to MQTT broker.")
            client.subscribe(TOPIC_FILL)
            client.subscribe(TOPIC_INTERACT)
            print(f"Subscribed to topics.")
            return
        except OSError as e:
            print(f"MQTT OSError: {e}")
        except Exception as e:
            print(f"Unexpected error connecting MQTT: {e}")

        # Clean up before retry if connection object exists
        if client and client.sock:
            try:
                client.disconnect()
            except Exception:
                pass
        client = None

        time.sleep(MQTT_RETRY_DELAY_S)


connect_wifi_startup()
connect_mqtt_startup()

BIN_HEIGHT_CM = 50.0
PERSON_DIST_CM = 50.0
last_fill_publish_ms = 0
last_person_publish_ms = 0
PUBLISH_FILL_INTERVAL_MS = 5000
PUBLISH_PERSON_INTERVAL_MS = 2000

print("Starting main loop...")
loop_count = 0
while True:
    now_ms = time.ticks_ms()
    loop_count +=1
    try:
        # Sensor Service: Bin Fill Percent
        if time.ticks_diff(now_ms, last_fill_publish_ms) > PUBLISH_FILL_INTERVAL_MS:
            dist = bin_sensor.distance_cm()
            if dist is not None and dist < BIN_HEIGHT_CM * 1.5 :
                fill_level = int(max(0, min((1 - dist / BIN_HEIGHT_CM) * 100, 100)))
                client.publish(TOPIC_FILL, str(fill_level))
            else:
                print("Bin sensor distance outside acceptable values, check sensor.")
            last_fill_publish_ms = now_ms

        # Sensor Service: Person Detection
        if time.ticks_diff(now_ms, last_person_publish_ms) > PUBLISH_PERSON_INTERVAL_MS:
            p_dist = person_sensor.distance_cm()
            if p_dist is not None:
                interact = 1 if p_dist < PERSON_DIST_CM else 0
                client.publish(TOPIC_INTERACT, str(interact))
            last_person_publish_ms = now_ms

        client.check_msg()

        # LED Control Logic
        if bin_full:
            led.value(1)
        else:
            led.value(0)

        # Lid Control Logic
        if person_nearby:
            last_person_time = now_ms
            if not bin_full:
                if not lid_is_actually_open:
                    set_servo(0)
                    lid_is_actually_open = True
            else:
                if lid_is_actually_open:
                    set_servo(90)
                    lid_is_actually_open = False
        else:
            if lid_is_actually_open:
                if time.ticks_diff(now_ms, last_person_time) > LID_AUTO_CLOSE_DELAY_MS:
                    set_servo(90)
                    lid_is_actually_open = False

        time.sleep(0.1)

    except MQTTException as e:
        print(f"MQTTException occurred: {e}")
        print("Reestablishing MQTT Connection")

        client = None
        connect_mqtt_startup()

    except Exception as e:
        print(e.__class__.__name__)
        print(f"Unexpected error in main loop: {e}")
        print("Rebooting in 20 seconds...")

        time.sleep(20)
        reset()