#!/usr/bin/env python3
import json
import time
import threading
import paho.mqtt.client as mqtt
from datetime import datetime

# ----------------- 公共 MQTT 配置 -----------------
MQTT_BROKER = "192.168.189.97"      # 换成实际 IP
MQTT_PORT   = 1883
KEEPALIVE   = 60
QOS         = 1

# -------------------------------------------------
# 1. order 指令模板（发往 uagv/v2/SEER/{sn}/order）
# -------------------------------------------------
ORDER_TMPL = {
    "headerId": 103852,
    "timestamp": "",
    "version": "2.0.0",
    "manufacturer": "SEER",
    "serialNumber": "ROHDL04",
    "orderId": "1972692682446491649",
    "orderUpdateId": 81076,
    "nodes": [
        {"nodeId": "LM128", "nodeDescription": "LM128", "released": True,
         "nodePosition": {"x": -24.226, "y": 1.071, "mapId": "ROH1122_0804"}},
        {"nodeId": "LM129", "nodeDescription": "LM129", "released": True,
         "nodePosition": {"x": -29.226, "y": 1.071, "mapId": "ROH1122_0804"}},
        {"nodeId": "LM108", "nodeDescription": "LM108", "released": True,
         "nodePosition": {"x": -34.221, "y": 1.093, "mapId": "ROH1122_0804"}}
    ],
    "edges": [
        {"edgeId": "LM128-LM129", "sequenceId": 216377, "released": True,
         "startNodeId": "LM128", "endNodeId": "LM129", "rotationAllowed": False,
         "trajectory": {"degree": 1.0, "knotVector": [0, 0, 1, 1],
                        "controlPoints": [{"x": -25.89, "y": 1.071},
                                          {"x": -27.559, "y": 1.071}]}},
        {"edgeId": "LM129-LM108", "sequenceId": 216378,
         "startNodeId": "LM129", "endNodeId": "LM108", "rotationAllowed": False}
    ]
}
ORDER_TOPIC = f"oagv/v2/asbm2_IRAYPLE/{ORDER_TMPL['serialNumber']}/order"

# -------------------------------------------------
# 2. instantActions 指令模板（发往 oagv/v2/1_VWED/{sn}/instantActions）
# -------------------------------------------------
INSTANT_TMPL = {
    "headerId": 103854,
    "timestamp": "",
    "version": "2.0.0",
    "serialNumber": "172.31.57.151-10067-17",
    "instantActions": [{
        "actionType": "writeValue",
        "actionId": "2fea9ad2-277f-4db3-b81d-d85454af111a",
        "actionDescription": "action parameters",
        "blockingType": "HARD",
        "actionParameters": [
            {"key": "registerName", "value": {"string_value": "set"}},
            {"key": "command",      "value": {"string_value": "cmd:close"}}
        ]
    }]
}
INSTANT_TOPIC = f"oagv/v2/asbm2_VWED/{INSTANT_TMPL['serialNumber']}/instantActions"

# -------------------------------------------------
# 工具：获取当前时间字符串
# -------------------------------------------------
def now_str():
    return datetime.utcnow().strftime("%Y-%m-%d %H:%M:%S")

# -------------------------------------------------
# 线程函数：循环发 order
# -------------------------------------------------
def publish_order(client):
    while True:
        try:
            ORDER_TMPL["timestamp"] = now_str()
            payload = json.dumps(ORDER_TMPL, separators=(',', ':'))
            client.publish(ORDER_TOPIC, payload, qos=QOS)
            print(f"[ORDER] {now_str()} -> {ORDER_TOPIC}")
        except Exception as e:
            print(f"[ORDER-ERROR] {e}")
        time.sleep(0.5)

# -------------------------------------------------
# 线程函数：循环发 instantActions
# -------------------------------------------------
def publish_instant(client):
    while True:
        try:
            INSTANT_TMPL["timestamp"] = now_str()
            payload = json.dumps(INSTANT_TMPL, separators=(',', ':'))
            client.publish(INSTANT_TOPIC, payload, qos=QOS)
            print(f"[INSTANT] {now_str()} -> {INSTANT_TOPIC}")
        except Exception as e:
            print(f"[INSTANT-ERROR] {e}")
        time.sleep(0.5)

# -------------------------------------------------
# 主入口
# -------------------------------------------------
def main():
    client = mqtt.Client()
    client.connect(MQTT_BROKER, MQTT_PORT, KEEPALIVE)
    client.loop_start()

    # threading.Thread(target=publish_order,   args=(client,), daemon=True).start()
    threading.Thread(target=publish_instant, args=(client,), daemon=True).start()

    # 主线程保持存活
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print("--- 用户中断 ---")
    finally:
        client.loop_stop()
        client.disconnect()

if __name__ == "__main__":
    main()