using System;

namespace MultiWheelC.Control.Allocation
{
    /// <summary>
    /// 表示发送给旧版多舵轮四轮解算前的有符号速度和前后GCP角度命令。
    /// </summary>
    public readonly struct GcpMotionCommand
    {
        /// <summary>
        /// 创建统一使用m/s和rad的前后几何控制点运动命令。
        /// </summary>
        public GcpMotionCommand(
            double speedMetersPerSecond,
            double frontAngleRadians,
            double rearAngleRadians)
        {
            EnsureFinite(
                speedMetersPerSecond,
                nameof(speedMetersPerSecond));
            EnsureFinite(
                frontAngleRadians,
                nameof(frontAngleRadians));
            EnsureFinite(
                rearAngleRadians,
                nameof(rearAngleRadians));

            SpeedMetersPerSecond =
                speedMetersPerSecond;
            FrontAngleRadians =
                frontAngleRadians;
            RearAngleRadians =
                rearAngleRadians;
        }

        /// <summary>
        /// 获取准备交给底盘的有符号纵向速度，单位为m/s，正值表示前进。
        /// </summary>
        public double SpeedMetersPerSecond { get; }

        /// <summary>
        /// 获取前几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
        /// </summary>
        public double FrontAngleRadians { get; }

        /// <summary>
        /// 获取后几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
        /// </summary>
        public double RearAngleRadians { get; }

        /// <summary>
        /// 检查底盘中间命令是否为有限值。
        /// </summary>
        private static void EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "GCP运动命令必须由有限值组成。");
            }
        }
    }
}