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

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

from base_move.turn_degree import turn_degree
from base_move.go_straight import go_straight
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 base_move.move_base_hori_line import go_straight_until_hori_line

from task_4.pass_bar import pass_bar

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

def run_task_4(ctrl, msg):
    """
    参数:
        ctrl: Robot_Ctrl对象
        msg: 控制消息对象
        image_processor: 可选的图像处理器实例
    """
    section('任务4-1：移动直到灰色天空比例小于阈值', "天空检测")
    go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)

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

    section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
    # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
    go_straight_until_hori_line(
        ctrl, 
        msg, 
        target_distance=0.5,   # 目标与黄线的距离(米)
        max_distance=8.0,      # 最大搜索距离(米)
        speed=0.35,            # 移动速度(米/秒)
        mode=11,               # 运动模式
        gait_id=3,             # 步态ID（快步跑）
        step_height=[0.21, 0.21],  # 摆动腿离地高度
        observe=True           # 显示调试信息
    )


def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, max_distance=10, speed=0.3):
    """
    控制机器人沿直线行走，直到灰色天空比例小于指定阈值
    
    参数:
        ctrl: Robot_Ctrl对象
        msg: 控制命令消息对象
        sky_ratio_threshold: 灰色天空比例阈值，当检测到的比例小于此值时停止
        step_distance: 每次移动的步长(米)
        max_distance: 最大移动距离(米)，防止无限前进
        speed: 移动速度(米/秒)
    
    返回:
        bool: 是否成功找到天空比例小于阈值的位置
    """
    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()
        if current_image is None:
            warning("无法获取图像，等待...", "图像")
            time.sleep(0.5)
            continue
        
        # 保存当前图像用于分析
        temp_image_path = "/tmp/current_sky_image.jpg"
        cv2.imwrite(temp_image_path, current_image)
        
        # 分析灰色天空比例
        try:
            sky_ratio = analyze_gray_sky_ratio(temp_image_path)
            info(f"当前灰色天空比例: {sky_ratio:.2%}", "分析")
            
            # 如果天空比例小于阈值，停止移动
            if sky_ratio < sky_ratio_threshold:
                success(f"检测到灰色天空比例({sky_ratio:.2%})小于阈值({sky_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} 米，但未找到天空比例小于 {sky_ratio_threshold:.2%} 的位置", "超时")
    
    return success_flag


def run_task_4_back(ctrl, msg):
    section('任务4-1：直线移动', "移动")
    # 设置机器人运动模式为快步跑
    msg.mode = 11  # 运动模式
    msg.gait_id = 3  # 步态ID（快步跑）
    msg.vel_des = [0.35, 0, 0]  # 期望速度
    msg.pos_des = [ 0, 0, 0]
    msg.duration = 0  # 零时长表示持续运动，直到接收到新命令
    msg.step_height = [0.21, 0.21]  # 持续运动时摆动腿的离地高度
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(5)  # 持续5秒钟

    section('任务4-2：移动直到灰色天空比例小于阈值', "天空检测")
    go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)

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