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):
    """
    结合里程计实现精确稳定的旋转指定角度
    
    参数:
    ctrl: Robot_Ctrl 对象，包含里程计信息
    msg: robot_control_cmd_lcmt 对象，用于发送命令
    degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
    absolute: 是否绝对角度，默认为 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.04  # 约2.3度
    
    # 计算最短旋转方向和距离
    def circle_dist(target, location):
        value1 = abs(target - location)
        value2 = 2 * math.pi - value1
        direction1 = 1 if target > location else 0  # 1为逆时针，0为顺时针
        
        # 计算两个方向哪个距离更短
        if value1 < value2:
            return direction1, value1
        else:
            return 1 - direction1, value2
    
    # 获取旋转方向和距离
    direction, dist = circle_dist(target_yaw, current_yaw)
    info(f"开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "旋转")
    
    if abs(dist) > limit:
        # 主要转向
        const_int = 2470  # 转1.57弧度约需2470的duration值
        
        # 设置转向命令
        msg.mode = 11  # Locomotion模式
        msg.gait_id = 26  # 自变频步态
        msg.vel_des = [0, 0, 0.5 if direction > 0 else -0.5]  # 转向速度
        msg.duration = int(const_int * abs(dist))
        msg.step_height = [0.06, 0.06]  # 抬腿高度
        msg.life_count += 1
        
        # 发送命令
        ctrl.Send_cmd(msg)
        debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 持续时间={msg.duration}", "旋转")
        
        # 等待转向完成
        wait_time = 7 * abs(dist) / 1.57
        debug(f"等待旋转完成: {wait_time:.2f}秒", "时间")
        time.sleep(wait_time)
        
        # 获取当前角度
        current_yaw = ctrl.odo_msg.rpy[2]
        info(f"主要转向完成: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度")
        
        # 计算剩余误差
        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:
            warning(f"剩余误差: {math.degrees(remaining_dist):.2f}°, 需要进行微调", "角度")
            # 进行微调
            const_int_tiny = 1200  # 微调使用较小的系数
            
            # 设置微调命令
            msg.mode = 11
            msg.gait_id = 26
            msg.vel_des = [0, 0, 0.5 if remaining_dist > 0 else -0.5]
            msg.duration = int(const_int_tiny * abs(remaining_dist))
            msg.step_height = [0.06, 0.06]
            msg.life_count += 1
            
            # 发送命令
            ctrl.Send_cmd(msg)
            debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 持续时间={msg.duration}", "旋转")
            
            # 等待微调完成
            time.sleep(5)
            
            # 获取最终角度
            final_yaw = ctrl.odo_msg.rpy[2]
            info(f"微调完成: 最终角度={math.degrees(final_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度")
            
            final_error = abs(target_yaw - final_yaw)
            if final_error > math.pi:
                final_error = 2 * math.pi - final_error
            
            # 判断是否成功达到目标
            if final_error <= limit:
                success(f"旋转成功，误差在允许范围内: {math.degrees(final_error):.2f}°", "成功")
                return True
            else:
                warning(f"旋转完成，但误差超出允许范围: {math.degrees(final_error):.2f}° > {math.degrees(limit):.2f}°", "警告")
                return False
    
    success("旋转成功完成", "完成")
    return True