import time
import sys
import os

# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from base_move.turn_degree import turn_degree, turn_degree_v2
from base_move.go_to_xy import go_straight, go_to_x_v2, go_to_y_v2
from base_move.move_base_hori_line import arc_turn_around_hori_line, align_to_horizontal_line
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

observe = True

def run_task_2_5(Ctrl, msg, direction='left'):
    section('任务2.5：预备进入任务3', "启动")

    go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)

    # TEST
    turn_degree(Ctrl, msg, 90, absolute=observe)

    section('任务2.5-1：第一次旋转', "移动")

    turn_success, res = arc_turn_around_hori_line(
        Ctrl,
        msg,
        angle_deg=90 if direction == 'left' else -90,
        target_distance=0.2,
        detect_func_version=3,
        observe=observe,
    )

    go_straight(Ctrl, msg, distance=0.1, speed=0.5, observe=observe)

    section('任务2.5-2：第二次旋转', "移动")

    turn_degree_v2(Ctrl, msg, degree=90, absolute=True)

    # arc_turn_around_hori_line(
    #     Ctrl,
    #     msg,
    #     angle_deg=70,
    #     # target_distance=0.5,
    #     pass_align=True,
    #     radius=res['radius'],
    #     detect_func_version=2,
    #     observe=observe,
    # )

def run_task_2_5_back(Ctrl, msg, direction='left'):
    section('任务2.5-back：预备进入任务3', "启动")
    turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)

    go_to_x_v2(Ctrl, msg, target_x=0.8, observe=observe)