"""工具函数模块

提供不依赖 omni.isaac 的纯工具函数。
"""

import math
from pathlib import Path

import numpy as np


def clamp(value, low, high):
    """将值限制在指定范围内"""
    return max(low, min(high, value))


def resolve_topic(prefix, suffix):
    """解析 ROS 话题名"""
    normalized_prefix = prefix.rstrip("/")
    normalized_suffix = suffix if suffix.startswith("/") else f"/{suffix}"
    return f"{normalized_prefix}{normalized_suffix}" if normalized_prefix else normalized_suffix


def quat_wxyz_to_yaw(quat_wxyz):
    """从四元数 (w, x, y, z) 提取偏航角"""
    w, x, y, z = quat_wxyz
    siny_cosp = 2.0 * (w * z + x * y)
    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
    return math.atan2(siny_cosp, cosy_cosp)


def normalize_angle(angle):
    """将角度归一化到 [-pi, pi] 范围"""
    return math.atan2(math.sin(angle), math.cos(angle))


def chassis_type_value(name, chassis_type_cls=None):
    """将底盘类型名称转换为枚举值"""
    if chassis_type_cls is None:
        return 0
    mapping = {
        "ackermann": chassis_type_cls.ACKERMANN,
        "differential": chassis_type_cls.DIFFERENTIAL,
        "single_steer": chassis_type_cls.SINGLE_STEER_WHEEL,
        "multi_steer": chassis_type_cls.MULTI_STEER_WHEEL,
    }
    return mapping.get(name, chassis_type_cls.CHASSIS_TYPE_UNSPECIFIED)


def controller_algorithm_value(name, controller_type_cls=None):
    """将控制器算法名称转换为枚举值"""
    if controller_type_cls is None:
        return 0
    mapping = {
        "pid": controller_type_cls.PID,
        "mpc": controller_type_cls.MPC,
        "lqr": controller_type_cls.LQR,
        "pure_pursuit": controller_type_cls.PURE_PURSUIT,
    }
    return mapping.get(name, controller_type_cls.CONTROLLER_ALGORITHM_UNSPECIFIED)


def vehicle_state_from_isaac(agv):
    """从 Isaac Sim 车辆对象获取状态"""
    position, quat = agv.get_world_pose()
    yaw_rad = quat_wxyz_to_yaw(quat)
    linear_velocity = agv.get_linear_velocity()
    try:
        angular_velocity = agv.get_angular_velocity()
    except Exception:
        angular_velocity = np.array([0.0, 0.0, 0.0])
    return position, yaw_rad, linear_velocity, angular_velocity


def make_mesh_path_absolute(mesh_filename, source_dir):
    """将相对网格路径转换为绝对路径"""
    mesh_path = Path(mesh_filename)
    if mesh_path.is_absolute():
        return str(mesh_path)
    return str((source_dir / mesh_path).resolve())
