import time
import sys
import os
import toml

# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
# 添加当前目录到路径，确保可以找到local文件
sys.path.append(os.path.dirname(os.path.abspath(__file__)))

from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
from base_move.turn_degree import turn_degree
from base_move.go_straight import go_straight
from file_send_lcmt import file_send_lcmt

# 创建本模块特定的日志记录器
logger = get_logger("任务3")

observe = True

def load_gait_file(file_path):
    """加载步态文件"""
    try:
        with open(file_path, 'r') as f:
            return f.read()
    except Exception as e:
        error(f"加载步态文件失败: {e}", "文件")
        return None

def run_task_3(ctrl, msg):
    section('任务3：步态切换', "启动")
    info('开始执行任务3...', "启动")
    
    # 创建一个LCM通信实例用于发送文件
    lc_file = ctrl.lc_s  # 复用已有的LCM发送通道
    
    section('任务3-1：切换到直立步态', "步态")
    # 加载直立步态文件
    gait_up_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_up.toml"))
    gait_up_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_up.toml"))
    
    if gait_up_def and gait_up_params:
        # 发送步态定义文件
        file_msg = file_send_lcmt()
        file_msg.data = gait_up_def
        lc_file.publish("Gait_Def", file_msg.encode())
        time.sleep(0.5)  # 等待处理
        
        # 发送步态参数文件
        file_msg.data = gait_up_params
        lc_file.publish("Gait_Params", file_msg.encode())
        time.sleep(0.5)  # 等待处理
        
        # 执行步态切换
        msg.mode = 62  # 用户自定义步态模式
        msg.gait_id = 110  # 根据Usergait_List.toml定义
        ctrl.Send_cmd(msg)
        
        # 等待步态切换完成
        success("直立步态切换完成", "步态")
        time.sleep(2)  # 给机器人一些时间适应新步态
        
        # 使用新步态进行简单移动
        go_straight(ctrl, msg, distance=0.5, speed=0.3, observe=observe)
    else:
        error("直立步态文件加载失败，无法执行任务3-1", "错误")
        return
    
    section('任务3-2：切换到后退步态', "步态")
    # 加载后退步态文件
    gait_moonwalk_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_moonwalk.toml"))
    gait_moonwalk_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_moonwalk.toml"))
    
    if gait_moonwalk_def and gait_moonwalk_params:
        # 发送步态定义文件
        file_msg = file_send_lcmt()
        file_msg.data = gait_moonwalk_def
        lc_file.publish("Gait_Def", file_msg.encode())
        time.sleep(0.5)  # 等待处理
        
        # 发送步态参数文件
        file_msg.data = gait_moonwalk_params
        lc_file.publish("Gait_Params", file_msg.encode())
        time.sleep(0.5)  # 等待处理
        
        # 执行步态切换
        msg.mode = 62  # 用户自定义步态模式
        msg.gait_id = 110  # 根据Usergait_List.toml定义
        ctrl.Send_cmd(msg)
        
        # 等待步态切换完成
        success("后退步态切换完成", "步态")
        time.sleep(2)  # 给机器人一些时间适应新步态
        
        # 使用新步态进行简单移动
        go_straight(ctrl, msg, distance=-0.5, speed=0.3, observe=observe)
    else:
        error("后退步态文件加载失败，无法执行任务3-2", "错误")
        return
    
    # 回到默认步态
    section('任务3-3：恢复默认步态', "步态")
    ctrl.base_msg.stand_up()  # 使用基础消息模块恢复到默认站立姿态
    
    success("任务3完成", "完成")