using System;
using MyParking.Shared;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 保存车体中心投影到二维参考轨迹后得到的只读结果。
    /// </summary>
    public readonly struct TrajectoryProjection
    {
        /// <summary>
        /// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
        /// </summary>
        public TrajectoryProjection(
            int segmentStartIndex,
            TrajectoryPoint referencePoint,
            double lateralErrorMeters,
            double headingErrorRadians,
            double distanceToTrajectoryMeters,
            double remainingDistanceMeters)
        {
            if (segmentStartIndex < 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(segmentStartIndex),
                    "投影线段起点索引不能为负数。");
            }

            NumericGuard.EnsureFinite(
                lateralErrorMeters,
                nameof(lateralErrorMeters));
            NumericGuard.EnsureFinite(
                headingErrorRadians,
                nameof(headingErrorRadians));
            NumericGuard.EnsureFiniteNonNegative(
                distanceToTrajectoryMeters,
                nameof(distanceToTrajectoryMeters));
            NumericGuard.EnsureFiniteNonNegative(
                remainingDistanceMeters,
                nameof(remainingDistanceMeters));

            SegmentStartIndex = segmentStartIndex;
            ReferencePoint = referencePoint;
            LateralErrorMeters = lateralErrorMeters;
            HeadingErrorRadians =
                AngleMath.NormalizeRadians(
                    headingErrorRadians);
            DistanceToTrajectoryMeters =
                distanceToTrajectoryMeters;
            RemainingDistanceMeters =
                remainingDistanceMeters;
        }

        /// <summary>
        /// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
        /// </summary>
        public int SegmentStartIndex { get; }

        /// <summary>
        /// 获取投影位置插值得到的车体中心参考轨迹点。
        /// </summary>
        public TrajectoryPoint ReferencePoint { get; }

        /// <summary>
        /// 获取相对累计弧长增加方向的有符号横向误差，单位为m，参考轨迹位于该方向左侧时为正。
        /// </summary>
        public double LateralErrorMeters { get; }

        /// <summary>
        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
        /// </summary>
        public double HeadingErrorRadians { get; }

        /// <summary>
        /// 获取车体中心到投影点的欧氏距离，单位为m。
        /// </summary>
        public double DistanceToTrajectoryMeters { get; }

        /// <summary>
        /// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
        /// </summary>
        public double RemainingDistanceMeters { get; }

        /// <summary>
        /// 获取投影位置从轨迹起点累计的弧长，单位为m。
        /// </summary>
        public double ArcLengthMeters =>
            ReferencePoint.ArcLengthMeters;

    }
}