import time
import network
from machine import Pin
from utime import sleep
from machine import PWM
from time import sleep_us
from machine import time_pulse_us
from umqttsimple import MQTTClient
import json

time.sleep(0.1) # Wait for USB to become ready

print("Hello, Pi Pico!")

#connect to WIFI
print("Connecting to Wifi", end="")
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
wlan.connect("Wokwi-GUEST", "")
while not wlan.isconnected():
    print(".", end="")
    time.sleep(0.1)
print(wlan.ifconfig())
print("Wifi connected")

#MQTT connection
MQTT_CLIENT_ID = "picow-01"
MQTT_BROKER = "c3a43312019042dd8adba9d73ceaf5dd.s1.eu.hivemq.cloud"
MQTT_USER = "user1"
MQTT_PASSWORD = "Password"
MQTT_TOPIC = "test"
print("Connecting to MQTT server...", end="")
client = MQTTClient(
    client_id = MQTT_CLIENT_ID,
    server = MQTT_BROKER, 
    user = MQTT_USER,
    password = MQTT_PASSWORD,
    keepalive = 7200,
    ssl = True,
    ssl_params={"server_hostname": MQTT_BROKER})
client.connect()
print("MQTT connected!")


trigger = Pin(28, Pin.OUT) #sends signal
echo = Pin(26, Pin.IN) #retrieves signal 

servo = PWM(Pin(11))
servo.freq(50)

def rotate_servo(servo, angle):
  duty = int(1638 + (angle/180.0) * (8192-1638))
  servo.duty_u16(duty)

pos = 0
mqtt_offline_queue = []
a = 0

while True:

    trigger.value(0)
    sleep_us(2)
    trigger.value(1)
    sleep_us(10)
    trigger.value(0)

    #testing the queuing
    if a == 5:
        client.disconnect()
        print("disconnect for testing")
    
    duration = time_pulse_us(echo, 1, 300000)

    if duration > 0:
        distance = duration / 58.2
    else:
        print("cannot read the distance")
    
    if distance < 5:
        pos = 90
    else:
        pos = 0

    rotate_servo(servo, pos)

    #send queued messages
    if mqtt_offline_queue:
        print("Sending queued messages...")
        
        while mqtt_offline_queue:
            try:
                queued_message = mqtt_offline_queue[0]
                client.publish(MQTT_TOPIC, queued_message)
                print("Queued message sent")
                mqtt_offline_queue.pop(0)
            except Exception:
                print("Still offline")
                break

    #write message
    payload = {
        "device": MQTT_CLIENT_ID,
        "distance_cm": round(distance, 1),
        "servo_angle": pos,
        }
    message = json.dumps(payload)

    #try to send messages
    try:
        client.publish(MQTT_TOPIC, message)
        print("MQTT:", message)

    #queue messages if offline
    except Exception as e:
        print("MQTT is offline", e)
        mqtt_offline_queue.append(message)
        try:
            client.connect()
            print("MQTT reconnected!")
        except Exception as e:
            print("Reconnect failed:", e)

    #testing
    a = a+1