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 const double ArcLengthConsistencyAbsoluteToleranceMeters =
            1e-6;
        private const double ArcLengthConsistencyRelativeTolerance =
            0.01;

        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)
        {
            NumericGuard.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)
        {
            NumericGuard.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 InterpolateSegment(
                segmentStartIndex,
                interpolationRatio);
        }

        /// <summary>
        /// 在指定线段上统一插值位置、车头航向、曲率和有符号参考速度。
        /// </summary>
        internal TrajectoryPoint InterpolateSegment(
            int segmentStartIndex,
            double interpolationRatio)
        {
            if (segmentStartIndex < 0 ||
                segmentStartIndex >= _points.Length - 1)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(segmentStartIndex),
                    "轨迹插值线段索引必须指向一条有效线段的起点。");
            }

            NumericGuard.EnsureFinite(
                interpolationRatio,
                nameof(interpolationRatio));

            if (interpolationRatio < 0.0 ||
                interpolationRatio > 1.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(interpolationRatio),
                    "轨迹线段插值比例必须位于[0,1]范围内。");
            }

            var segmentStart =
                _points[segmentStartIndex];
            var segmentEnd =
                _points[segmentStartIndex + 1];
            var arcLengthMeters =
                InterpolationMath.Lerp(
                    segmentStart.ArcLengthMeters,
                    segmentEnd.ArcLengthMeters,
                    interpolationRatio);

            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>
        internal int FindSegmentStartIndex(
            double arcLengthMeters)
        {
            NumericGuard.EnsureFinite(
                arcLengthMeters,
                nameof(arcLengthMeters));

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

            if (arcLengthMeters >= TotalLengthMeters)
            {
                return _points.Length - 2;
            }

            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);
            }

            var segmentLengthMeters =
                Math.Sqrt(segmentLengthSquared);
            var arcLengthIncrementMeters =
                current.ArcLengthMeters -
                previous.ArcLengthMeters;
            var maximumAllowedDifferenceMeters =
                Math.Max(
                    ArcLengthConsistencyAbsoluteToleranceMeters,
                    ArcLengthConsistencyRelativeTolerance *
                    Math.Max(
                        segmentLengthMeters,
                        arcLengthIncrementMeters));

            // 当前轨迹在相邻采样点之间按直线段投影，因此累计弧长增量
            // 必须与该离散线段长度近似一致，防止进度和实际几何脱节。
            if (Math.Abs(
                    arcLengthIncrementMeters -
                    segmentLengthMeters) >
                maximumAllowedDifferenceMeters)
            {
                throw new ArgumentException(
                    $"轨迹点{currentIndex}的累计弧长增量" +
                    $"{arcLengthIncrementMeters:F6}m与离散线段长度" +
                    $"{segmentLengthMeters:F6}m不一致。",
                    parameterName);
            }
        }
    }
}
