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 move_to_hori_line, arc_turn_around_hori_line, align_to_horizontal_line
from utils.detect_track import detect_horizontal_track_edge
from base_move.move_base_hori_line import calculate_distance_to_line

observe = True

def run_task_1(ctrl, msg):
    print('🐶 Running task 1...')

    # v2
    print('😺 task 1 - 1')
    arc_turn_around_hori_line(ctrl, msg, angle_deg=85, left=False, observe=observe)
    
    print('😺 task 1 - 2')
    # 使用内置的QR码扫描功能执行移动
    move_success, qr_result = move_to_hori_line(
        ctrl=ctrl,
        msg=msg,
        target_distance=1,
        observe=observe,
        scan_qrcode=True,  # 启用QR码扫描
        qr_check_interval=0.3  # 每0.3秒检查一次QR码结果
    )
    
    if qr_result:
        print(f"🎯 成功扫描到QR码: {qr_result}")
    else:
        print("⚠️ 未能扫描到任何QR码")

    print('😺 task 1 - 3')
    arc_turn_around_hori_line(ctrl, msg, angle_deg=180, target_distance=0.4, left=True, observe=observe)

    return

    print('😺 task 1 - 4')
    move_to_hori_line(ctrl, msg, observe=observe)


# 保留move_with_qr_scan函数作为备份，如果需要特殊处理可以使用
def move_with_qr_scan(ctrl, msg, target_distance=0.5, observe=False):
    """
    结合移动到指定位置和 QR 码扫描功能的函数，使用异步扫描
    
    参数:
        ctrl: Robot_Ctrl 对象
        msg: 机器人控制消息对象
        target_distance: 目标距离
        observe: 是否打印调试信息
    """
    # 直接使用内置的扫描功能
    return move_to_hori_line(
        ctrl=ctrl,
        msg=msg, 
        target_distance=target_distance,
        observe=observe,
        scan_qrcode=True,
        qr_check_interval=0.3
    )