using System;

namespace MultiWheelC.Control.Abstractions
{
    /// <summary>
    /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
    /// </summary>
    public readonly struct LateralControlCommand
    {
        /// <summary>
        /// 创建前、后GCP目标转角命令。
        /// </summary>
        public LateralControlCommand(
            double frontGcpAngleRadians,
            double rearGcpAngleRadians)
        {
            EnsureFinite(
                frontGcpAngleRadians,
                nameof(frontGcpAngleRadians));
            EnsureFinite(
                rearGcpAngleRadians,
                nameof(rearGcpAngleRadians));

            FrontGcpAngleRadians =
                frontGcpAngleRadians;
            RearGcpAngleRadians =
                rearGcpAngleRadians;
        }

        /// <summary>
        /// 获取前GCP目标转角，单位为rad，逆时针为正。
        /// </summary>
        public double FrontGcpAngleRadians { get; }

        /// <summary>
        /// 获取后GCP目标转角，单位为rad，逆时针为正。
        /// </summary>
        public double RearGcpAngleRadians { get; }

        /// <summary>
        /// 获取前后GCP的共同转角分量，主要用于横向平移修正。
        /// </summary>
        public double CommonAngleRadians =>
            (FrontGcpAngleRadians +
             RearGcpAngleRadians) / 2.0;

        /// <summary>
        /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
        /// </summary>
        public double DifferentialAngleRadians =>
            (FrontGcpAngleRadians -
             RearGcpAngleRadians) / 2.0;

        /// <summary>
        /// 创建前后GCP均保持车头方向的直线命令。
        /// </summary>
        public static LateralControlCommand Straight =>
            new LateralControlCommand(0.0, 0.0);

        /// <summary>
        /// 检查GCP目标转角是否为有限值。
        /// </summary>
        private static void EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "GCP目标转角必须是有限值。");
            }
        }
    }
}