using System;
using MultiWheelC.Control.Abstractions;
using MyParking.Shared;
// 限制目标角度的最大绝对值，例如不能超过60°。
namespace MultiWheelC.Control.Allocation
{
    /// <summary>
    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
    /// </summary>
    public sealed class GcpCommandAllocator
    {
        /// <summary>
        /// 创建使用指定前后GCP最大转角的命令分配器。
        /// </summary>
        public GcpCommandAllocator(double maximumGcpAngleRadians)
        {
            NumericGuard.EnsureFinitePositive(
                maximumGcpAngleRadians,
                nameof(maximumGcpAngleRadians));

            if (maximumGcpAngleRadians >= Math.PI / 2.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(maximumGcpAngleRadians),
                    "最大GCP转角必须小于π/2。");
            }

            MaximumGcpAngleRadians = maximumGcpAngleRadians;
        }

        /// <summary>
        /// 获取前后GCP允许的最大转角绝对值，单位为rad。
        /// </summary>
        public double MaximumGcpAngleRadians { get; }

        /// <summary>
        /// 将纵向速度和前后GCP转角组合为底盘运动命令。
        /// </summary>
        public GcpMotionCommand Allocate(
            double speedMetersPerSecond,
            LateralControlCommand lateralCommand)
        {
            NumericGuard.EnsureFinite(
                speedMetersPerSecond,
                nameof(speedMetersPerSecond));

            var frontAngleRadians = ClampSymmetric(
                lateralCommand.FrontGcpAngleRadians,
                MaximumGcpAngleRadians);
            var rearAngleRadians = ClampSymmetric(
                lateralCommand.RearGcpAngleRadians,
                MaximumGcpAngleRadians);

            return new GcpMotionCommand(
                speedMetersPerSecond,
                frontAngleRadians,
                rearAngleRadians);
        }

        /// <summary>
        /// 将数值按正负对称方式限制在指定绝对值内。
        /// </summary>
        private static double ClampSymmetric(
            double value,
            double maximumAbsoluteValue)
        {
            return Math.Max(
                -maximumAbsoluteValue,
                Math.Min(maximumAbsoluteValue, value));
        }

    }
}