using System;
using MultiWheelC.Control.Abstractions;

namespace MultiWheelC.Control.Allocation
{
    /// <summary>
    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
    /// </summary>
    public sealed class GcpCommandAllocator
    {
        /// <summary>
        /// 创建使用指定前后GCP最大转角的命令分配器。
        /// </summary>
        public GcpCommandAllocator(double maximumGcpAngleRadians)
        {
            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)
        {
            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));
        }

        /// <summary>
        /// 检查参数是否为正有限值。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "GCP分配参数必须是正有限值。");
            }
        }

        /// <summary>
        /// 检查参数或命令是否为有限值。
        /// </summary>
        private static void EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "GCP分配参数和命令必须是有限值。");
            }
        }
    }
}