# SCRIPT A: MPU6050 NODE
import network, time, machine, ustruct
from umqtt.simple import MQTTClient

# --- CONFIG ---
SSID, PASS = "Wokwi-GUEST", ""
BROKER = "broker.hivemq.com"
CLIENT_ID = "UiTM_Node_A_MPU"
TOPIC_PUB = b"uitm/class/mpu"
TOPIC_SUB = b"uitm/class/control"

# --- HARDWARE ---
i2c = machine.I2C(0, scl=machine.Pin(22), sda=machine.Pin(21))
mpu_addr = 0x68
i2c.writeto_mem(mpu_addr, 0x6B, b'\x00') # Wake up MPU6050

led_green = machine.Pin(18, machine.Pin.OUT)
led_red = machine.Pin(19, machine.Pin.OUT)
led_green.on() # Default: Operation Normal

# --- MQTT CALLBACK (Recieve Command from Cloud) ---
def sub_cb(topic, msg):
    print(f"Command received: {msg}")
    if msg == b"HALT":
        led_green.off()
        led_red.on()  # RED = STOP
    elif msg == b"GO":
        led_green.on()
        led_red.off() # GREEN = GO

# --- SETUP ---
print("Connecting Wi-Fi...")
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
wlan.connect(SSID, PASS)
while not wlan.isconnected(): time.sleep(0.5)

print("Connecting MQTT...")
client = MQTTClient(CLIENT_ID, BROKER)
client.set_callback(sub_cb)
client.connect()
client.subscribe(TOPIC_SUB)
print("Node A Ready.")

# --- MAIN LOOP ---
while True:
    client.check_msg() # Check for "HALT" commands
    
    # Read Accel X (Register 0x3B)
    data = i2c.readfrom_mem(mpu_addr, 0x3B, 2)
    accel_x = ustruct.unpack(">h", data)[0] / 16384.0 # Convert to G-force
    
    msg = str(accel_x)
    client.publish(TOPIC_PUB, msg)
    print(f"Sent Accel: {msg}")
    
    time.sleep(2)