#!/usr/bin/env python3
"""
ROS1 伴飞桥节点：订阅 JSON（std_msgs/String），校验 flight_intent v1 后调用 MAVROS 执行。

话题（默认）：
  全局 /input（std_msgs/String），与 rostopic pub、语音端 ROCKET_FLIGHT_BRIDGE_TOPIC 一致。
  可通过私有参数 ~input_topic 覆盖（须带前导 / 才是全局名）。

示例：
  rostopic pub -1 /input std_msgs/String \\
    '{data: "{\"is_flight_intent\":true,\"version\":1,\"actions\":[{\"type\":\"land\",\"args\":{}}],\"summary\":\"降\"}"}'

前提是：已 roslaunch mavros px4.launch …，且 /mavros/state connected。
"""

from __future__ import annotations

import json
import threading

import rospy
from std_msgs.msg import String

from voice_drone.core.flight_intent import parse_flight_intent_dict
from voice_drone.flight_bridge.ros1_mavros_executor import MavrosFlightExecutor


def _coerce_flight_intent_dict(raw: dict) -> dict:
    """允许仅传 {actions, summary?}，补全顶层字段。"""
    if raw.get("is_flight_intent") is True and raw.get("version") == 1:
        return raw
    actions = raw.get("actions")
    if isinstance(actions, list) and actions:
        summary = str(raw.get("summary") or "bridge").strip() or "bridge"
        return {
            "is_flight_intent": True,
            "version": 1,
            "actions": actions,
            "summary": summary,
        }
    raise ValueError("JSON 须为完整 flight_intent 或含 actions 数组")


class FlightIntentBridgeNode:
    def __init__(self) -> None:
        self._exec = MavrosFlightExecutor()
        self._busy = threading.Lock()
        # 默认用绝对名 /input：若用相对名 "input"，anonymous 节点下会变成 /flight_intent_mavros_bridge_*/input，与 rostopic pub /input 不一致。
        topic = rospy.get_param("~input_topic", "/input")
        self._sub = rospy.Subscriber(
            topic,
            String,
            self._on_input,
            queue_size=1,
        )
        rospy.loginfo("flight_intent_bridge 就绪：订阅 %s", topic)

    def _on_input(self, msg: String) -> None:
        data = (msg.data or "").strip()
        if not data:
            return
        if not self._busy.acquire(blocking=False):
            rospy.logwarn("上一段 flight_intent 仍在执行，忽略本条")
            return

        def _run() -> None:
            try:
                try:
                    raw = json.loads(data)
                except json.JSONDecodeError as e:
                    rospy.logerr("JSON 解析失败: %s", e)
                    return
                if not isinstance(raw, dict):
                    rospy.logerr("顶层须为 JSON object")
                    return
                raw = _coerce_flight_intent_dict(raw)
                parsed, errors = parse_flight_intent_dict(raw)
                if errors or parsed is None:
                    rospy.logerr("flight_intent 校验失败: %s", errors)
                    return
                self._exec.execute(parsed)
            finally:
                self._busy.release()

        threading.Thread(target=_run, daemon=True, name="flight-intent-exec").start()


def main() -> None:
    FlightIntentBridgeNode()
    rospy.spin()


if __name__ == "__main__":
    main()