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
)
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_1(ctrl, msg):
    section('任务1：寻找横线并绕行', "启动")
    info('开始执行任务1...', "启动")

    # v2
    section('任务1-1：转弯并扫描QR码', "移动")
    # 在 arc_turn_around_hori_line 中启用 QR 码扫描
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=-85,  # 负值表示右转
        #
        observe=observe,
        scan_qrcode=True,  # 启用 QR 码扫描
        qr_check_interval=0.3  # 每 0.3 秒检查一次扫描结果
    )
    
    if res and res['qr_result']:
        success(f"在任务1-1中成功扫描到QR码: {res['qr_result']}", "扫描")
    else:
        warning("任务1-1中未能扫描到任何QR码", "警告")

    section('任务1-2：移动到横线', "移动")
    # 执行常规的移动操作，不需要 QR 码扫描
    move_to_hori_line(ctrl, msg, target_distance=1, observe=observe)

    section('任务1-3：转弯', "旋转")
    direction = False # if res['qr_result'] == 'A-2' else True
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=-170,  # 负值表示右转
        target_distance=0.5,
        #
        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)
    else:
        go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)

    return

    section('任务1-5：模拟装货', "停止")
    info('机器人躺下，模拟装货过程', "信息")
    start_time = time.time()
    ctrl.base_msg.lie_down(wait_time=3000)  # TODO 比赛时改成 5s
    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'] * 2), speed=0.3, observe=observe)

    # turn and back
    turn_degree(ctrl, msg, degree=-90)

    section('任务1-7：90度转弯', "旋转")
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        radius=res['radius'] * 2,
        angle_deg=85,
        #
        pass_align=True,
        observe=observe
    )

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

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

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