using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;

/// <summary>
/// 从车辆参数提取运动学限制的辅助方法。
/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
/// </summary>
public static class VehicleKinematics
{
    /// <summary>
    /// 尝试获取车辆允许的最大绝对曲率。
    /// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
    /// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
    /// </summary>
    public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
    {
        maximumCurvaturePerMeter = 0d;
        if (vehicle == null) return false;

        bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
        bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
        if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
        if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
        if (!hasMaximumCurvature && !hasMinimumRadius) return false;

        if (hasMaximumCurvature && hasMinimumRadius)
        {
            maximumCurvaturePerMeter = System.Math.Min(
                vehicle.MaximumCurvaturePerMeter.Value,
                1d / vehicle.MinimumTurningRadiusMeters.Value);
            return true;
        }

        maximumCurvaturePerMeter = hasMaximumCurvature
            ? vehicle.MaximumCurvaturePerMeter.Value
            : 1d / vehicle.MinimumTurningRadiusMeters.Value;
        return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
    }
}