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, turn_degree_twice, turn_degree_v2
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('角度为',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.72,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] 对应 [左转, 右转, 左转]
    
    gotoxy=[
        (0.87,0.23),
        (1.19,0.22),
        (0.91,1.55),     
        (0.81,1.46),     
        (0.67,2.59),     
        (0.92,2.43),     
        (1.53,3.62),     
        (1.15,3.70),     
        (0.63,4.73),     
    ]
    
    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)
        
        # 根据转弯方向调整持续时间（假设半圆需要3秒）
        go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True)
        # 打印当前方向
        print(f"开始 {text[i]} ")
        print(f"第{i}次",'角度为',ctrl.odo_msg.rpy[2])
        print(f"第{i}次",'x为',ctrl.odo_msg.xyz[0])
        print(f"第{i}次",'y为',ctrl.odo_msg.xyz[1])
        print(f"第{i}次",'z为',ctrl.odo_msg.xyz[2])
        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):
    # go_to_xy(ctrl, msg, 0.2, 1, speed=0.5, observe=True)
    go_to_xy(ctrl, msg, 0.7, 5, speed=0.5, observe=True)
    turn_degree_v2(ctrl, msg, -106.5, absolute=True)

    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])

    text=["直线走一段","第一个圆弧","微调路径","第二个圆弧","微调路径","第三个圆弧","微调路径","第四个圆弧","绕出来","前走一点"]
    
    # 连续三重弯道行走
    directions = [
        (0.3,0,0,0,0,0,0.6),
        #第四个圆弧
        (0.4,0,0.69,0,0,0,3),
        #微调路径
        (0.3,0,0,0,0,0,1.3),
        # 第三个圆弧
        (0.4,0,-0.73,0,0,0,6),
        #微调路径
        (0.3,0,0,0,0,0,1.4),
        #第二个圆弧
        (0.4,0,0.74,0,0,0,7.4),
        #微调路径
        (0.3,0,0,0,0,0,1.2),
        #第一个圆弧
        (0.45, 0, -0.78,0,0,0,5.8),
        #直线走一段
        (0.4,0.05,0,0,0,0,0.3),
        (0.4,0,0,0,0,0,1.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秒