import time
import sys
import os
import math

sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from base_move.go_to_xy import go_to_xy
from base_move.turn_degree import turn_degree
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

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
)

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

observe = True

def run_task_2(ctrl, msg):

    # 微调 xy 和角度

    go_to_xy(ctrl, msg, 0.9, 0.25, speed=0.5, observe=True)
    turn_degree(ctrl, msg, 0.8, absolute=True)

    # 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)
    print('角度为',ctrl.odo_msg.rpy[2])
    #获取微调角度
    text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
    # 连续三重弯道行走
    directions = [
        #直线走一段
        (0.4,0,0,0,0,0,1.2),
        #第一个圆弧
        (0.45, 0, 0.77,0,0,0,6.5),
        #微调路径
        (0.3,0,0,0,0,0,0.4),
        #第二个圆弧
        (0.4,0,-0.73,0,0,0,7.9),
        #微调路径
        (0.3,0,0,0,0,0,1),
        # 第三个圆弧
        (0.4,0,0.73,0,0,0,6.4),
        #微调路径
        (0.3,0,0,0,0,0,1.3),
        #第四个圆弧
        (0.4,0,-0.69,0,0,0,3.6),
        #绕出来
        (0.3,0,0.6,0,0,0,1),
        # #前走一点
        # (0.3,0,0,0,0,0,0.5)
        ]  # [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"开始 {text[i]} ")
        
        # 根据转弯方向调整持续时间（假设半圆需要3秒）
        time.sleep(t) # 持续3秒

    print('角度为',ctrl.odo_msg.rpy[2])
    print('x为',ctrl.odo_msg.xyz[0])
    print('y为',ctrl.odo_msg.xyz[1])
    print('z为',ctrl.odo_msg.xyz[2])
    # # 停止
    # msg.mode = 7    # PureDamper
    # msg.gait_id = 0
    # msg.life_count += 1
    # Ctrl.Send_cmd(msg)
    # Ctrl.Wait_finish(7, 0)

def run_task_2_back(ctrl, msg):

    text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
    
    # 连续三重弯道行走
    directions = [
        (0.3,0,0.6,0,0,0,1),
        #第四个圆弧
        (0.4,0,0.69,0,0,0,3.6),
        #微调路径
        (0.3,0,0,0,0,0,1.3),
        # 第三个圆弧
        (0.4,0,-0.73,0,0,0,6.4),
        #微调路径
        (0.3,0,0,0,0,0,1),
        #第二个圆弧
        (0.4,0,0.73,0,0,0,7.9),
        #微调路径
        (0.3,0,0,0,0,0,0.4),
        #第一个圆弧
        (0.45, 0, -0.77,0,0,0,6.5),
        #直线走一段
        (0.4,0,0,0,0,0,1.2),
    ]  # [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"开始 {text[i]} ")
        
        # 根据转弯方向调整持续时间（假设半圆需要3秒）
        time.sleep(t) # 持续3秒