import time
import sys
import os
import toml
import copy
import math
import lcm

# 添加父目录到路径，以便能够导入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

robot_cmd = {
    'mode':0, 'gait_id':0, 'contact':0, 'life_count':0,
    'vel_des':[0.0, 0.0, 0.0],
    'rpy_des':[0.0, 0.0, 0.0],
    'pos_des':[0.0, 0.0, 0.0],
    'acc_des':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    'ctrl_point':[0.0, 0.0, 0.0],
    'foot_pose':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    'step_height':[0.0, 0.0],
    'value':0,  'duration':0
}

def pass_bar(ctrl, msg):
    """
    俯身通过一个栅栏
    """
    # 切换步态
    usergait_msg = file_send_lcmt()
    lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
    
    try:
        steps = toml.load("./task_4/Gait_Params_stoop.toml")
        full_steps = {'step':[robot_cmd]}
        k =0
        for i in steps['step']:
            cmd = copy.deepcopy(robot_cmd)
            cmd['duration'] = i['duration']
            if i['type'] == 'usergait':                
                cmd['mode'] = 11 # LOCOMOTION
                cmd['gait_id'] = 110 # USERGAIT
                cmd['vel_des'] = i['body_vel_des']
                cmd['rpy_des'] = i['body_pos_des'][0:3]
                cmd['pos_des'] = i['body_pos_des'][3:6]
                cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2]
                cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5]
                cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8]
                cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
                cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3
                cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3
                cmd['acc_des'] = i['weight']
                cmd['value'] = i['use_mpc_traj']
                cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
                cmd['ctrl_point'][2] =  i['mu']
            if k == 0:
                full_steps['step'] = [cmd]
            else:
                full_steps['step'].append(cmd)
            k=k+1
        f = open("./task_4/Gait_Params_stoop_full.toml", 'w')
        f.write("# Gait Params\n")
        f.writelines(toml.dumps(full_steps))
        f.close()

        # pre
        file_obj_gait_def = open("./task_4/Gait_Def_stoop.toml",'r')
        file_obj_gait_params = open("./task_4/Gait_Params_stoop_full.toml",'r')
        usergait_msg.data = file_obj_gait_def.read()
        lcm_usergait.publish("user_gait_file",usergait_msg.encode())
        time.sleep(0.5)
        usergait_msg.data = file_obj_gait_params.read()
        lcm_usergait.publish("user_gait_file",usergait_msg.encode())
        time.sleep(0.1)
        file_obj_gait_def.close()
        file_obj_gait_params.close()

        file_obj_gait_params = open("./task_4/Gait_Params_stoop_full.toml",'r')
        usergait_msg.data = file_obj_gait_params.read()
        lcm_usergait.publish("user_gait_file", usergait_msg.encode())
        time.sleep(0.5)
        file_obj_gait_params.close()

        msg.mode = 62
        msg.value = 0
        msg.contact = 15
        msg.gait_id = 110
        msg.duration = 1000
        msg.life_count += 1
        
        for i in range(50):
            ctrl.Send_cmd(msg)
            time.sleep(0.2)

    except KeyboardInterrupt:
        msg.mode = 7 #PureDamper before KeyboardInterrupt:
        msg.gait_id = 0
        msg.duration = 0
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        pass