using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using MyParking.Shared;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 保存一条经过基本合法性检查的只读二维参考轨迹。
    /// </summary>
    public sealed class Trajectory2D
    {
        private const double StartArcLengthToleranceMeters = 1e-9;
        private const double MinimumSegmentLengthMeters = 1e-6;

        private readonly TrajectoryPoint[] _points;
        private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;

        /// <summary>
        /// 复制并验证按累计弧长升序排列的参考轨迹点。
        /// </summary>
        public Trajectory2D(
            IEnumerable<TrajectoryPoint> points)
        {
            if (points == null)
            {
                throw new ArgumentNullException(
                    nameof(points));
            }

            _points = points.ToArray();

            if (_points.Length < 2)
            {
                throw new ArgumentException(
                    "二维轨迹至少需要两个轨迹点。",
                    nameof(points));
            }

            if (Math.Abs(_points[0].ArcLengthMeters) >
                StartArcLengthToleranceMeters)
            {
                throw new ArgumentException(
                    "二维轨迹起点的累计弧长必须为0m。",
                    nameof(points));
            }

            for (var index = 1;
                 index < _points.Length;
                 index++)
            {
                ValidateSegment(
                    _points[index - 1],
                    _points[index],
                    index,
                    nameof(points));
            }

            _readOnlyPoints =
                Array.AsReadOnly(_points);
        }

        /// <summary>
        /// 获取轨迹点数量。
        /// </summary>
        public int Count => _points.Length;

        /// <summary>
        /// 获取指定索引处的轨迹点。
        /// </summary>
        public TrajectoryPoint this[int index] =>
            _points[index];

        /// <summary>
        /// 获取不可修改的有序轨迹点集合。
        /// </summary>
        public IReadOnlyList<TrajectoryPoint> Points =>
            _readOnlyPoints;

        /// <summary>
        /// 获取轨迹起点。
        /// </summary>
        public TrajectoryPoint StartPoint =>
            _points[0];

        /// <summary>
        /// 获取轨迹终点。
        /// </summary>
        public TrajectoryPoint EndPoint =>
            _points[_points.Length - 1];

        /// <summary>
        /// 获取轨迹总弧长，单位为m。
        /// </summary>
        public double TotalLengthMeters =>
            EndPoint.ArcLengthMeters;

        /// <summary>
        /// 根据当前累计弧长计算到轨迹终点的剩余距离。
        /// </summary>
        public double GetRemainingDistanceMeters(
            double arcLengthMeters)
        {
            EnsureFinite(
                arcLengthMeters,
                nameof(arcLengthMeters));

            if (arcLengthMeters <= 0.0)
                return TotalLengthMeters;

            if (arcLengthMeters >= TotalLengthMeters)
                return 0.0;

            return TotalLengthMeters - arcLengthMeters;
        }

        /// <summary>
        /// 按累计弧长在线性位置、航向、曲率和参考速度之间插值得到轨迹点。
        /// </summary>
        public TrajectoryPoint SampleAtArcLength(
            double arcLengthMeters)
        {
            EnsureFinite(
                arcLengthMeters,
                nameof(arcLengthMeters));

            if (arcLengthMeters <= 0.0)
            {
                return StartPoint;
            }

            if (arcLengthMeters >= TotalLengthMeters)
            {
                return EndPoint;
            }

            var segmentStartIndex =
                FindSegmentStartIndex(
                    arcLengthMeters);
            var segmentStart =
                _points[segmentStartIndex];
            var segmentEnd =
                _points[segmentStartIndex + 1];
            var interpolationRatio =
                (arcLengthMeters -
                 segmentStart.ArcLengthMeters) /
                (segmentEnd.ArcLengthMeters -
                 segmentStart.ArcLengthMeters);

            return new TrajectoryPoint(
                arcLengthMeters,
                new Pose2D(
                    InterpolationMath.Lerp(
                        segmentStart.PoseInWorld.XMeters,
                        segmentEnd.PoseInWorld.XMeters,
                        interpolationRatio),
                    InterpolationMath.Lerp(
                        segmentStart.PoseInWorld.YMeters,
                        segmentEnd.PoseInWorld.YMeters,
                        interpolationRatio),
                    AngleMath.LerpRadians(
                        segmentStart.PoseInWorld.YawRadians,
                        segmentEnd.PoseInWorld.YawRadians,
                        interpolationRatio)),
                InterpolationMath.Lerp(
                    segmentStart.CurvaturePerMeter,
                    segmentEnd.CurvaturePerMeter,
                    interpolationRatio),
                InterpolationMath.Lerp(
                    segmentStart.ReferenceSpeedMetersPerSecond,
                    segmentEnd.ReferenceSpeedMetersPerSecond,
                    interpolationRatio));
        }

        /// <summary>
        /// 使用二分查找获取包含指定累计弧长的线段起点索引。
        /// </summary>
        private int FindSegmentStartIndex(
            double arcLengthMeters)
        {
            var lowerIndex = 0;
            var upperIndex = _points.Length - 1;

            while (upperIndex - lowerIndex > 1)
            {
                var middleIndex =
                    lowerIndex +
                    (upperIndex - lowerIndex) / 2;

                if (_points[middleIndex].ArcLengthMeters <=
                    arcLengthMeters)
                {
                    lowerIndex = middleIndex;
                }
                else
                {
                    upperIndex = middleIndex;
                }
            }

            return lowerIndex;
        }

        /// <summary>
        /// 检查相邻轨迹点是否构成有效的非零长度有序线段。
        /// </summary>
        private static void ValidateSegment(
            TrajectoryPoint previous,
            TrajectoryPoint current,
            int currentIndex,
            string parameterName)
        {
            if (current.ArcLengthMeters <=
                previous.ArcLengthMeters)
            {
                throw new ArgumentException(
                    $"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
                    parameterName);
            }

            var deltaX =
                current.PoseInWorld.XMeters -
                previous.PoseInWorld.XMeters;
            var deltaY =
                current.PoseInWorld.YMeters -
                previous.PoseInWorld.YMeters;
            var segmentLengthSquared =
                deltaX * deltaX +
                deltaY * deltaY;
            var minimumLengthSquared =
                MinimumSegmentLengthMeters *
                MinimumSegmentLengthMeters;

            if (segmentLengthSquared <
                minimumLengthSquared)
            {
                throw new ArgumentException(
                    $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
                    parameterName);
            }
        }

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