from machine import Pin, I2C, PWM, time_pulse_us
import machine
import ssd1306
import time
import ubinascii
import network
from umqtt.simple import MQTTClient

SSID = "Wokwi-GUEST"
PASSWORD = ""

# ตั้งค่า MQTT Broker (HiveMQ หรือ Local Broker)
MQTT_BROKER = "broker.hivemq.com"  # ใช้ HiveMQ
MQTT_PORT = 1883
MQTT_TOPIC1 = b"wokwi/test1"
MQTT_TOPIC2 = b"wokwi/test2"
MQTT_TOPIC_SW = b"wokwi/test/sw"
MQTT_TOPIC_RED = b"wokwi/test/red1"
MQTT_TOPIC_YELLOW = b"wokwi/test/yellow1"
MQTT_TOPIC_GREEN = b"wokwi/test/green1"
MQTT_TOPIC_DISPLAY = b"wokwi/test/display1"

#set up
red = Pin(42, Pin.OUT)
yellow = Pin(41, Pin.OUT)
green = Pin(40, Pin.OUT)
sw = Pin(2, Pin.IN, Pin.PULL_UP)
i2c = I2C(0, scl=Pin(47), sda=Pin(48))
display = ssd1306.SSD1306_I2C(128, 64, i2c)
trig = Pin(4, Pin.OUT)
echo = Pin(12, Pin.IN)
buzzer = PWM(Pin(5))
buzzer.freq(1000) 
client_id = ubinascii.hexlify(machine.unique_id()).decode()  # ใช้ ID ของอุปกรณ์
mqtt = MQTTClient(client_id, MQTT_BROKER, MQTT_PORT)




def connect_wifi():
    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    wlan.connect(SSID, PASSWORD)
    
    while not wlan.isconnected():
        print("Connecting to WiFi...")
        time.sleep(1)

    print("WiFi Connected:", wlan.ifconfig())

# เชื่อมต่อ MQTT
def connect_mqtt():
    mqtt.connect()
    mqtt.set_callback(mqtt_callback)  # กำหนด Callback
    mqtt.subscribe(MQTT_TOPIC1) 
    mqtt.subscribe(MQTT_TOPIC2)
    mqtt.subscribe(MQTT_TOPIC_DISPLAY)
    mqtt.subscribe(MQTT_TOPIC_RED)
    mqtt.subscribe(MQTT_TOPIC_YELLOW)
    mqtt.subscribe(MQTT_TOPIC_GREEN)
    print("Connected to MQTT Broker")
    

# ฟังก์ชัน Callback เมื่อได้รับข้อความจาก MQTT
def mqtt_callback(topic, payload):
    if topic.decode() == MQTT_TOPIC2:
        try:
            if int(payload) <= 100 and dist <= 100:
                green.value(1)
            elif int(payload) > 100 and dist > 100:
                green.value(0)
                red.value(1)
        except ValueError:
            pass
    elif topic.decode() == MQTT_TOPIC_RED:
        try:
            red.value(int(payload))
        except ValueError:
            pass
    elif topic.decode() == MQTT_TOPIC_YELLOW:
        try:
            yellow.value(int(payload))
        except ValueError:
            pass
    elif topic.decode() == MQTT_TOPIC_GREEN:
        try:
            green.value(int(payload))
        except ValueError:
            pass


buzzer.duty_u16(0)
connect_wifi()
connect_mqtt()
buzzer.duty_u16(0)
def measure_distance():
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    duration = time_pulse_us(echo, 1, 30000)
    distance = (duration * 0.0343) / 2
    return distance

def clear_LED():
    red.value(0)
    yellow.value(0)
    green.value(0)
    display.fill(0)

count = 5
traf = 0
dist = measure_distance()
while True:
    mqtt.check_msg()

    buzzer.duty_u16(0)
    clear_LED()
    print(f"Distance: {dist:.2f} cm")
    mqtt.publish(MQTT_TOPIC1, str(dist))
    if dist <= 100:
        traf = 0   
        if count != 0:  
            display.text("You want cross?",0 ,0)
            if sw.value() == 0 :
                mqtt.publish(MQTT_TOPIC_SW, '1')
                count = 0
                display.fill(0)
                display.text("GO", 50, 25)
                green.value(1)
                buzzer.duty_u16(30000)
                time.sleep_ms(500)  # เล่นเสียงตามระยะเวลา
                buzzer.duty_u16(0)
            else:
                pass
        elif count == 0:
            green.value(1)
            display.fill(0)
            display.text("GO", 50, 25)
            
    else:
        mqtt.publish(MQTT_TOPIC_SW, '0')
        count = 2
        buzzer.duty_u16(0)
        if traf == 0:
            green.value(0)
            yellow.value(1)
            traf += 1
        else:
            yellow.value(0)
            red.value(1)
    time.sleep(1)
    display.show()