import network
import ujson
from machine import Pin
from time import sleep
from umqtt.simple import MQTTClient
import time

queue = []

def mqtt_message(topic, msg):
    try:
        msg = ujson.loads(msg)
        print(msg)
        distance = msg.get('Distance')
        light_id = msg.get("ID")
        print("Distance:", distance)
        if distance < 60:
            if light_id not in queue:
                queue.append(light_id)
    except Exception as e:
        print("Error:", e)

wifi = None

def connect_wifi():
    global wifi
    print("Connecting to WiFi...", end="")
    wifi = network.WLAN(network.STA_IF)
    wifi.active(True)
    wifi.connect("Wokwi-GUEST", "")
    while not wifi.isconnected():
        sleep(0.5)
        print(".", end="")
    print("Done")

print("Connecting to WiFi...")
connect_wifi()

print("Connecting to MQTT...")
client = MQTTClient("TrafficLightController", "mqtt-dashboard.com")
client.set_callback(mqtt_message)
client.connect()
client.subscribe("light-0")
client.subscribe("light-1")
client.subscribe("light-2")
client.subscribe("light-3")
print("Connected!")

on_off = "on"
active_light_id = None
active_time = 5
start_time = time.time()

while True:
    if not wifi.isconnected():
        connect_wifi()

    client.check_msg()
    client.check_msg()
    client.check_msg()
    client.check_msg()

    time.sleep(1)
    print("Queue:", queue)
    
    if active_light_id is None:
        start_time = time.time()
        if len(queue) == 0:
            active_light_id = 0    
        else:
            active_light_id = queue.pop(0)  # Use pop(0) to get the first element in the queue
            print("Pop from queue")

    if (time.time() - start_time) >= active_time:
        start_time = time.time()
        if len(queue) == 0:
            active_light_id = (active_light_id + 1) % 4
            start_time = time.time()
        else:
            active_light_id = queue.pop(0)  # Use pop(0) to get the first element in the queue
            print("Pop from queue (time)")

    message = ujson.dumps({
        "ACTIVE_LIGHT_ID": active_light_id,
    })
    print("Publishing message:", ujson.loads(message))
    client.publish("controller", message)