import time
import machine
import ubinascii
import network
import ujson
from servo import Servo
from hcsr04 import HCSR04
from umqtt.simple import MQTTClient

#Sensor HC-SR04 Config
sensor = HCSR04(trigger_pin=4, echo_pin=5, echo_timeout_us=10000)

# Servo config
motor=Servo(pin=2)

# WiFi Parameters
WIFI_SSID = 'Wokwi-GUEST'
WIFI_PASSWORD = ''

print("Ket noi den WiFi", end="")
sta_if = network.WLAN(network.STA_IF)
sta_if.active(True)
sta_if.connect(WIFI_SSID, WIFI_PASSWORD)

# Timeout for WiFi connection
timeout = 10  # seconds
start_time = time.time()

while not sta_if.isconnected() and (time.time() - start_time) < timeout:
    print(".", end="")
    time.sleep(0.5)

if sta_if.isconnected():
    print("\nKet noi WiFi thanh cong!")
else:
    print("\nKhong the ket noi den WiFi. Kiem tra lai thong tin!")
    machine.reset()

# MQTT Server Parameters
MQTT_CLIENT_ID = f"TNam123-{ubinascii.hexlify(machine.unique_id()).decode()}"
MQTT_SERVER = "mqtt-dashboard.com"
MQTT_TOPIC = ""

print("Ket noi toi MQTT server... ", end="")
client = MQTTClient(MQTT_CLIENT_ID, MQTT_SERVER)

# Ham reconnect mqtt
def mqtt_reconnect():
    while True:
        try:
            client.connect()
            print("Ket noi lai MQTT thanh cong!")
            break
        except Exception as e:
            print(f"Loi ket noi MQTT: {e}, thu lai sau 5 giay...")
            time.sleep(5)

# Thong bao mqtt
try:
    client.connect()
    print("Ket noi thanh cong!")
except Exception as e:
    print(f"Khong the ket noi MQTT server: {e}")
    machine.reset()


while True:
    distance = sensor.distance_cm()
    messageEcho = ujson.dumps(
    distance,
    )
    try:
        # Doc khoang cach
        print(f"Khoang cach: {distance:.2f} cm")

        #Quay servo
        motor.move(0) # tourne le servo à 0°
        time.sleep(0.3)
        motor.move(30) # tourne le servo à 90°
        time.sleep(0.3)
        motor.move(180) # tourne le servo à 180°
        time.sleep(0.3)
        motor.move(90) # tourne le servo à 90°
        time.sleep(0.3)
        motor.move(30) # tourne le servo à 0°
        time.sleep(0.3)
        
        try:
            # Gui du lieu den MQTT
            client.publish("echo", messageEcho)
            print(f"Da gui: {messageEcho}")
        except EOFError as e:
            print(f"Khong gui duoc du lieu den MQTT: {e}")

    except OSError as e:
        print(f"Loi do cam bien: {e}")

    except Exception as e:
        print(f"Loi khac: {e}")
        mqtt_reconnect()

    # Delay between measurements
    time.sleep(5)