#!/usr/bin/env python3
import lcm
import time

from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
from utils.robot_control_response_lcmt import robot_control_response_lcmt

class SimpleStandTest:
    def __init__(self):
        # LCM通信初始化
        self.lc = lcm.LCM("udpm://239.255.76.67:7670?ttl=255")
        self.cmd_msg = robot_control_cmd_lcmt()
        
        # 订阅状态反馈
        self.lc.subscribe("robot_control_response", self.handle_response)
        
    def handle_response(self, channel, data):
        """处理状态反馈"""
        msg = robot_control_response_lcmt().decode(data)
        print(f"[状态反馈] 模式:{msg.mode} 进度:{msg.order_process_bar}% 电机状态:{msg.motor_error}  ori:{msg.ori_error} foot:{msg.footpos_error[0]}")

    def send_stand_command(self):
        # 1. 退出当前模式
        self.cmd_msg.mode = 0
        self.lc.publish("robot_control_cmd", self.cmd_msg.encode())
        time.sleep(1)

        # 2. 进入阻尼模式（准备阶段）
        self.cmd_msg.mode = 7
        self.cmd_msg.pos_des = [0, 0, 0.1]  # 先降到10cm高度
        self.lc.publish("robot_control_cmd", self.cmd_msg.encode())
        time.sleep(2)  # 等待稳定

        # 3. 切换到站立模式
        self.cmd_msg.mode = 12
        self.cmd_msg.pos_des = [0, 0, 0.3]  # 目标高度30cm
        self.cmd_msg.vel_des = [0, 0, 0]  # 零速度
        self.cmd_msg.rpy_des = [0, 0, 0]  # 水平姿态
        self.cmd_msg.step_height = [0.05, 0.05]  # 抬腿高度5cm
        self.lc.publish("robot_control_cmd", self.cmd_msg.encode())

    def run(self, timeout=5):
        """运行测试"""
        start_time = time.time()
        while time.time() - start_time < timeout:
            self.send_stand_command()
            self.lc.handle()  # 处理接收到的消息
            time.sleep(0.1)  # 控制发送频率

if __name__ == '__main__':
    print("=== 机器狗站立测试 ===")
    tester = SimpleStandTest()
    
    try:
        print("尝试站立...")
        tester.run(timeout=10)  # 持续运行10秒
    except KeyboardInterrupt:
        print("用户中断测试")
    finally:
        print("测试结束")