import time
import sys
import os
import math

# sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
# from base_move.move_base_hori_line import (
#     arc_turn_around_hori_line,
#     go_straight_by_hori_line,
#     move_to_hori_line,
#     align_to_horizontal_line
# )
# from base_move.go_straight import go_straight
# from base_move.turn_degree import turn_degree
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

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

observe = True

def run_task_2(ctrl, msg):

    # print("Recovery stand")
    # Ctrl.base_msg.stand_up()
    
    # # time.sleep(100) # TEST 

    # run_task_1(Ctrl, msg)

    # x,y=image_processor.get_current_x()
    # print('分别是',x,y)
    # # 初始恢复站立
    # msg.mode = 12 # Recovery stand
    # msg.gait_id = 0
    # msg.life_count += 1
    # Ctrl.Send_cmd(msg)
    # Ctrl.Wait_finish(12, 0)
    # print('1')
    # x,y=image_processor.get_current_x()
    # print('分别是',x,y)

    # # 初始恢复站立
    # msg.mode = 11 # Recovery stand
    # msg.gait_id = 27
    # msg.vel_des = [0.3,0,0]
    # msg.pos_des[2]=0.24
    # msg.duration = 0
    # msg.step_height = [0.01, 0.01]
    # msg.life_count += 1
    # Ctrl.Send_cmd(msg)
    # time.sleep(1)

    #获取微调角度
    

    # 连续三重弯道行走
    directions = [
        #直线走一段
        (0.4,0,0,0,0,0,0.5),
        #第一个圆弧
        (0.45, 0, 0.77,0,0,0,7.3),
        #微调路径
        (0.4,0,0,0,0,0,0.8),
        #第二个圆弧
        (0.4,0,-0.72,0,0,0,7.7),
        #第三个圆弧
        (0.4,0.05,-0.75,0,0,0,1.2),
        #微调路径
        (0.45, 0, 0.8,0,0,0,7.2),
        #第四个圆弧
        (0.4,0,-0.72,0,0,0,4)]  # [vel_x, vel_z] 对应 [左转, 右转, 左转]
    

    for i, (vel_x, vel_y, vel_z ,rpy_x,rpy_y,rpy_z,t) in enumerate(directions):
        # 设置转向命令
        msg.mode = 11 # Locomotion
        msg.gait_id = 26 # TROT_FAST
        msg.vel_des = [vel_x, vel_y, vel_z] # [x, y, z]，y=0表示不侧移
        msg.rpy_des = [rpy_x, rpy_y, rpy_z]
        msg.duration = 0 # 连续运动
        msg.step_height = [0.03, 0.03]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        
        # 打印当前方向
        print(f"开始第 {i+1} 次转弯")
        
        # 根据转弯方向调整持续时间（假设半圆需要3秒）
        time.sleep(t) # 持续3秒

    # # 停止
    # msg.mode = 7    # PureDamper
    # msg.gait_id = 0
    # msg.life_count += 1
    # Ctrl.Send_cmd(msg)
    # Ctrl.Wait_finish(7, 0)