#!/usr/bin/env python3

import os
import signal
import sys
from datetime import datetime

try:
    import rclpy
    from rclpy.node import Node
    from std_msgs.msg import String, Int32, Bool
    HAS_ROS2 = True
except ImportError:
    HAS_ROS2 = False


class CommandNode(Node):
    def __init__(self):
        super().__init__('command_node')
        self.state_publisher = self.create_publisher(Int32, '/state_translate', 10)
        self.activation_subscriber = self.create_subscription(Bool, 'llm_activation', self.activation_callback, 10)
        self.voice_words_subscriber = self.create_subscription(String, 'voice_words', self.voice_words_callback, 10)
        self.is_llm_activated = False
        print("命令节点已初始化，正在订阅 'voice_words' 和 'llm_activation' 话题")

    def activation_callback(self, msg):
        self.is_llm_activated = msg.data
        print(f'LLM激活状态已更新: {self.is_llm_activated}')

    def voice_words_callback(self, msg):
        if not self.is_llm_activated:
            return

        text = msg.data
        print(f'收到语音识别结果: {text}')

        state_msg = Int32()
        publish = False

        matched_command = text

        if matched_command:
            if '起来' in matched_command or '起立' in matched_command:
                state_msg.data = 1
                publish = True
            elif '向前走' in matched_command or '前进' in matched_command:
                state_msg.data = 2
                publish = True
            elif '左转' in matched_command:
                state_msg.data = 3
                publish = True
            elif '右转' in matched_command:
                state_msg.data = 4
                publish = True
            elif '向左走' in matched_command:
                state_msg.data = 5
                publish = True
            elif '向右走' in matched_command:
                state_msg.data = 6
                publish = True
            elif '坐下' in matched_command:
                state_msg.data = 7
                publish = True
            elif '后转' in matched_command:
                state_msg.data = 8
                publish = True

            if publish:
                self.state_publisher.publish(state_msg)
                print(f'检测到命令，发布 state_translate: {state_msg.data}')


def main(args=None):
    if HAS_ROS2:
        rclpy.init(args=args)
        node = CommandNode()
        print("命令节点运行中 ...")
        rclpy.spin(node)
        node.destroy_node()
        rclpy.shutdown()
    else:
        print("ROS2 不可用")


if __name__ == '__main__':
    main()
