import time
import sys
import os
import math

# 添加父目录到路径，以便能够导入utils
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, turn_degree_twice, turn_degree_v2
from base_move.center_on_dual_tracks import center_on_dual_tracks
from base_move.go_to_xy import go_to_xy
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
from utils.speech_demo import speak
# 创建本模块特定的日志记录器
logger = get_logger("任务1")

observe = False  # 控制是否输出更多调试信息
direction = True

def run_task_1(ctrl, msg, time_sleep=5000):
    global direction
    section('任务1：寻找横线并绕行', "启动")
    info('开始执行任务1...', "启动")
 
    # TEST better align
    # aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
    # print(aligned)
    # go_straight(ctrl, msg, -10, observe=observe)
    # return

    # v2
    section('任务1-1：转弯并扫描QR码', "移动")
    # 在 arc_turn_around_hori_line 中启用 QR 码扫描
    turn_success, res = arc_turn_around_hori_line(
        # pass_align=True,  // TEST 测试用，直接以 1.5 m 半径右转。
        # radius=1.5,
        ctrl=ctrl,
        msg=msg,
        angle_deg=-85,  # 负值表示右转  # INFO 目前来看，这个 -85 + 内部的微调，跟镜像里不断测试出来的效果好了很多，暂时可用。
        #
        observe=False,  # INFO 暂时关闭观察
        scan_qrcode=True,  # 启用 QR 码扫描
        qr_check_interval=0.3  # 每 0.3 秒检查一次扫描结果
    )
    
    if res and res.get('qr_result'):
        success(f"在任务1-1中成功扫描到QR码: {res.get('qr_result')}", "扫描")
    else:
        warning("任务1-1中未能扫描到任何QR码", "警告")

    section('任务1-2：移动到横线', "移动")
    # 执行常规的移动操作
    move_to_hori_line(ctrl, msg, target_distance=1.1, observe=observe)

    section('任务1-3：转弯', "旋转")
    direction = False if res.get('qr_result') == 'A-1' else True  # TODO 需要检查一下，这个方向是否正确

    # if direction == False :speak("A 区库位 1")
    # else:speak("A 区库位 2")

    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=-170 if direction else 170,  # TODO need check
        target_distance=0.6,
        min_radius=0.3,
        max_radius=0.4,
        #
        pass_align=True,
        observe=observe,
        # TODO clear
        bad_big_angle_corret=True,
    )

    if turn_success:
        success("任务1-3完成", "完成")
    else:
        warning("任务1-3失败", "警告")
        return
    
    section('任务1-4：直线移动', "移动")
    move_distance = 0.3
    move_speed = 0.5
    if not direction:
        # TODO to check
        # go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
        turn_degree_v2(ctrl, msg, degree=90, absolute=True, precision=True)
        go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
    else:
        go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)

    section('任务1-5：模拟装货', "停止")
    info('机器人躺下，模拟装货过程', "信息")
    start_time = time.time()
    ctrl.base_msg.lie_down(wait_time=time_sleep)
    elapsed = time.time() - start_time
    timing("装货过程", elapsed)

    # 站起来
    info('机器人站起来，准备继续任务', "信息")
    ctrl.base_msg.stand_up()

    section('任务1-6：返回', "移动")
    go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.3), observe=observe)

    # turn and back
    # turn_degree_twice(ctrl, msg, degree=0, absolute=True, precision=True)
    turn_degree_v2(ctrl, msg, degree=0, absolute=True, precision=True)

    # add
    go_straight(ctrl, msg, distance=0.3, observe=observe)

    section('任务1-7：90度转弯', "旋转")
    radius = res['radius'] * 2 + 0.1
    info(f"任务1-7: 转弯半径: {radius}", "信息")
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        radius=radius,
        angle_deg=85 if direction else -85,
        #
        pass_align=True,
        observe=observe,
        no_end_reset=True,
    )

    section('任务1-8：直线移动', "移动")
    move_to_hori_line(ctrl, msg, target_distance=0.3, observe=observe)

    section('任务1-9：90度旋转', "旋转")
    turn_degree_v2(ctrl, msg, degree=0, absolute=True, precision=True)

    section('任务1-10: y校准，准备 task-2', "移动")
    # TODO
    
    success("任务1完成", "完成")


def run_task_1_back(ctrl, msg, time_sleep=5000):
    section('任务1-11: 返回', "移动")
    go_straight(ctrl, msg, distance=0.2, observe=observe)
    turn_degree_v2(ctrl, msg, -90, absolute=True)  # 不确定 odo 效果如何；

    section('任务1-11: 直线移动', "移动")
    move_to_hori_line(ctrl, msg, target_distance=0.2, observe=observe)

    section('任务1-12: 180度旋转', "旋转")
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=170 if direction else -170,
        target_distance=0.6,
        min_radius=0.3,
        max_radius=0.4,
        pass_align=True,
        observe=observe,
        no_end_reset=True,
    )

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

    section('任务1-13: 直线移动', "移动")
    move_distance = 0.5
    go_straight(ctrl, msg, distance=move_distance, observe=observe)

    section('任务1-14: 模拟装货', "停止")
    info('机器人躺下，模拟装货过程', "信息")
    start_time = time.time()
    ctrl.base_msg.lie_down(wait_time=time_sleep)
    elapsed = time.time() - start_time
    timing("装货过程", elapsed)

    section('任务1-15: 站起来', "移动")
    ctrl.base_msg.stand_up()

    section('任务1-16: 返回', "移动")
    go_straight(ctrl, msg, distance=-(move_distance + res['radius']), observe=observe)

    turn_degree_v2(ctrl, msg, degree=179, absolute=True)

    section('任务1-17: 90度转弯', "旋转")
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=-85 if direction else 85,
        radius=res['radius'] * 2,
        pass_align=True,
        observe=observe,
    )

    section('任务1-18: 直线移动', "移动")
    move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)

    section('任务1-19: 90度旋转', "旋转")
    turn_degree_v2(ctrl, msg, degree=0, absolute=True)

    go_straight(ctrl, msg, distance=-1.3, observe=observe)
    # go_to_xy(ctrl, msg, target_x=-0.2, target_y=0, observe=observe)  # TEST

    success("任务1-back完成", "完成")