using System;
using MultiWheelC.Control.Abstractions;

namespace MultiWheelC.Control.Allocation
{
    /// <summary>
    /// 将车体中心目标曲率按对称前后转向策略转换为旧版底盘的前后GCP方向。
    /// </summary>
    public sealed class AckermannGcpAllocator
    {
        /// <summary>
        /// 创建使用指定GCP半间距和最大GCP转角的对称转向分配器。
        /// </summary>
        public AckermannGcpAllocator(
            double controlPointRadiusMeters,
            double maximumGcpAngleRadians)
        {
            EnsureFinitePositive(
                controlPointRadiusMeters,
                nameof(controlPointRadiusMeters));
            EnsureFinitePositive(
                maximumGcpAngleRadians,
                nameof(maximumGcpAngleRadians));

            if (maximumGcpAngleRadians >=
                Math.PI / 2.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(maximumGcpAngleRadians),
                    "最大GCP转角必须小于π/2，避免曲率换算出现奇异值。");
            }

            ControlPointRadiusMeters =
                controlPointRadiusMeters;
            MaximumGcpAngleRadians =
                maximumGcpAngleRadians;
        }

        /// <summary>
        /// 获取车体中心到前、后GCP的距离，单位为m。
        /// </summary>
        public double ControlPointRadiusMeters { get; }

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

        /// <summary>
        /// 获取当前GCP几何和转角限制允许的最大车体中心曲率，单位为1/m。
        /// </summary>
        public double MaximumCurvaturePerMeter =>
            Math.Tan(MaximumGcpAngleRadians) /
            ControlPointRadiusMeters;

        /// <summary>
        /// 将纵向命令速度和车体中心目标曲率分配为前后GCP运动命令。
        /// </summary>
        public GcpMotionCommand Allocate(
            double speedMetersPerSecond,
            LateralControlCommand lateralCommand)
        {
            EnsureFinite(
                speedMetersPerSecond,
                nameof(speedMetersPerSecond));

            var limitedCurvaturePerMeter =
                Clamp(
                    lateralCommand
                        .TargetCurvaturePerMeter,
                    -MaximumCurvaturePerMeter,
                    MaximumCurvaturePerMeter);

            var frontAngleRadians =
                Math.Atan(
                    limitedCurvaturePerMeter *
                    ControlPointRadiusMeters);
            var rearAngleRadians =
                -frontAngleRadians;

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

        /// <summary>
        /// 将数值限制在指定闭区间内。
        /// </summary>
        private static double Clamp(
            double value,
            double minimum,
            double maximum)
        {
            return Math.Max(
                minimum,
                Math.Min(maximum, 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分配参数和命令必须是有限值。");
            }
        }
    }
}