#!/usr/bin/env python3
import rospy
import paho.mqtt.client as mqtt
from std_msgs.msg import String
import pkg_resources
import json

# 获取 paho-mqtt 版本
try:
    PAHO_VERSION = pkg_resources.get_distribution("paho-mqtt").version
    PAHO_MAJOR_VERSION = int(PAHO_VERSION.split('.')[0])
except Exception:
    PAHO_VERSION = "unknown"
    PAHO_MAJOR_VERSION = 1

# MQTT 配置
MQTT_BROKER = "211.154.252.186"    # MQTT 代理地址
MQTT_PORT = 1883             # MQTT 代理端口
MQTT_CLIENT_ID = "ros_mqtt_bridge"
MQTT_TOPIC_CONTROL = "head_servo/control/1"  # 接收控制指令的 MQTT 主题
# MQTT_TOPIC_CONTROL = "robot/yuntai_control/+"
MQTT_TOPIC_STATUS = "head_servo/status"    # 发布状态的 MQTT 主题

# ROS 配置
ROS_TOPIC_CONTROL = "/head_servo/control_cmd"  # 发布控制指令的 ROS 话题
ROS_TOPIC_STATUS = "/head_servo/status"        # 接收状态的 ROS 话题

class MQTTROSBridge:
    def __init__(self):
        # 初始化 ROS 节点
        rospy.init_node("mqtt_ros_bridge", anonymous=True)
        rospy.loginfo(f"MQTT-ROS 桥接器启动中 (paho-mqtt 版本: {PAHO_VERSION})")
        
        # 根据版本选择初始化方式
        if PAHO_MAJOR_VERSION >= 2:
            # 版本 2.0+ 使用新 API
            self.mqtt_client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2, clean_session=True)
        else:
            # 版本 1.x 使用旧 API
            self.mqtt_client = mqtt.Client(client_id=MQTT_CLIENT_ID)
            
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_message = self.on_message
        
        # 连接 MQTT 代理
        try:
            self.mqtt_client.connect(MQTT_BROKER, MQTT_PORT, 60)
            self.mqtt_client.loop_start()  # 启动 MQTT 网络循环（非阻塞）
            rospy.loginfo(f"已连接到 MQTT 代理: {MQTT_BROKER}:{MQTT_PORT}")
        except Exception as e:
            rospy.logerr(f"MQTT 连接失败: {e}")
            exit(1)
        
        # 创建 ROS 发布者和订阅者
        self.control_pub = rospy.Publisher(ROS_TOPIC_CONTROL, String, queue_size=10)
        rospy.Subscriber(ROS_TOPIC_STATUS, String, self.status_callback)
        
        rospy.loginfo("MQTT-ROS 桥接器已就绪")
    
    def on_connect(self, client, userdata, flags, rc, properties):
        """MQTT 连接成功回调"""
        rospy.loginfo(f"MQTT 连接成功，返回代码: {rc}")
        # 订阅 MQTT 控制主题
        client.subscribe(MQTT_TOPIC_CONTROL)
        rospy.loginfo(f"已订阅 MQTT 主题: {MQTT_TOPIC_CONTROL}")
    
    # def on_message(self, client, userdata, msg):
    #     """MQTT 消息到达回调"""
    #     try:
    #         # 解析 MQTT 消息
    #         payload = msg.payload.decode()
    #         rospy.loginfo(f"收到 MQTT 消息: [{msg.topic}] {payload}")
    #         # 将消息发布到 ROS 话题
    #         ros_msg = String()
    #         ros_msg.data = payload
    #         self.control_pub.publish(ros_msg)
            
    #     except Exception as e:
    #         rospy.logerr(f"处理 MQTT 消息时出错: {e}")

    # 修改 MQTT-ROS 桥接脚本中的消息处理部分
    def on_message(self, client, userdata, msg):
        try:
            payload = msg.payload.decode()
            rospy.loginfo(f"收到 MQTT 消息: [{msg.topic}] {payload}")
            
            # 解析 JSON 命令
            try:
                cmd_data = json.loads(payload)
                ros_msg = String()
                ros_msg.data = json.dumps(cmd_data)  # 保持 JSON 格式
                self.control_pub.publish(ros_msg)
            except json.JSONDecodeError:
                # 如果不是 JSON 格式，作为普通字符串处理
                ros_msg = String()
                ros_msg.data = payload
                self.control_pub.publish(ros_msg)
                
        except Exception as e:
            rospy.logerr(f"处理 MQTT 消息时出错: {e}")


    
    def status_callback(self, msg):
        """ROS 状态消息回调"""
        try:
            # 将 ROS 状态消息发布到 MQTT 主题
            self.mqtt_client.publish(MQTT_TOPIC_STATUS, msg.data)
            rospy.loginfo(f"已发布到 MQTT 主题 [{MQTT_TOPIC_STATUS}]: {msg.data}")
        except Exception as e:
            rospy.logerr(f"发布到 MQTT 时出错: {e}")
    
    def run(self):
        """运行桥接器"""
        rospy.spin()  # 进入 ROS 循环
        # 关闭 MQTT 连接
        self.mqtt_client.loop_stop()
        self.mqtt_client.disconnect()
        rospy.loginfo("桥接器已关闭")

if __name__ == "__main__":
    try:
        bridge = MQTTROSBridge()
        bridge.run()
    except rospy.ROSInterruptException:
        pass