using System;
using System.Collections.Generic;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC
{
    /// <summary>
    /// 为新版控制器实验生成不依赖正式规划层的简单世界坐标系参考轨迹。
    /// </summary>
    public static class TestTrajectoryFactory
    {
        private const double StraightLengthMeters = 4.0;

        /// <summary>
        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成带梯形速度规划的4m直线轨迹。
        /// </summary>
        public static Trajectory2D CreateStraight4Meters(
            Pose2D startPoseInWorld,
            double cruiseSpeedMetersPerSecond = 0.30,
            double accelerationMetersPerSecondSquared = 0.20,
            double decelerationMetersPerSecondSquared = 0.20,
            double pointSpacingMeters = 0.02)
        {
            return CreateStraight(
                startPoseInWorld,
                StraightLengthMeters,
                cruiseSpeedMetersPerSecond,
                accelerationMetersPerSecondSquared,
                decelerationMetersPerSecondSquared,
                pointSpacingMeters);
        }

        /// <summary>
        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成指定长度并在终点停车的直线轨迹。
        /// </summary>
        public static Trajectory2D CreateStraight(
            Pose2D startPoseInWorld,
            double lengthMeters,
            double cruiseSpeedMetersPerSecond = 0.30,
            double accelerationMetersPerSecondSquared = 0.20,
            double decelerationMetersPerSecondSquared = 0.20,
            double pointSpacingMeters = 0.02)
        {
            NumericGuard.EnsureFinite(
                startPoseInWorld,
                nameof(startPoseInWorld));
            NumericGuard.EnsureFinitePositive(
                lengthMeters,
                nameof(lengthMeters));
            var travelDirection = GetTravelDirection(
                cruiseSpeedMetersPerSecond,
                nameof(cruiseSpeedMetersPerSecond));
            NumericGuard.EnsureFinitePositive(
                accelerationMetersPerSecondSquared,
                nameof(accelerationMetersPerSecondSquared));
            NumericGuard.EnsureFinitePositive(
                decelerationMetersPerSecondSquared,
                nameof(decelerationMetersPerSecondSquared));
            NumericGuard.EnsureFinitePositive(
                pointSpacingMeters,
                nameof(pointSpacingMeters));

            if (pointSpacingMeters > lengthMeters)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(pointSpacingMeters),
                    "直线轨迹点间距不能大于轨迹总长度。");
            }

            var segmentCount = (int)Math.Ceiling(
                lengthMeters /
                pointSpacingMeters);
            var points = new List<TrajectoryPoint>(
                segmentCount + 1);
            var directionX = travelDirection *
                Math.Cos(startPoseInWorld.YawRadians);
            var directionY = travelDirection *
                Math.Sin(startPoseInWorld.YawRadians);

            for (var index = 0;
                 index <= segmentCount;
                 index++)
            {
                // 均分后最后一个点严格落在指定终点，避免浮点累加越界。
                var arcLengthMeters =
                    lengthMeters *
                    index /
                    segmentCount;
                var remainingDistanceMeters =
                    lengthMeters -
                    arcLengthMeters;
                var referenceSpeedMetersPerSecond =
                    CalculateReferenceSpeed(
                        arcLengthMeters,
                        remainingDistanceMeters,
                        cruiseSpeedMetersPerSecond,
                        accelerationMetersPerSecondSquared,
                        decelerationMetersPerSecondSquared);

                points.Add(
                    new TrajectoryPoint(
                        arcLengthMeters,
                        new Pose2D(
                            startPoseInWorld.XMeters +
                                directionX * arcLengthMeters,
                            startPoseInWorld.YMeters +
                                directionY * arcLengthMeters,
                            startPoseInWorld.YawRadians),
                        curvaturePerMeter: 0.0,
                        referenceSpeedMetersPerSecond:
                            referenceSpeedMetersPerSecond));
            }

            return new Trajectory2D(points);
        }

        /// <summary>
        /// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
        /// </summary>
        public static Trajectory2D CreateStraightLeftSemicircleStraight(
            Pose2D startPoseInWorld,
            double straightLengthMeters = 3.0,
            double turnRadiusMeters = 2.0,
            double curvatureTransitionLengthMeters = 0.60,
            double straightMaximumSpeedMetersPerSecond = 0.30,
            double semicircleMaximumSpeedMetersPerSecond = 0.25,
            double accelerationMetersPerSecondSquared = 0.20,
            double decelerationMetersPerSecondSquared = 0.12,
            double pointSpacingMeters = 0.02)
        {
            return CreateStraightSmoothLeftTurnStraight(
                startPoseInWorld,
                straightLengthMeters,
                turnRadiusMeters,
                Math.PI,
                curvatureTransitionLengthMeters,
                straightMaximumSpeedMetersPerSecond,
                semicircleMaximumSpeedMetersPerSecond,
                accelerationMetersPerSecondSquared,
                decelerationMetersPerSecondSquared,
                pointSpacingMeters);
        }

        /// <summary>
        /// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
        /// </summary>
        public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
            Pose2D startPoseInWorld,
            double straightLengthMeters,
            double turnRadiusMeters,
            double turnAngleRadians,
            double curvatureTransitionLengthMeters,
            double straightMaximumSpeedMetersPerSecond,
            double turnMaximumSpeedMetersPerSecond,
            double accelerationMetersPerSecondSquared,
            double decelerationMetersPerSecondSquared,
            double pointSpacingMeters)
        {
            NumericGuard.EnsureFinite(
                startPoseInWorld,
                nameof(startPoseInWorld));
            NumericGuard.EnsureFinitePositive(
                straightLengthMeters,
                nameof(straightLengthMeters));
            NumericGuard.EnsureFinitePositive(
                turnRadiusMeters,
                nameof(turnRadiusMeters));
            NumericGuard.EnsureFinitePositive(
                turnAngleRadians,
                nameof(turnAngleRadians));
            NumericGuard.EnsureFinitePositive(
                curvatureTransitionLengthMeters,
                nameof(curvatureTransitionLengthMeters));
            var travelDirection = GetCommonTravelDirection(
                straightMaximumSpeedMetersPerSecond,
                nameof(straightMaximumSpeedMetersPerSecond),
                turnMaximumSpeedMetersPerSecond,
                nameof(turnMaximumSpeedMetersPerSecond));
            NumericGuard.EnsureFinitePositive(
                accelerationMetersPerSecondSquared,
                nameof(accelerationMetersPerSecondSquared));
            NumericGuard.EnsureFinitePositive(
                decelerationMetersPerSecondSquared,
                nameof(decelerationMetersPerSecondSquared));
            NumericGuard.EnsureFinitePositive(
                pointSpacingMeters,
                nameof(pointSpacingMeters));

            if (turnAngleRadians > 2.0 * Math.PI)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(turnAngleRadians),
                    "单段平滑左转角度不能大于2π。");
            }

            var nominalTurnArcLengthMeters =
                turnAngleRadians * turnRadiusMeters;
            var constantCurvatureLengthMeters =
                nominalTurnArcLengthMeters -
                curvatureTransitionLengthMeters;

            if (constantCurvatureLengthMeters <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(curvatureTransitionLengthMeters),
                    "曲率过渡段长度必须小于指定转角对应的圆弧长度。");
            }

            // 两段平滑过渡的平均曲率均为最大曲率的一半；
            // 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于指定转角。
            var turnLengthMeters =
                2.0 * curvatureTransitionLengthMeters +
                constantCurvatureLengthMeters;
            var turnStartArcLengthMeters =
                straightLengthMeters;
            var turnEndArcLengthMeters =
                straightLengthMeters +
                turnLengthMeters;
            var maximumCurvaturePerMeter =
                1.0 / turnRadiusMeters;

            var sampleArcLengths =
                BuildCompositeArcLengthSamples(
                    straightLengthMeters,
                    curvatureTransitionLengthMeters,
                    constantCurvatureLengthMeters,
                    pointSpacingMeters);
            var speedLimits = new double[
                sampleArcLengths.Count];
            var referenceSpeeds = new double[
                sampleArcLengths.Count];

            for (var index = 0;
                 index < sampleArcLengths.Count;
                 index++)
            {
                var arcLengthMeters =
                    sampleArcLengths[index];

                // 整个转弯及两侧曲率过渡段采用转弯限速。
                speedLimits[index] =
                    arcLengthMeters >=
                        turnStartArcLengthMeters &&
                    arcLengthMeters <=
                        turnEndArcLengthMeters
                        ? Math.Abs(
                            turnMaximumSpeedMetersPerSecond)
                        : Math.Abs(
                            straightMaximumSpeedMetersPerSecond);
            }

            ApplyAccelerationAndBrakingLimits(
                sampleArcLengths,
                speedLimits,
                referenceSpeeds,
                accelerationMetersPerSecondSquared,
                decelerationMetersPerSecondSquared);

            for (var index = 0;
                 index < referenceSpeeds.Length;
                 index++)
            {
                referenceSpeeds[index] *=
                    travelDirection;
            }

            var points = new List<TrajectoryPoint>(
                sampleArcLengths.Count);
            var startCos = Math.Cos(
                startPoseInWorld.YawRadians);
            var startSin = Math.Sin(
                startPoseInWorld.YawRadians);
            var localX = 0.0;
            var localY = 0.0;
            var localYawRadians = 0.0;
            var previousArcLengthMeters = 0.0;

            for (var index = 0;
                 index < sampleArcLengths.Count;
                 index++)
            {
                var arcLengthMeters =
                    sampleArcLengths[index];
                if (index > 0)
                {
                    var segmentLengthMeters =
                        arcLengthMeters -
                        previousArcLengthMeters;
                    var segmentMiddleArcLengthMeters =
                        (arcLengthMeters +
                         previousArcLengthMeters) /
                        2.0;
                    var segmentCurvaturePerMeter =
                        CalculateSmoothTurnCurvature(
                            segmentMiddleArcLengthMeters -
                                turnStartArcLengthMeters,
                            curvatureTransitionLengthMeters,
                            constantCurvatureLengthMeters,
                            maximumCurvaturePerMeter);
                    var segmentYawChangeRadians =
                        segmentCurvaturePerMeter *
                        segmentLengthMeters;

                    if (Math.Abs(segmentCurvaturePerMeter) <=
                        1e-12)
                    {
                        localX += travelDirection *
                            Math.Cos(localYawRadians) *
                            segmentLengthMeters;
                        localY += travelDirection *
                            Math.Sin(localYawRadians) *
                            segmentLengthMeters;
                    }
                    else
                    {
                        var nextYawRadians =
                            localYawRadians +
                            segmentYawChangeRadians;
                        localX += travelDirection *
                            (Math.Sin(nextYawRadians) -
                             Math.Sin(localYawRadians)) /
                            segmentCurvaturePerMeter;
                        localY += travelDirection *
                            (Math.Cos(localYawRadians) -
                             Math.Cos(nextYawRadians)) /
                            segmentCurvaturePerMeter;
                    }

                    localYawRadians +=
                        segmentYawChangeRadians;
                }

                var curvaturePerMeter =
                    CalculateSmoothTurnCurvature(
                        arcLengthMeters -
                            turnStartArcLengthMeters,
                        curvatureTransitionLengthMeters,
                        constantCurvatureLengthMeters,
                        maximumCurvaturePerMeter);

                var worldX =
                    startPoseInWorld.XMeters +
                    startCos * localX -
                    startSin * localY;
                var worldY =
                    startPoseInWorld.YMeters +
                    startSin * localX +
                    startCos * localY;
                var worldYawRadians =
                    AngleMath.NormalizeRadians(
                        startPoseInWorld.YawRadians +
                        localYawRadians);

                points.Add(
                    new TrajectoryPoint(
                        arcLengthMeters,
                        new Pose2D(
                            worldX,
                            worldY,
                            worldYawRadians),
                        curvaturePerMeter,
                        referenceSpeeds[index]));

                previousArcLengthMeters =
                    arcLengthMeters;
            }

            return new Trajectory2D(points);
        }

        /// <summary>
        /// 分别采样直线、入弯过渡、等曲率段和出弯过渡，保证所有边界均为精确轨迹点。
        /// </summary>
        private static List<double> BuildCompositeArcLengthSamples(
            double straightLengthMeters,
            double curvatureTransitionLengthMeters,
            double constantCurvatureLengthMeters,
            double pointSpacingMeters)
        {
            var samples = new List<double> { 0.0 };
            var accumulatedArcLengthMeters = 0.0;

            AppendSectionArcLengthSamples(
                samples,
                ref accumulatedArcLengthMeters,
                straightLengthMeters,
                pointSpacingMeters);
            AppendSectionArcLengthSamples(
                samples,
                ref accumulatedArcLengthMeters,
                curvatureTransitionLengthMeters,
                pointSpacingMeters);
            AppendSectionArcLengthSamples(
                samples,
                ref accumulatedArcLengthMeters,
                constantCurvatureLengthMeters,
                pointSpacingMeters);
            AppendSectionArcLengthSamples(
                samples,
                ref accumulatedArcLengthMeters,
                curvatureTransitionLengthMeters,
                pointSpacingMeters);
            AppendSectionArcLengthSamples(
                samples,
                ref accumulatedArcLengthMeters,
                straightLengthMeters,
                pointSpacingMeters);

            return samples;
        }

        /// <summary>
        /// 计算平滑左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
        /// </summary>
        private static double CalculateSmoothTurnCurvature(
            double distanceInTurnMeters,
            double transitionLengthMeters,
            double constantCurvatureLengthMeters,
            double maximumCurvaturePerMeter)
        {
            var totalTurnLengthMeters =
                2.0 * transitionLengthMeters +
                constantCurvatureLengthMeters;

            if (distanceInTurnMeters <= 0.0 ||
                distanceInTurnMeters >= totalTurnLengthMeters)
            {
                return 0.0;
            }

            if (distanceInTurnMeters < transitionLengthMeters)
            {
                return maximumCurvaturePerMeter *
                       SmoothStep01(
                           distanceInTurnMeters /
                           transitionLengthMeters);
            }

            var exitTransitionStartMeters =
                transitionLengthMeters +
                constantCurvatureLengthMeters;
            if (distanceInTurnMeters <=
                exitTransitionStartMeters)
            {
                return maximumCurvaturePerMeter;
            }

            var exitRatio =
                (distanceInTurnMeters -
                 exitTransitionStartMeters) /
                transitionLengthMeters;
            return maximumCurvaturePerMeter *
                   (1.0 - SmoothStep01(exitRatio));
        }

        /// <summary>
        /// 将零到一的比例转换为两端一阶导数均为零的三次平滑比例。
        /// </summary>
        private static double SmoothStep01(double ratio)
        {
            var limitedRatio = Math.Max(
                0.0,
                Math.Min(1.0, ratio));
            return limitedRatio *
                   limitedRatio *
                   (3.0 - 2.0 * limitedRatio);
        }

        /// <summary>
        /// 将一段指定长度的轨迹追加为均匀弧长采样，并使最后一个点严格落在该段终点。
        /// </summary>
        private static void AppendSectionArcLengthSamples(
            ICollection<double> samples,
            ref double accumulatedArcLengthMeters,
            double sectionLengthMeters,
            double pointSpacingMeters)
        {
            var sectionStartArcLengthMeters =
                accumulatedArcLengthMeters;
            var segmentCount = (int)Math.Ceiling(
                sectionLengthMeters /
                pointSpacingMeters);

            for (var index = 1;
                 index <= segmentCount;
                 index++)
            {
                samples.Add(
                    sectionStartArcLengthMeters +
                    sectionLengthMeters *
                    index /
                    segmentCount);
            }

            accumulatedArcLengthMeters =
                sectionStartArcLengthMeters +
                sectionLengthMeters;
        }

        /// <summary>
        /// 对逐点速度幅值上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
        /// </summary>
        private static void ApplyAccelerationAndBrakingLimits(
            IReadOnlyList<double> arcLengthsMeters,
            IReadOnlyList<double> speedLimitsMetersPerSecond,
            double[] referenceSpeedsMetersPerSecond,
            double accelerationMetersPerSecondSquared,
            double decelerationMetersPerSecondSquared)
        {
            referenceSpeedsMetersPerSecond[0] = 0.0;

            for (var index = 1;
                 index < arcLengthsMeters.Count;
                 index++)
            {
                var segmentLengthMeters =
                    arcLengthsMeters[index] -
                    arcLengthsMeters[index - 1];
                var accelerationLimitedSpeed = Math.Sqrt(
                    referenceSpeedsMetersPerSecond[index - 1] *
                    referenceSpeedsMetersPerSecond[index - 1] +
                    2.0 *
                    accelerationMetersPerSecondSquared *
                    segmentLengthMeters);

                referenceSpeedsMetersPerSecond[index] =
                    Math.Min(
                        speedLimitsMetersPerSecond[index],
                        accelerationLimitedSpeed);
            }

            var finalIndex =
                referenceSpeedsMetersPerSecond.Length - 1;
            referenceSpeedsMetersPerSecond[finalIndex] = 0.0;

            for (var index = finalIndex - 1;
                 index >= 0;
                 index--)
            {
                var segmentLengthMeters =
                    arcLengthsMeters[index + 1] -
                    arcLengthsMeters[index];
                var brakingLimitedSpeed = Math.Sqrt(
                    referenceSpeedsMetersPerSecond[index + 1] *
                    referenceSpeedsMetersPerSecond[index + 1] +
                    2.0 *
                    decelerationMetersPerSecondSquared *
                    segmentLengthMeters);

                referenceSpeedsMetersPerSecond[index] =
                    Math.Min(
                        referenceSpeedsMetersPerSecond[index],
                        brakingLimitedSpeed);
            }
        }

        /// <summary>
        /// 根据起步、巡航和制动能力计算指定弧长位置允许的有符号参考速度。
        /// </summary>
        private static double CalculateReferenceSpeed(
            double arcLengthMeters,
            double remainingDistanceMeters,
            double cruiseSpeedMetersPerSecond,
            double accelerationMetersPerSecondSquared,
            double decelerationMetersPerSecondSquared)
        {
            // 由v²=2as分别得到从静止起步和到终点静止允许的速度上限。
            var accelerationLimitedSpeed = Math.Sqrt(
                2.0 *
                accelerationMetersPerSecondSquared *
                Math.Max(0.0, arcLengthMeters));
            var brakingLimitedSpeed = Math.Sqrt(
                2.0 *
                decelerationMetersPerSecondSquared *
                Math.Max(0.0, remainingDistanceMeters));

            var travelDirection = GetTravelDirection(
                cruiseSpeedMetersPerSecond,
                nameof(cruiseSpeedMetersPerSecond));
            var speedMagnitude = Math.Min(
                Math.Abs(cruiseSpeedMetersPerSecond),
                Math.Min(
                    accelerationLimitedSpeed,
                    brakingLimitedSpeed));

            return travelDirection * speedMagnitude;
        }

        /// <summary>
        /// 获取非零有符号速度表示的前进或倒车方向。
        /// </summary>
        private static double GetTravelDirection(
            double signedSpeedMetersPerSecond,
            string parameterName)
        {
            NumericGuard.EnsureFinite(
                signedSpeedMetersPerSecond,
                parameterName);

            if (signedSpeedMetersPerSecond == 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "测试轨迹的最大速度不能为零；正值表示前进，负值表示倒车。");
            }

            return Math.Sign(
                signedSpeedMetersPerSecond);
        }

        /// <summary>
        /// 确保直线段和转弯段速度使用相同的前进或倒车方向。
        /// </summary>
        private static double GetCommonTravelDirection(
            double firstSpeedMetersPerSecond,
            string firstParameterName,
            double secondSpeedMetersPerSecond,
            string secondParameterName)
        {
            var firstDirection = GetTravelDirection(
                firstSpeedMetersPerSecond,
                firstParameterName);
            var secondDirection = GetTravelDirection(
                secondSpeedMetersPerSecond,
                secondParameterName);

            if (firstDirection != secondDirection)
            {
                throw new ArgumentException(
                    "同一条测试轨迹的直线段和转弯段速度必须同号，不能在运动中直接切换前进与倒车方向。",
                    firstParameterName);
            }

            return firstDirection;
        }
    }
}