#!/usr/bin/env python3
"""
LLM节点测试工具

功能：
1. 发布wake_word/status话题唤醒/休眠LLM节点
2. 发布asr/text话题以文字输入方式测试LLM节点
3. 订阅llm/response话题接收智能体输出
"""

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import threading


class LLMTestNode(Node):
    """LLM节点测试工具"""

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

        # 创建wake_word/status发布者
        self.wake_publisher = self.create_publisher(
            String,
            'wake_word/status',
            10
        )

        # 创建asr/text发布者
        self.asr_publisher = self.create_publisher(
            String,
            'asr/text',
            10
        )

        # 创建llm/response订阅者
        self.llm_response_subscriber = self.create_subscription(
            String,
            'llm/response',
            self.llm_response_callback,
            10
        )

        self.get_logger().info('LLM测试工具初始化完成')
        self.get_logger().info('可用命令:')
        self.get_logger().info('  wake   - 唤醒LLM节点')
        self.get_logger().info('  sleep  - 休眠LLM节点')
        self.get_logger().info('  输入文字 - 发送给LLM节点处理')
        self.get_logger().info('  quit   - 退出程序')

    def llm_response_callback(self, msg):
        """处理LLM回复回调"""
        self.get_logger().info(f'LLM回复: {msg.data}')

    def publish_wake(self):
        """发布唤醒状态"""
        msg = String()
        msg.data = 'wake'
        self.wake_publisher.publish(msg)
        self.get_logger().info('已发送唤醒信号')

    def publish_sleep(self):
        """发布休眠状态"""
        msg = String()
        msg.data = 'sleep'
        self.wake_publisher.publish(msg)
        self.get_logger().info('已发送休眠信号')

    def publish_asr_text(self, text):
        """发布ASR文字"""
        msg = String()
        msg.data = text
        self.asr_publisher.publish(msg)
        self.get_logger().info(f'已发送文字: {text}')


def user_input_thread(test_node):
    """用户输入线程"""
    print('\n' + '='*50)
    print('LLM节点测试工具')
    print('='*50)
    print('可用命令:')
    print('  wake   - 唤醒LLM节点')
    print('  sleep  - 休眠LLM节点')
    print('  输入文字 - 发送给LLM节点处理')
    print('  quit   - 退出程序')
    print('='*50 + '\n')

    while True:
        try:
            user_input = input('>>> ').strip()

            if not user_input:
                continue

            if user_input.lower() == 'quit':
                print('退出程序...')
                rclpy.shutdown()
                break
            elif user_input.lower() == 'wake':
                test_node.publish_wake()
            elif user_input.lower() == 'sleep':
                test_node.publish_sleep()
            else:
                test_node.publish_asr_text(user_input)

        except (EOFError, KeyboardInterrupt):
            print('\n退出程序...')
            rclpy.shutdown()
            break


def main():
    """主函数"""
    rclpy.init()

    test_node = LLMTestNode()

    try:
        # 创建用户输入线程
        input_thread = threading.Thread(target=user_input_thread, args=(test_node,))
        input_thread.daemon = True
        input_thread.start()

        # 主线程运行ROS2事件循环
        rclpy.spin(test_node)

    except KeyboardInterrupt:
        print('\n退出程序...')
    finally:
        test_node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
