using System;
using MyParking.Shared;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
    /// </summary>
    public readonly struct TrajectoryPoint
    {
        /// <summary>
        /// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
        /// </summary>
        public TrajectoryPoint(
            double arcLengthMeters,
            Pose2D poseInWorld,
            double curvaturePerMeter,
            double referenceSpeedMetersPerSecond)
        {
            EnsureFiniteNonNegative(
                arcLengthMeters,
                nameof(arcLengthMeters));
            EnsureFinite(
                poseInWorld.XMeters,
                nameof(poseInWorld));
            EnsureFinite(
                poseInWorld.YMeters,
                nameof(poseInWorld));
            EnsureFinite(
                poseInWorld.YawRadians,
                nameof(poseInWorld));
            EnsureFinite(
                curvaturePerMeter,
                nameof(curvaturePerMeter));
            EnsureFinite(
                referenceSpeedMetersPerSecond,
                nameof(referenceSpeedMetersPerSecond));
            ArcLengthMeters = arcLengthMeters;
            PoseInWorld = new Pose2D(
                poseInWorld.XMeters,
                poseInWorld.YMeters,
                AngleMath.NormalizeRadians(
                    poseInWorld.YawRadians));
            CurvaturePerMeter = curvaturePerMeter;
            ReferenceSpeedMetersPerSecond =
                referenceSpeedMetersPerSecond;
        }

        /// <summary>
        /// 获取从轨迹起点累计到当前点的弧长，单位为m。
        /// </summary>
        public double ArcLengthMeters { get; }

        /// <summary>
        /// 获取车体中心参考坐标系在世界坐标系中的位姿。
        /// </summary>
        public Pose2D PoseInWorld { get; }

        /// <summary>
        /// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
        /// </summary>
        public double CurvaturePerMeter { get; }

        /// <summary>
        /// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
        /// </summary>
        public double ReferenceSpeedMetersPerSecond { get; }

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

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

            if (value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "轨迹累计弧长不能为负数。");
            }
        }
    }
}