import time
import sys
import os
import cv2
import numpy as np
import math

# 添加父目录到路径，以便能够导入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_straight import go_straight, go_lateral
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
from utils.gray_sky_analyzer import analyze_gray_sky_ratio
from utils.detect_track import detect_horizontal_track_edge
from utils.detect_dual_track_lines import detect_dual_track_lines
from base_move.move_base_hori_line import calculate_distance_to_line
from task_4.pass_bar import pass_bar
from base_move.center_on_dual_tracks import center_on_dual_tracks
from task_3.task_3 import pass_stone

# 创建本模块特定的日志记录器
logger = get_logger("任务4")

observe = True

STONE_DISTANCE = 4.0  # TODO 距离参数需要微调
RED_RATIO_THRESHOLD = 0.35  # TODO 红色区域比例阈值需要微调

def run_task_4(ctrl, msg):
    section('任务4-1：直线移动', "移动")
    pass_stone(ctrl, msg, distance=STONE_DISTANCE)                    

    section('任务4-2：移动直到红色区域比例大于阈值', "红色检测")
    go_straight_until_red_bar(ctrl, msg, red_ratio_threshold=RED_RATIO_THRESHOLD, speed=0.2)

    section('任务4-3：通过栏杆', "移动")
    pass_bar(ctrl, msg)

def run_task_4_back(ctrl, msg):
    """
    参数:
        ctrl: Robot_Ctrl对象
        msg: 控制消息对象
        image_processor: 可选的图像处理器实例
    """
    turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
    center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)

    go_straight(ctrl, msg, distance=2, speed=1, observe=True)
    center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)

    # TODO 向右移动0.5秒 (或许不需要了)
    # section('任务4-回程：向右移动', "移动")
    # go_lateral(ctrl, msg, distance=-0.1, speed=0.15, observe=True)  # DEBUG

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

    section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
    go_straight_until_red_bar(ctrl, msg, red_ratio_threshold=RED_RATIO_THRESHOLD, speed=0.2)

    section('任务4-2：通过栏杆', "移动")
    turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
    pass_bar(ctrl, msg)

    turn_degree_v2(ctrl, msg, degree=-90, absolute=True) 
    section('任务4-3：stone', "移动")
    go_straight(ctrl, msg, distance=1, speed=2)
    turn_degree_v2(ctrl, msg, degree=-90, absolute=True) 

    # Use enhanced calibration for better Y-axis correction on stone path
    # go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)
    pass_stone(ctrl, msg, distance=STONE_DISTANCE)
    # go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35, 
    #                                      mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True)

    section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
    # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
    # 获取相机高度
    camera_height = 0.355  # 单位: 米 # INFO from TF-tree
    edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=True, save_log=True)
    current_distance = calculate_distance_to_line(edge_info, camera_height, observe=True)
    go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21])

def go_straight_until_red_bar(ctrl, msg,
                                    red_ratio_threshold=0.2,
                                    step_distance=0.5,
                                    max_distance=5,
                                    speed=0.3
                                    ):
    """
    控制机器人沿直线行走，直到红色区域比例高于指定阈值

    参数:
        ctrl: Robot_Ctrl对象
        msg: 控制命令消息对象
        red_ratio_threshold: 红色区域比例阈值，当检测到的比例高于此值时停止
        step_distance: 每次移动的步长(米)
        max_distance: 最大移动距离(米)，防止无限前进
        speed: 移动速度(米/秒)

    返回:
        bool: 是否成功找到红色区域比例高于阈值的位置
    """
    def analyze_red_area_ratio(image):
        """
        分析图像中红色区域的占比
        输入: BGR图像
        返回: 红色区域占比 (0~1)
        """
        # 转换到HSV空间
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        # 红色有两个区间
        lower_red1 = np.array([0, 70, 50])
        upper_red1 = np.array([10, 255, 255])
        lower_red2 = np.array([160, 70, 50])
        upper_red2 = np.array([180, 255, 255])
        mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
        mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
        mask = cv2.bitwise_or(mask1, mask2)
        red_pixels = np.count_nonzero(mask)
        total_pixels = mask.shape[0] * mask.shape[1]
        ratio = red_pixels / total_pixels if total_pixels > 0 else 0
        return ratio

    total_distance = 0
    success_flag = False

    # 设置移动命令
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    msg.step_height = [0.06, 0.06]  # 抬腿高度

    while total_distance < max_distance:
        # 获取当前图像
        current_image = ctrl.image_processor.get_current_image('ai')
        if current_image is None:
            warning("无法获取图像，等待...", "图像")
            time.sleep(0.5)
            continue

        # 分析红色区域比例
        try:
            red_ratio = analyze_red_area_ratio(current_image)
            info(f"当前红色区域比例: {red_ratio:.2%}", "分析")

            # 如果红色区域比例高于阈值，停止移动
            if red_ratio > red_ratio_threshold:
                success(f"检测到红色区域比例({red_ratio:.2%})高于阈值({red_ratio_threshold:.2%})，停止移动", "完成")
                success_flag = True
                break

        except Exception as e:
            error(f"分析图像时出错: {e}", "错误")

        # 继续前进一段距离
        info(f"继续前进 {step_distance} 米...", "移动")

        # 设置移动速度和方向
        msg.vel_des = [speed, 0, 0]  # [前进速度, 侧向速度, 角速度]
        msg.duration = 0  # wait next cmd
        msg.life_count += 1

        # 发送命令
        ctrl.Send_cmd(msg)

        # 估算前进时间
        move_time = step_distance / speed
        time.sleep(move_time)

        # 累计移动距离
        total_distance += step_distance
        info(f"已移动总距离: {total_distance:.2f} 米", "距离")

    # 平滑停止
    if hasattr(ctrl.base_msg, 'stop_smooth'):
        ctrl.base_msg.stop_smooth()
    else:
        ctrl.base_msg.stop()

    if not success_flag and total_distance >= max_distance:
        warning(f"已达到最大移动距离 {max_distance} 米，但未找到红色区域比例高于 {red_ratio_threshold:.2%} 的位置", "超时")

    return success_flag