import time

class BaseMsg:
    def __init__(self, ctrl, msg):
        self.ctrl = ctrl
        self.msg = msg

    def stop_force(self):
        """
        强制停止，但是大部分场景不好用，容易导致 robot 崩坏。
        """
        self.msg.mode = 0
        self.msg.gait_id = 0
        self.msg.duration = 0
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        self.ctrl.Wait_finish(0, 0)

    def stop(self, wait_time=200):
        self.msg.mode = 11
        self.msg.gait_id = 27
        self.msg.vel_des = [0, 0, 0]
        self.msg.duration = wait_time
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        if wait_time:
            time.sleep(wait_time / 1000)
    
    def stop_smooth(self, wait_time=300):
        """平滑停止，采用逐渐减速的方式实现更柔和的停止过程"""
        # 获取当前速度
        current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') and self.msg.vel_des else [0, 0, 0]
        
        # 如果当前速度已经为零，直接调用普通停止
        if all(abs(v) < 0.01 for v in current_vel):
            self.stop(wait_time)
            return
            
        # 计算减速步数和每步减速量
        steps = 5  # 减速分5步完成
        step_time = wait_time / steps
        vel_step = [v / steps for v in current_vel]
        
        # 逐步减速
        for i in range(steps):
            # 计算当前步骤的目标速度
            target_vel = [current_vel[j] - vel_step[j] * (i + 1) for j in range(len(current_vel))]
            
            # 发送减速命令
            self.msg.mode = 11
            self.msg.gait_id = 26
            self.msg.vel_des = target_vel
            self.msg.duration = step_time
            self.msg.life_count += 1
            self.ctrl.Send_cmd(self.msg)
            time.sleep(step_time / 1000)
        
        # 最后确保完全停止
        self.msg.vel_des = [0, 0, 0]
        self.msg.duration = step_time
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        time.sleep(step_time / 1000)

    def lie_down(self, wait_time=500):
        """发送趴下指令
        
        参数:
            wait_time: 等待时间(毫秒)，默认为500ms
        """
        assert wait_time != 0 or wait_time > 3000, "wait_time 必须在 0 or > 3000"
        self.msg.mode = 7
        self.msg.gait_id = 1
        self.msg.duration = wait_time
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        if wait_time:
            time.sleep(wait_time / 1000)

    def stand_up(self):
        self.msg.mode = 12
        self.msg.gait_id = 0
        self.msg.life_count += 1
        self.ctrl.Send_cmd(self.msg)
        self.ctrl.Wait_finish(12, 0)