#!/usr/bin/env python3
"""
配置管理节点 (Config Node)
从 MySQL 读取配置并发布到 /ai/config topic

功能:
1. 从数据库读取配置（当前阶段使用模拟数据）
2. 定时刷新配置
3. 发布配置到 /ai/config topic (JSON 格式)

Author: sunrise
"""

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import json
import time
import yaml
import os
from datetime import datetime
from ament_index_python.packages import get_package_share_directory


class ConfigNode(Node):
    """
    配置管理节点

    从 MySQL 读取配置，定时发布到 /ai/config topic
    当前阶段使用模拟数据，后续接入 MySQL
    """

    def __init__(self):
        super().__init__('config_node')

        # ========== 参数声明 ==========
        self.declare_parameter('config_file', '')
        self.declare_parameter('refresh_interval', 1.0)  # 秒
        self.declare_parameter('use_mock_data', True)  # 使用模拟数据
        self.declare_parameter('use_yaml_fallback', True)  # 使用 YAML fallback
        self.declare_parameter('topic_name', '/ai/config')

        # 获取参数
        pkg_share = get_package_share_directory('config_node')
        default_config_file = os.path.join(pkg_share, 'config', 'database.yaml')
        self.config_file = self.get_parameter('config_file').value or default_config_file
        self.refresh_interval = self.get_parameter('refresh_interval').value
        self.use_mock_data = self.get_parameter('use_mock_data').value
        self.use_yaml_fallback = self.get_parameter('use_yaml_fallback').value
        self.topic_name = self.get_parameter('topic_name').value

        # ========== 数据库连接管理器 (预留) ==========
        self.db_manager = None
        self.db_config = {}

        # ========== 配置状态 ==========
        self.current_config = {}
        self.config_version = 0
        self.last_update_time = None

        # ========== 加载配置 ==========
        self.load_database_config()
        self.load_config()

        # ========== Publisher ==========
        self.config_publisher = self.create_publisher(
            String,
            self.topic_name,
            10
        )

        # ========== 定时器 (定时发布配置) ==========
        self.timer = self.create_timer(
            self.refresh_interval,
            self.timer_callback
        )

        # ========== 初始化时立即发布一次 ==========
        self.publish_config()

        self.get_logger().info('=' * 60)
        self.get_logger().info('Config Node 启动成功')
        self.get_logger().info(f'配置文件: {self.config_file}')
        self.get_logger().info(f'刷新间隔: {self.refresh_interval} 秒')
        self.get_logger().info(f'使用模拟数据: {self.use_mock_data}')
        self.get_logger().info(f'发布 Topic: {self.topic_name}')
        self.get_logger().info('=' * 60)

    def load_database_config(self):
        """加载数据库配置文件"""
        if not os.path.exists(self.config_file):
            self.get_logger().warn(f'配置文件不存在: {self.config_file}，使用默认配置')
            return

        try:
            with open(self.config_file, 'r', encoding='utf-8') as f:
                config = yaml.safe_load(f)

            self.db_config = config.get('mysql', {})
            refresh_config = config.get('refresh', {})
            self.refresh_interval = refresh_config.get('interval_seconds', 1.0)

            self.get_logger().info(f'成功加载数据库配置: {self.config_file}')

        except Exception as e:
            self.get_logger().error(f'加载数据库配置失败: {e}')

    def load_config(self):
        """加载配置数据"""
        if self.use_mock_data:
            self.load_mock_config()
        else:
            self.load_from_database()

    def load_mock_config(self):
        """加载模拟配置数据"""
        self.get_logger().info('加载模拟配置数据...')

        self.current_config = {
            "version": "1.0.0",
            "timestamp": self.get_timestamp(),
            "source": "config_node (mock)",
            "config": {
                # ========== ASR 节点配置 ==========
                "asr": {
                    "VAD_MODE": 2,
                    "sample_rate": 16000,
                    "frame_duration_ms": 30,
                    "use_oline_asr": False,
                    "mic_serial_port": "/dev/ttyUSB0",
                    "mic_index": -2,
                    "language": "zh",
                    "regional_setting": "China"
                },

                # ========== Action Service 节点配置 ==========
                "action_service": {
                    "Speed_topic": "/cmd_vel",
                    "text_chat_mode": False,
                    "image_topic": "/camera/color/image_raw",
                    "useolinetts": True,
                    "language": "zh",
                    "regional_setting": "China"
                },

                # ========== Model Service 节点配置 ==========
                "model_service": {
                    "language": "zh",
                    "regional_setting": "China",
                    "text_chat_mode": True
                },

                # ========== 大模型接口配置 ==========
                "large_model": {
                    # 阿里百炼配置
                    "tongyi_api_key": "sk-3b05bc8f1bdc40cc87bd05f864890bd8",
                    "tongyi_app_id": "6ed9f00173214e7883af7310731a5d7b",

                    # 多模态模型
                    "multimodel": "qwen-vl-max",

                    # TTS 配置
                    "tts_supplier": "aliyun",
                    "tts_language": "zh",
                    "oline_tts_model": "qwen-tts",
                    "voice_tone": "Cherry",

                    # ASR 配置
                    "oline_asr_sample_rate": 16000,
                    "oline_asr_model": "paraformer-realtime-v2",

                    # 百度 TTS 配置
                    "baidu_API_KEY": "Ppprf0XqOyQ6uOv2rGg34oR7",
                    "baidu_SECRET_KEY": "1jGl14dF6efRcMOCgiBKwSO8CnlPjBdz",
                    "CUID": "nLSB0tSszSlc2vxM9gQ96FksFuSrQ2cp",
                    "PER": 103,
                    "SPD": 5,
                    "PIT": 5,
                    "VOL": 5,

                    # 网络适配器
                    "network_adapter": "wlP1p1s0",
                },

                # ========== 国际版配置 ==========
                "international": {
                    "decision_AI_api_key": "app-E7cPag5diUJSo4VmRLQXCJoB",
                    "execution_AI_api_key": "app-2DomQwAo3VdUhjf6qiR9NPmP"
                },

                # ========== 本地模型路径配置 ==========
                "model_paths": {
                    "zh_tts_model": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/zh/zh_CN-huayan-medium.onnx",
                    "zh_tts_json": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/zh/zh_CN-huayan-medium.onnx.json",
                    "en_tts_model": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/en/en_US-libritts-high.onnx",
                    "en_tts_json": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/tts/en/en_US-libritts-high.onnx.json",
                    "local_asr_model": "/home/sunrise/opt/app/yahboom_ws/src/largemodel/MODELS/asr/SenseVoiceSmall"
                },

                # ========== 系统配置 ==========
                "system": {
                    "tongyi_base_url": "https://dashscope.aliyuncs.com/compatible-mode/v1",
                    "local_tts_enabled": True,
                    "local_asr_enabled": True
                },

                # ========== Topic 配置 ==========
                "topics": {
                    # action_service 节点
                    "action_service": {
                        "Speed_topic": "/cmd_vel",           # 速度控制话题
                        "image_topic": "/camera/color/image_raw",  # 图像话题
                        "tts_topic": "tts_topic",            # TTS 语音合成话题
                        "reset_flag": "reset_flag",          # 复位标志话题
                        "interrupt_flag": "interrupt_flag",  # 中断标志话题
                        "arm_done_topic": "/largemodel_arm_done",  # 机械臂完成话题
                        "wakeup_topic": "wakeup",            # 唤醒话题
                        "record_status_topic": "record_status"  # 录音状态话题
                    },

                    # model_service 节点
                    "model_service": {
                        "actionstatus_topic": "actionstatus",  # 动作状态话题
                        "asr_topic": "asr",                 # ASR 语音识别话题
                        "seewhat_topic": "seewhat_handle",  # 视觉检测话题
                        "text_response_topic": "text_response"  # 文字回复话题
                    },

                    # environment_node 节点
                    "environment_node": {
                        "environment_topic": "/ai/env"  # 与 environment.publish_topic 保持一致
                    }
                },

                # ========== Environment 节点配置 ==========
                "environment": {
                    # 发布配置
                    "publish_topic": "/ai/env",
                    "intervals": {
                        "battery_seconds": 1,
                        "temperature_seconds": 1,
                        "weather_seconds": 1,
                        "map_seconds": 1
                    }
                },

                # ========== MQTT 配置 ==========
                "mqtt": {
                    "enable": True,
                    "broker_host": "192.168.0.30",
                    "broker_port": 1883,
                    "username": "gbd01",
                    "password": "gbd2025!",
                    "client_id": "agent_01",
                    "keepalive": 60,
                    "command_topic": "/ai/agent/command",
                    "status_topic": "/ai/module/status",
                    "event_topic": "/ai/module/event"
                }
            }
        }

        self.config_version += 1
        self.last_update_time = datetime.now()
        self.get_logger().info('模拟配置数据加载完成')

    def load_from_database(self):
        """
        从 MySQL 数据库加载配置
        预留接口，后续实现
        """
        self.get_logger().info('准备从数据库加载配置...')

        # ========== TODO: 实现 MySQL 连接 ==========
        # 1. 建立数据库连接 (长连接)
        # 2. 查询配置表
        # 3. 解析配置数据
        # 4. 更新 current_config
        # 5. 错误处理和重连

        # 当前返回模拟数据作为 fallback
        self.get_logger().warn('数据库功能尚未实现，使用 YAML fallback 配置')
        self.load_yaml_fallback()

    def load_yaml_fallback(self):
        """从 YAML 文件加载 fallback 配置"""
        self.get_logger().info('从 YAML 文件加载 fallback 配置...')

        # 尝试从 largemodel 包加载配置
        try:
            import subprocess
            result = subprocess.run(
                ['find', '/home/sunrise/opt/dev/project/aiagent/brain/PlannerNode2', '-name', 'yahboom.yaml'],
                capture_output=True, text=True, timeout=5
            )
            yaml_paths = result.stdout.strip().split('\n')

            if yaml_paths and yaml_paths[0]:
                self.get_logger().info(f'找到 YAML 配置文件: {yaml_paths[0]}')
                with open(yaml_paths[0], 'r', encoding='utf-8') as f:
                    yaml_config = yaml.safe_load(f)

                # 转换格式
                self.current_config = self.convert_yaml_to_json(yaml_config)
                self.config_version += 1
                self.last_update_time = datetime.now()
                self.get_logger().info('YAML fallback 配置加载完成')
            else:
                self.get_logger().warn('未找到 YAML 配置文件，使用默认模拟数据')
                self.load_mock_config()

        except Exception as e:
            self.get_logger().error(f'加载 YAML fallback 配置失败: {e}')
            self.get_logger().warn('使用默认模拟数据')
            self.load_mock_config()

    def convert_yaml_to_json(self, yaml_config):
        """将 YAML 配置转换为统一格式"""
        config = {
            "version": "1.0.0",
            "timestamp": self.get_timestamp(),
            "source": "config_node (yaml_fallback)",
            "config": {}
        }

        # 提取 asr 配置
        if 'asr' in yaml_config:
            config['config']['asr'] = yaml_config['asr'].get('ros__parameters', {})

        # 提取 action_service 配置
        if 'action_service' in yaml_config:
            config['config']['action_service'] = yaml_config['action_service'].get('ros__parameters', {})

        # 提取 model_service 配置
        if 'model_service' in yaml_config:
            config['config']['model_service'] = yaml_config['model_service'].get('ros__parameters', {})

        return config

    def get_timestamp(self):
        """获取当前时间戳"""
        return datetime.now().strftime('%Y-%m-%dT%H:%M:%SZ')

    def timer_callback(self):
        """定时器回调 - 刷新并发布配置"""
        # 重新加载配置
        self.load_config()

        # 发布配置
        self.publish_config()

    def publish_config(self):
        """发布配置到 /ai/config topic"""
        if not self.current_config:
            self.get_logger().warn('配置为空，跳过发布')
            return

        # 更新时间戳
        self.current_config['timestamp'] = self.get_timestamp()
        self.current_config['version'] = f"1.0.{self.config_version}"

        # 序列化为 JSON
        try:
            json_str = json.dumps(self.current_config, ensure_ascii=False, indent=2)

            # 发布消息
            msg = String()
            msg.data = json_str
            self.config_publisher.publish(msg)

            self.get_logger().info(f'配置已发布 (version: {self.current_config["version"]})')
            self.get_logger().debug(f'配置内容: {json_str[:200]}...' if len(json_str) > 200 else f'配置内容: {json_str}')

        except Exception as e:
            self.get_logger().error(f'发布配置失败: {e}')

    def connect_database(self):
        """
        建立 MySQL 数据库连接
        预留接口
        """
        # ========== TODO: 实现长连接 ==========
        # import pymysql
        # self.db_manager = pymysql.connect(
        #     host=self.db_config.get('host', 'localhost'),
        #     port=self.db_config.get('port', 3306),
        #     user=self.db_config.get('user', 'root'),
        #     password=self.db_config.get('password', ''),
        #     database=self.db_config.get('database', 'robot_config'),
        #     charset=self.db_config.get('charset', 'utf8mb4'),
        #     connect_timeout=30
        # )
        pass

    def reconnect_database(self):
        """
        重新连接数据库
        预留接口
        """
        # ========== TODO: 实现重连逻辑 ==========
        # max_attempts = self.db_config.get('reconnect', {}).get('max_attempts', 5)
        # retry_interval = self.db_config.get('reconnect', {}).get('retry_interval', 5)
        #
        # for attempt in range(max_attempts):
        #     try:
        #         self.connect_database()
        #         self.get_logger().info('数据库重连成功')
        #         return True
        #     except Exception as e:
        #         self.get_logger().warn(f'数据库重连失败 (尝试 {attempt + 1}/{max_attempts}): {e}')
        #         time.sleep(retry_interval)
        #
        # self.get_logger().error('数据库重连失败，使用 fallback 配置')
        # return False
        pass

    def shutdown(self):
        """关闭节点时清理资源"""
        self.get_logger().info('关闭 Config Node...')
        if self.db_manager:
            try:
                self.db_manager.close()
                self.get_logger().info('数据库连接已关闭')
            except Exception as e:
                self.get_logger().warn(f'关闭数据库连接时出错: {e}')


def main(args=None):
    rclpy.init(args=args)

    node = ConfigNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.shutdown()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
