using System;
using MyParking.Shared;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 将实际车体中心位姿投影到二维离散参考轨迹，并支持按上次进度限制搜索范围。
    /// </summary>
    public static class TrajectoryProjector
    {
        private const double DistanceTieToleranceSquaredMeters =
            1e-12;

        /// <summary>
        /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
        /// </summary>
        public static TrajectoryProjection Project(
            Trajectory2D trajectory,
            Pose2D vehiclePoseInWorld)
        {
            ValidateProjectionInput(
                trajectory,
                vehiclePoseInWorld,
                nameof(vehiclePoseInWorld));

            return ProjectRange(
                trajectory,
                vehiclePoseInWorld,
                firstSegmentStartIndex: 0,
                lastSegmentStartIndex:
                    trajectory.Count - 2,
                preferredArcLengthMeters: null);
        }

        /// <summary>
        /// 以上次投影弧长为中心，仅在指定前后物理距离窗口内查找最近线段。
        /// </summary>
        public static TrajectoryProjection Project(
            Trajectory2D trajectory,
            Pose2D vehiclePoseInWorld,
            double previousArcLengthMeters,
            double maximumBackwardSearchDistanceMeters,
            double maximumForwardSearchDistanceMeters)
        {
            ValidateProjectionInput(
                trajectory,
                vehiclePoseInWorld,
                nameof(vehiclePoseInWorld));
            NumericGuard.EnsureFiniteNonNegative(
                previousArcLengthMeters,
                nameof(previousArcLengthMeters));
            NumericGuard.EnsureFiniteNonNegative(
                maximumBackwardSearchDistanceMeters,
                nameof(maximumBackwardSearchDistanceMeters));
            NumericGuard.EnsureFinitePositive(
                maximumForwardSearchDistanceMeters,
                nameof(maximumForwardSearchDistanceMeters));

            if (previousArcLengthMeters >
                trajectory.TotalLengthMeters)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(previousArcLengthMeters),
                    "上次投影弧长不能超过轨迹总长度。");
            }

            var searchStartArcLengthMeters =
                Math.Max(
                    0.0,
                    previousArcLengthMeters -
                    maximumBackwardSearchDistanceMeters);
            var searchEndArcLengthMeters =
                Math.Min(
                    trajectory.TotalLengthMeters,
                    previousArcLengthMeters +
                    maximumForwardSearchDistanceMeters);
            var firstSegmentStartIndex =
                trajectory.FindSegmentStartIndex(
                    searchStartArcLengthMeters);
            var lastSegmentStartIndex =
                trajectory.FindSegmentStartIndex(
                    searchEndArcLengthMeters);

            return ProjectRange(
                trajectory,
                vehiclePoseInWorld,
                firstSegmentStartIndex,
                lastSegmentStartIndex,
                previousArcLengthMeters);
        }

        /// <summary>
        /// 在闭区间线段索引范围内查找最近投影，并在距离并列时优先保持原进度。
        /// </summary>
        private static TrajectoryProjection ProjectRange(
            Trajectory2D trajectory,
            Pose2D vehiclePoseInWorld,
            int firstSegmentStartIndex,
            int lastSegmentStartIndex,
            double? preferredArcLengthMeters)
        {
            var bestSegmentStartIndex = 0;
            var bestInterpolationRatio = 0.0;
            var bestDistanceSquared =
                double.PositiveInfinity;
            var bestProgressDifferenceMeters =
                double.PositiveInfinity;

            for (var segmentStartIndex =
                     firstSegmentStartIndex;
                 segmentStartIndex <=
                     lastSegmentStartIndex;
                 segmentStartIndex++)
            {
                var segmentStart =
                    trajectory[segmentStartIndex];
                var segmentEnd =
                    trajectory[segmentStartIndex + 1];

                var segmentX =
                    segmentEnd.PoseInWorld.XMeters -
                    segmentStart.PoseInWorld.XMeters;
                var segmentY =
                    segmentEnd.PoseInWorld.YMeters -
                    segmentStart.PoseInWorld.YMeters;
                var segmentLengthSquared =
                    segmentX * segmentX +
                    segmentY * segmentY;

                var vehicleFromSegmentStartX =
                    vehiclePoseInWorld.XMeters -
                    segmentStart.PoseInWorld.XMeters;
                var vehicleFromSegmentStartY =
                    vehiclePoseInWorld.YMeters -
                    segmentStart.PoseInWorld.YMeters;

                var interpolationRatio =
                    InterpolationMath.Clamp01(
                        (vehicleFromSegmentStartX * segmentX +
                         vehicleFromSegmentStartY * segmentY) /
                        segmentLengthSquared);

                var projectedX =
                    InterpolationMath.Lerp(
                        segmentStart.PoseInWorld.XMeters,
                        segmentEnd.PoseInWorld.XMeters,
                        interpolationRatio);
                var projectedY =
                    InterpolationMath.Lerp(
                        segmentStart.PoseInWorld.YMeters,
                        segmentEnd.PoseInWorld.YMeters,
                        interpolationRatio);

                var projectionErrorX =
                    projectedX -
                    vehiclePoseInWorld.XMeters;
                var projectionErrorY =
                    projectedY -
                    vehiclePoseInWorld.YMeters;
                var distanceSquared =
                    projectionErrorX * projectionErrorX +
                    projectionErrorY * projectionErrorY;

                var progressDifferenceMeters =
                    preferredArcLengthMeters.HasValue
                        ? Math.Abs(
                            InterpolationMath.Lerp(
                                segmentStart.ArcLengthMeters,
                                segmentEnd.ArcLengthMeters,
                                interpolationRatio) -
                            preferredArcLengthMeters.Value)
                        : 0.0;
                var hasMeaningfullyShorterDistance =
                    distanceSquared <
                    bestDistanceSquared -
                    DistanceTieToleranceSquaredMeters;
                var hasEquivalentDistanceAndCloserProgress =
                    preferredArcLengthMeters.HasValue &&
                    Math.Abs(
                        distanceSquared -
                        bestDistanceSquared) <=
                    DistanceTieToleranceSquaredMeters &&
                    progressDifferenceMeters <
                    bestProgressDifferenceMeters;

                if (!hasMeaningfullyShorterDistance &&
                    !hasEquivalentDistanceAndCloserProgress)
                {
                    continue;
                }

                bestSegmentStartIndex =
                    segmentStartIndex;
                bestInterpolationRatio =
                    interpolationRatio;
                bestDistanceSquared = distanceSquared;
                bestProgressDifferenceMeters =
                    progressDifferenceMeters;
            }

            return BuildProjection(
                trajectory,
                vehiclePoseInWorld,
                bestSegmentStartIndex,
                bestInterpolationRatio,
                bestDistanceSquared);
        }

        /// <summary>
        /// 根据最近线段和插值比例生成控制器使用的完整投影结果。
        /// </summary>
        private static TrajectoryProjection BuildProjection(
            Trajectory2D trajectory,
            Pose2D vehiclePoseInWorld,
            int segmentStartIndex,
            double interpolationRatio,
            double distanceSquared)
        {
            var segmentStart =
                trajectory[segmentStartIndex];
            var segmentEnd =
                trajectory[segmentStartIndex + 1];
            var referencePoint =
                trajectory.InterpolateSegment(
                    segmentStartIndex,
                    interpolationRatio);

            var segmentX =
                segmentEnd.PoseInWorld.XMeters -
                segmentStart.PoseInWorld.XMeters;
            var segmentY =
                segmentEnd.PoseInWorld.YMeters -
                segmentStart.PoseInWorld.YMeters;
            var segmentLength =
                Math.Sqrt(
                    segmentX * segmentX +
                    segmentY * segmentY);

            // 以轨迹线段的前进方向判断左右：
            // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
            var vehicleToProjectionX =
                referencePoint.PoseInWorld.XMeters -
                vehiclePoseInWorld.XMeters;
            var vehicleToProjectionY =
                referencePoint.PoseInWorld.YMeters -
                vehiclePoseInWorld.YMeters;
            var lateralErrorMeters =
                (segmentX * vehicleToProjectionY -
                 segmentY * vehicleToProjectionX) /
                segmentLength;

            var headingErrorRadians =
                AngleMath.ShortestDifferenceRadians(
                    referencePoint.PoseInWorld.YawRadians,
                    vehiclePoseInWorld.YawRadians);

            return new TrajectoryProjection(
                segmentStartIndex,
                referencePoint,
                lateralErrorMeters,
                headingErrorRadians,
                Math.Sqrt(distanceSquared),
                trajectory.GetRemainingDistanceMeters(
                    referencePoint.ArcLengthMeters));
        }

        /// <summary>
        /// 检查轨迹对象和用于投影的实际车体中心位姿。
        /// </summary>
        private static void ValidateProjectionInput(
            Trajectory2D trajectory,
            Pose2D pose,
            string parameterName)
        {
            if (trajectory == null)
            {
                throw new ArgumentNullException(
                    nameof(trajectory));
            }

            NumericGuard.EnsureFinite(
                pose,
                parameterName);
        }
    }
}