import network
import ujson
from time import sleep
from umqtt.simple import MQTTClient
from machine import Pin, PWM, I2C
from lcd_api import LcdApi
from i2c_lcd import I2cLcd

# Servo setup on pin 13, using PWM
servoPin = 13
servo = PWM(Pin(servoPin), freq=50)  # Servo operates at 50Hz

# LCD setup (I2C)
I2C_ADDR = 0x27  # I2C address of the LCD
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=400000)  # Setup I2C on ESP32
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)  # 16x2 LCD

# MQTT Topic for publishing messages
MQTT_TOPIC = "wokwi-servo"
message = ""

# Function to set the servo angle and update the LCD
def set_angle(angle):
    # Convert angle to duty cycle (for 180-degree servo)
    # duty = int(((angle / 180) * (1023-20)) + 20)
    duty = int(((angle / 180) *(1023/8)) +20)
    servo.duty(duty)
    
    # Create a JSON message to publish after updating the angle
    message = ujson.dumps({
        "angle": angle,
        "status": "changed",
    })
    
    # Display angle on LCD
    lcd.clear()
    lcd.putstr("Angle: {} deg".format(angle))
    
    # Publish message to MQTT broker if there's an update
    if message != "":
        print("Updated!")
        print("Reporting to MQTT topic {}: {}".format(MQTT_TOPIC, message))
        client.publish(MQTT_TOPIC, message)
        sleep(1)

# Function to handle incoming MQTT messages
def mqtt_message(topic, msg):
    print("Incoming message:", msg)
    try:
        # Decode the JSON message and set the servo angle
        msg = ujson.loads(msg)
        set_angle(int((msg[0])))
    except Exception as e:
        print("Error:", e)

# Connecting to Wi-Fi (Wokwi's guest Wi-Fi)
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")

# Connecting to the HiveMQ MQTT broker
print("Connecting to MQTT...")
client = MQTTClient("wokwi1", "broker.hivemq.com")
client.set_callback(mqtt_message)
client.connect()
client.subscribe("servoDegrees")  # Subscribe to the topic for controlling the servo
print("Connected!")

# Initialize LCD display
lcd.clear()
lcd.putstr("Waiting for MQTT")

# Main loop to wait for incoming MQTT messages
while True:
    client.wait_msg()