# rover.py
import time, json, struct
import paho.mqtt.client as mqtt

BROKER_IP = "192.168.99.120"   # IP da Base
RID = "rover01"
TOP_TEL = f"agrobot/{RID}/telemetry"
TOP_CMD = f"agrobot/{RID}/cmd"
TOP_HEALTH = f"agrobot/{RID}/health"
TOP_LWT = f"agrobot/{RID}/lwt"

def on_connect(c, u, f, rc):
    print("Rover conectado, rc=", rc)
    c.subscribe(TOP_CMD, qos=1)
    # Publica estado online (retido)
    c.publish(TOP_HEALTH, json.dumps({"status":"online","t":time.time()}), qos=1, retain=True)

def on_message(c, u, msg):
    if msg.topic == TOP_CMD:
        # Exemplo de payload binário: vel (float32), ang (float32)
        vel, ang = struct.unpack("<ff", msg.payload)
        print(f"[CMD] vel={vel:.2f} m/s, ang={ang:.2f}°")

client = mqtt.Client(client_id=f"{RID}", clean_session=True)
client.will_set(TOP_LWT, payload="offline", qos=1, retain=True)  # LWT
client.on_connect = on_connect
client.on_message = on_message
client.connect(BROKER_IP, 1883, keepalive=60)
client.loop_start()

try:
    i = 0
    while True:
        # Telemetria json (poderia ser binário também)
        tel = {"t": time.time(), "heading": 123.45+i*0.01, "vel": 1.6}
        client.publish(TOP_TEL, json.dumps(tel), qos=0, retain=False)
        i += 1
        time.sleep(0.2)
except KeyboardInterrupt:
    pass
finally:
    client.loop_stop()
    client.disconnect()