import math
import time
import sys
import os

sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

# 创建本模块特定的日志记录器
logger = get_logger("旋转控制")

def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False):
    """
    结合里程计实现精确稳定的旋转指定角度
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
    absolute: 是否绝对角度，默认为 False
    precision: 是否启用高精度模式，该模式下会使用更慢的速度和更细致的微调，默认为 False

    返回:
    Bool: 是否成功旋转到指定角度
    """
    # 将角度转换为弧度
    target_rad = math.radians(degree)
    
    # 获取当前朝向
    current_yaw = ctrl.odo_msg.rpy[2]
    
    # 计算目标朝向
    if absolute:
        target_yaw = target_rad
    else:
        target_yaw = current_yaw + target_rad
    
    # 标准化目标角度到 [-pi, pi] 范围
    if target_yaw > math.pi:
        target_yaw -= 2 * math.pi
    if target_yaw < -math.pi:
        target_yaw += 2 * math.pi
    
    # 定义允许的误差范围（弧度）
    # 如果是精确模式，使用更小的误差阈值
    limit = 0.03 if precision else 0.04  # 约1.7度或2.3度
    
    # 确定最短旋转方向
    remaining_dist = target_yaw - current_yaw
    if remaining_dist > math.pi:
        remaining_dist -= 2 * math.pi
    elif remaining_dist < -math.pi:
        remaining_dist += 2 * math.pi
    
    # 如果误差已经在允许范围内，无需旋转
    if abs(remaining_dist) <= limit:
        success(f"当前角度误差已在允许范围内，无需旋转", "成功")
        return True
    
    # 确定旋转方向和速度
    turn_direction = 1 if remaining_dist > 0 else -1
    
    # 根据精确模式选择旋转速度
    if precision:
        # 精确模式下，角度较大时使用中等速度，较小时使用慢速
        if abs(remaining_dist) > 0.5:  # 大约28度以上
            turn_speed = 0.25
        else:
            turn_speed = 0.15
    else:
        # 非精确模式下，角度较大时使用快速，较小时使用中等速度
        if abs(remaining_dist) > 0.5:  # 大约28度以上
            turn_speed = 0.5
        else:
            turn_speed = 0.3
    
    info(f"开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°, 需旋转{math.degrees(abs(remaining_dist)):.2f}°", "旋转")
    
    # 发送旋转命令（持续旋转）
    msg.mode = 11  # Locomotion模式
    msg.gait_id = 26  # 自变频步态
    msg.vel_des = [0, 0, turn_speed * turn_direction]  # 转向速度和方向
    msg.duration = 30000  # 设置一个大的持续时间值，后面会主动停止
    msg.step_height = [0.06, 0.06]  # 抬腿高度
    msg.life_count += 1
    
    # 发送命令
    ctrl.Send_cmd(msg)
    debug(f"开始持续旋转：方向={'逆时针' if turn_direction > 0 else '顺时针'}, 速度={turn_speed}", "旋转")
    
    # 等待并监测旋转进度
    start_time = time.time()
    last_yaw = current_yaw
    stable_count = 0
    
    # 设置最大超时时间（秒）防止无限等待
    max_timeout = 15 if abs(remaining_dist) > 1.0 else 10
    
    while True:
        time.sleep(0.05)  # 高频监测旋转状态
        
        # 获取当前角度
        current_yaw_now = ctrl.odo_msg.rpy[2]
        
        # 计算当前与目标的角度差
        current_error = target_yaw - current_yaw_now
        if current_error > math.pi:
            current_error -= 2 * math.pi
        elif current_error < -math.pi:
            current_error += 2 * math.pi
            
        # 计算已旋转角度（用于检测机器人是否在移动）
        rotated = current_yaw_now - last_yaw
        if abs(rotated) > math.pi:
            rotated = 2 * math.pi - abs(rotated)
            rotated *= -1 if current_yaw_now > last_yaw else 1
            
        last_yaw = current_yaw_now
        
        # 每0.5秒打印一次进度
        elapsed = time.time() - start_time
        if int(elapsed * 2) % 10 == 0:  # 每5秒
            debug(f"旋转进度: {elapsed:.1f}s, 剩余角度: {math.degrees(abs(current_error)):.2f}°", "进度")
            
        # 判断是否已经接近目标角度
        if abs(current_error) <= limit:
            # 立即发送停止命令
            msg.mode = 11
            msg.gait_id = 26
            msg.vel_des = [0, 0, 0]  # 停止旋转
            msg.duration = 200  # 短暂停止命令
            msg.life_count += 1
            ctrl.Send_cmd(msg)
            
            success(f"旋转成功，误差在允许范围内: {math.degrees(abs(current_error)):.2f}°", "成功")
            return True
            
        # 检测是否接近目标角度，需要减速
        if abs(current_error) < 0.3 and abs(turn_speed) > 0.15:  # 约17度内减速
            turn_speed = 0.15 * (1 if turn_speed > 0 else -1)
            msg.vel_des = [0, 0, turn_speed]
            msg.life_count += 1
            ctrl.Send_cmd(msg)
            debug(f"接近目标，减速至: {turn_speed}", "减速")
            
        # 如果旋转方向不对（过头或方向错误），修正方向
        if (current_error > 0 and turn_direction < 0) or (current_error < 0 and turn_direction > 0):
            turn_direction = 1 if current_error > 0 else -1
            turn_speed = 0.15  # 方向调整时使用低速
            msg.vel_des = [0, 0, turn_speed * turn_direction]
            msg.life_count += 1
            ctrl.Send_cmd(msg)
            debug(f"调整旋转方向: {'逆时针' if turn_direction > 0 else '顺时针'}, 速度={turn_speed}", "方向")
        
        # 检查超时
        if elapsed > max_timeout:
            # 发送停止命令
            msg.mode = 11
            msg.gait_id = 26
            msg.vel_des = [0, 0, 0]  # 停止旋转
            msg.duration = 500
            msg.life_count += 1
            ctrl.Send_cmd(msg)
            
            error(f"旋转超时，已停止。当前误差: {math.degrees(abs(current_error)):.2f}°", "超时")
            return False
        
        # 检测机器人是否卡住（长时间无明显角度变化）
        if abs(rotated) < 0.005:  # 角度变化很小
            stable_count += 1
        else:
            stable_count = 0
            
        # 如果连续多次检测到角度几乎不变，可能卡住了
        if stable_count > 30:  # 连续30次检测（约1.5秒）角度几乎不变
            # 尝试增加速度或改变方向
            if abs(turn_speed) < 0.5:
                turn_speed = 0.5 * turn_direction
                msg.vel_des = [0, 0, turn_speed]
                msg.life_count += 1
                ctrl.Send_cmd(msg)
                debug(f"检测到旋转停滞，增加速度至: {turn_speed}", "调整")
                stable_count = 0
            else:
                # 如果速度已经很大还是卡住，可能有障碍，停止旋转
                msg.vel_des = [0, 0, 0]  # 停止旋转
                msg.life_count += 1
                ctrl.Send_cmd(msg)
                error(f"检测到旋转异常，已停止。当前误差: {math.degrees(abs(current_error)):.2f}°", "异常")
                return False