import network
import time
from machine import Pin,PWM, ADC
from umqtt import MQTTClient

# ========== CONFIG ==========
WIFI_SSID = "Wokwi-GUEST"
WIFI_PASS = ""

MQTT_BROKER = "io.adafruit.com"
MQTT_PORT = 1883
AIO_USERNAME = "dkiranvvit"
AIO_KEY = "aio_OVVt85ybGFsKzE6u9mCcQBrWsUEj"

FEED_SERVO = f"{AIO_USERNAME}/feeds/servo-angle"

# ========== SERVO ==========
servo = PWM(Pin(15))
servo.freq(50)

def set_servo_angle(angle):
    duty = int(1000 + (angle / 180) * 8000)
    servo.duty_u16(duty)
    print("Servo angle set to:", angle)

# ========== POT ==========
pot = ADC(Pin(26))
last_pot_angle = -1

def read_pot_angle():
    raw = pot.read_u16()
    return int((raw / 65535) * 180)

# ========== WIFI ==========
wifi = network.WLAN(network.STA_IF)
wifi.active(True)
wifi.connect(WIFI_SSID, WIFI_PASS)

print("Connecting WiFi...")
while not wifi.isconnected():
    time.sleep(0.2)

print("WiFi Connected")

# ========== MQTT ==========
current_angle = 90   # single source of truth

def on_message(topic, msg):
    global current_angle
    try:
        angle = int(msg)
        if 0 <= angle <= 180:
            current_angle = angle
            set_servo_angle(current_angle)
            print("MQTT → Servo:", angle)
    except:
        print("Invalid MQTT payload")

client = MQTTClient(
    "pico-servo",
    MQTT_BROKER,
    user=AIO_USERNAME,
    password=AIO_KEY,
    port=MQTT_PORT
)

client.set_callback(on_message)
client.connect()
client.subscribe(FEED_SERVO)

print("MQTT Connected")

# ========== MAIN LOOP ==========
while True:
    client.check_msg()

    pot_angle = read_pot_angle()

    # Manual override detection
    if abs(pot_angle - last_pot_angle) > 2:
        current_angle = pot_angle
        set_servo_angle(current_angle)

        # Sync back to Digital Twin
        client.publish(FEED_SERVO, str(current_angle))
        print("Pot → Twin:", current_angle)

        last_pot_angle = pot_angle

    #time.sleep(0.1)