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)
        {
            EnsureFinitePose(
                startPoseInWorld,
                nameof(startPoseInWorld));
            EnsureFinitePositive(
                lengthMeters,
                nameof(lengthMeters));
            EnsureFinitePositive(
                cruiseSpeedMetersPerSecond,
                nameof(cruiseSpeedMetersPerSecond));
            EnsureFinitePositive(
                accelerationMetersPerSecondSquared,
                nameof(accelerationMetersPerSecondSquared));
            EnsureFinitePositive(
                decelerationMetersPerSecondSquared,
                nameof(decelerationMetersPerSecondSquared));
            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 = Math.Cos(
                startPoseInWorld.YawRadians);
            var directionY = 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直线、平滑进入半径2m左转、平滑退出、3m直线”的180°转弯轨迹。
        /// </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)
        {
            EnsureFinitePose(
                startPoseInWorld,
                nameof(startPoseInWorld));
            EnsureFinitePositive(
                straightLengthMeters,
                nameof(straightLengthMeters));
            EnsureFinitePositive(
                turnRadiusMeters,
                nameof(turnRadiusMeters));
            EnsureFinitePositive(
                turnAngleRadians,
                nameof(turnAngleRadians));
            EnsureFinitePositive(
                curvatureTransitionLengthMeters,
                nameof(curvatureTransitionLengthMeters));
            EnsureFinitePositive(
                straightMaximumSpeedMetersPerSecond,
                nameof(straightMaximumSpeedMetersPerSecond));
            EnsureFinitePositive(
                turnMaximumSpeedMetersPerSecond,
                nameof(turnMaximumSpeedMetersPerSecond));
            EnsureFinitePositive(
                accelerationMetersPerSecondSquared,
                nameof(accelerationMetersPerSecondSquared));
            EnsureFinitePositive(
                decelerationMetersPerSecondSquared,
                nameof(decelerationMetersPerSecondSquared));
            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
                        ? turnMaximumSpeedMetersPerSecond
                        : straightMaximumSpeedMetersPerSecond;
            }

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

            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 +=
                            Math.Cos(localYawRadians) *
                            segmentLengthMeters;
                        localY +=
                            Math.Sin(localYawRadians) *
                            segmentLengthMeters;
                    }
                    else
                    {
                        var nextYawRadians =
                            localYawRadians +
                            segmentYawChangeRadians;
                        localX +=
                            (Math.Sin(nextYawRadians) -
                             Math.Sin(localYawRadians)) /
                            segmentCurvaturePerMeter;
                        localY +=
                            (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));

            return Math.Min(
                cruiseSpeedMetersPerSecond,
                Math.Min(
                    accelerationLimitedSpeed,
                    brakingLimitedSpeed));
        }

        /// <summary>
        /// 检查世界坐标系起点位姿是否全部为有限值。
        /// </summary>
        private static void EnsureFinitePose(
            Pose2D pose,
            string parameterName)
        {
            if (!IsFinite(pose.XMeters) ||
                !IsFinite(pose.YMeters) ||
                !IsFinite(pose.YawRadians))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "直线测试轨迹的起点位姿必须由有限值组成。");
            }
        }

        /// <summary>
        /// 检查测试轨迹参数是否为正有限值。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            if (!IsFinite(value) || value <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "直线测试轨迹的速度、加速度和点间距必须是正有限值。");
            }
        }

        /// <summary>
        /// 判断数值是否可用于轨迹计算。
        /// </summary>
        private static bool IsFinite(double value)
        {
            return !double.IsNaN(value) &&
                   !double.IsInfinity(value);
        }
    }
}