import time
import sys
import os

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

from task_5.detect_arrow_direction import ArrowDetector

def run_task_5(ctrl, msg, image_processor=None):
    # 初始化箭头检测器
    detector = ArrowDetector() if image_processor is None else ArrowDetector(image_processor)
    
    try:
        # 设置俯身姿态
        msg.mode = 3  # 姿态控制模式
        msg.gait_id = 0
        msg.pos_des = [0, 0, 0.15]  # 设置较低的姿态高度
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(3, msg.gait_id)
        
        # 等待姿态稳定
        time.sleep(1.0)
        
        # 等待获取图像和检测箭头方向
        print("正在检测箭头方向...")
        time.sleep(2.0)  # 给检测器一些时间来获取图像
        
        # 检测箭头方向
        direction = detector.get_arrow_direction()
        print(f"检测到的箭头方向: {direction}")
        
        # 保存检测结果的可视化图像
        detector.visualize_current_detection("arrow_detection_result.jpg")
        
        # 根据箭头方向决定移动方向
        vel_x = 0.5  # 前进速度
        vel_y = 0.0  # 初始侧向速度为0
        
        if direction == "left":
            # 如果是左箭头，向左移动
            vel_y = 0.3  # 向左移动的速度
            print("根据箭头方向，向左移动")
        elif direction == "right":
            # 如果是右箭头，向右移动
            vel_y = -0.3  # 向右移动的速度
            print("根据箭头方向，向右移动")
        else:
            # 如果无法确定方向，直接前进
            print("无法确定箭头方向，直接前进")
        
        # 开始运动
        msg.mode = 11  # 运动控制模式
        msg.gait_id = 3  # 使用 trot 步态
        msg.vel_des = [vel_x, vel_y, 0]  # 设置移动速度，x和y方向
        msg.duration = 3000  # 运动持续3秒
        msg.step_height = [0.03, 0.03]  # 设置步高
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(11, msg.gait_id)
        
        # 恢复站立姿态
        msg.mode = 3
        msg.gait_id = 0
        msg.pos_des = [0, 0, 0.25]  # 恢复到正常站立高度
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(3, msg.gait_id)
    
    finally:
        # 清理资源
        detector.destroy()