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

BROKER_IP = "0.0.0.0"  # se a base RODA o broker, tanto faz; para conectar, use "127.0.0.1"
RID = "rover01"
TOP_TEL = f"agrobot/{RID}/telemetry"
TOP_CMD = f"agrobot/{RID}/cmd"

last_t = None

def on_connect(c, u, f, rc):
    print("Base conectada, rc=", rc)
    c.subscribe(TOP_TEL, qos=0)

def on_message(c, u, msg):
    global last_t
    if msg.topic == TOP_TEL:
        tel = json.loads(msg.payload.decode("utf-8"))
        now = time.time()
        if "t" in tel:
            rtt_ms = (now - tel["t"]) * 1000
        else:
            rtt_ms = None
        print(f"[TEL] vel={tel.get('vel')}, heading={tel.get('heading')}, rtt≈{rtt_ms:.1f} ms")

client = mqtt.Client(client_id="base01", clean_session=True)
client.on_connect = on_connect
client.on_message = on_message
client.connect("127.0.0.1", 1883, keepalive=60)  # se a base é o broker; senão use o IP do broker
client.loop_start()

# Envia um comando binário exemplo (QoS 1)
time.sleep(1.0)
vel, ang = 0.5, 10.0
pkt = struct.pack("<ff", vel, ang)
client.publish(TOP_CMD, pkt, qos=1, retain=False)
print("[BASE] Comando enviado")

try:
    while True:
        time.sleep(0.5)
except KeyboardInterrupt:
    pass
finally:
    client.loop_stop()
    client.disconnect()