import sys
import time
import threading

# 原代码5. 运动控制模块完整逻辑（保留原路径导入）
PROJECT_ROOT = "/home/duckpi/open_duck_mini_ws/OPEN_DUCK_MINI/Open_Duck_Mini_Runtime-2"
sys.path.append(PROJECT_ROOT)
from v2_rl_walk_mujoco import RLWalk

class RobotMotionController:
    def __init__(self, onnx_model_path, tts_controller, feedback_text):
        # 接收调度脚本传入的TTS实例和反馈文本，保持原逻辑调用
        self.tts_controller = tts_controller
        self.FEEDBACK_TEXT = feedback_text
        try:
            self.rl_walk = RLWalk(
                onnx_model_path=onnx_model_path,
                cutoff_frequency=40,
                pid=[30, 0, 0]
            )
            self.walk_thread = threading.Thread(target=self.rl_walk.run, daemon=True)
            self.walk_thread.start()
            time.sleep(1)
            print("🤖 运动控制模块初始化成功")
        except Exception as e:
            print(f"❌ 运动控制失败：{str(e)}")
            sys.exit(1)

    def execute_motion(self, action_name: str, seconds: int):
        # 全局变量由调度脚本传入，保留原逻辑
        global is_processing
        is_processing = True
        try:
            # 播放反馈（同步执行确保声音输出）
            self.tts_controller.speak(self.FEEDBACK_TEXT[action_name])

            # 执行运动
            seconds = max(2, min(seconds, 5))
            if action_name == "move_forward":
                print(f"\n🚶 前进{seconds}秒...")
                self.rl_walk.last_commands[0] = 0.17
            elif action_name == "move_backward":
                print(f"\n🚶 后退{seconds}秒...")
                self.rl_walk.last_commands[0] = -0.17
            elif action_name == "turn_left":
                print(f"\n🔄 左转{seconds}秒...")
                self.rl_walk.last_commands[2] = 1.1
            elif action_name == "turn_right":
                print(f"\n🔄 右转{seconds}秒...")
                self.rl_walk.last_commands[2] = -1.1
            
            time.sleep(seconds)
            self.rl_walk.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
            print(f"✅ 运动完成")
        except Exception as e:
            print(f"❌ 运动执行失败：{str(e)}")
            self.rl_walk.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        finally:
            is_processing = False