using System;
using System.Collections.Generic;
using System.Diagnostics;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Control.Execution;
using MultiWheelC.Control.Lateral;
using MultiWheelC.Control.Longitudinal;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC
{
    /// <summary>
    /// 使用新版横纵向控制器持续跟踪一条世界坐标系二维轨迹。
    /// </summary>
    public sealed class TrajectoryTrackingMovement
        : MovementDefinition
    {
        private const double ReferenceSpeedDeadbandMetersPerSecond =
            1e-6;
        private const double FixedMotionDirectionToleranceRadians =
            3.0 * Math.PI / 180.0;

        /// <summary>
        /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
        /// </summary>
        public Trajectory2D Trajectory;

        /// <summary>
        /// 获取或设置本次动作使用的车辆状态源；为空时组合Detour位姿与电机反馈速度。
        /// </summary>
        public IVehicleStateProvider StateProvider;

        /// <summary>
        /// 获取或设置横向控制器创建委托；参数为车辆控制点半径（m），为空时使用配置化Stanley控制器。
        /// </summary>
        public Func<double, ILateralController>
            LateralControllerFactory;

        /// <summary>
        /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
        /// </summary>
        public Action<ParkingGeometricController> CycleObserver;

        /// <summary>
        /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；为空时从轨迹自动推导。
        /// </summary>
        public double? MotionDirectionInBodyRadians = 0.0;

        /// <summary>
        /// 获取本次执行最终采用的运动坐标系方向，动作尚未开始时为空。
        /// </summary>
        public double? ResolvedMotionDirectionInBodyRadians
        {
            get;
            private set;
        }

        /// <summary>
        /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
        /// </summary>
        public bool ReturnWheelsForwardAfterCompletion;

        /// <summary>
        /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
        /// </summary>
        public double? StanleyCrossTrackGainPerSecond;

        /// <summary>
        /// 获取或设置本次动作的Stanley航向误差增益覆盖值；为空时读取车辆配置。
        /// </summary>
        public double? StanleyHeadingErrorGain;

        /// <summary>
        /// 获取或设置本次动作的Stanley低速分母保护速度覆盖值，单位为m/s；为空时读取车辆配置。
        /// </summary>
        public double? StanleyMinimumSpeedMetersPerSecond;

        /// <summary>
        /// 获取或设置本次动作是否使用实际纵向速度的覆盖值；为空时读取车辆配置。
        /// </summary>
        public bool? StanleyUsesActualSpeed;

        /// <summary>
        /// 获取或设置本次动作的Stanley曲率前馈预瞄时间覆盖值，单位为s，0为关闭；为空时读取车辆配置。
        /// </summary>
        public double? StanleyCurvaturePreviewSeconds;

        /// <summary>
        /// 获取或设置本次动作的Stanley曲率前馈最大预瞄距离覆盖值，单位为m；为空时读取车辆配置。
        /// </summary>
        public double? StanleyMaximumCurvaturePreviewMeters;

        /// <summary>
        /// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
        /// </summary>
        public double? MaximumCrossTrackCorrectionRadians;

        /// <summary>
        /// 获取或设置本次动作的Stanley航向修正上限覆盖值，单位为rad；为空时读取车辆配置。
        /// </summary>
        public double? MaximumHeadingCorrectionRadians;

        /// <summary>
        /// 获取或设置本次动作的纵向速度比例增益覆盖值；为空时读取车辆配置。
        /// </summary>
        public double? LongitudinalKp;

        /// <summary>
        /// 获取或设置本次动作的纵向速度积分增益覆盖值，单位为1/s；为空时读取车辆配置。
        /// </summary>
        public double? LongitudinalKiPerSecond;

        /// <summary>
        /// 获取或设置本次动作的纵向速度微分增益覆盖值，单位为s；为空时读取车辆配置。
        /// </summary>
        public double? LongitudinalKdSeconds;

        /// <summary>
        /// 获取或设置本次动作的纵向积分修正上限覆盖值，单位为m/s；为空时读取车辆配置。
        /// </summary>
        public double? MaximumIntegralCorrectionMetersPerSecond;

        /// <summary>
        /// 获取或设置本次动作的纵向速度误差死区覆盖值，单位为m/s；为空时读取车辆配置。
        /// </summary>
        public double? LongitudinalSpeedErrorDeadbandMetersPerSecond;

        /// <summary>
        /// 获取或设置本次动作的底盘纵向命令速度上限覆盖值，单位为m/s；为空时读取车辆配置。
        /// </summary>
        public double? MaximumCommandSpeedMetersPerSecond;

        /// <summary>
        /// 获取或设置本次动作的GCP转角上限覆盖值，单位为rad；为空时读取车辆配置。
        /// </summary>
        public double? MaximumGcpAngleRadians;

        /// <summary>
        /// 获取或设置本次动作的GCP转角变化率上限覆盖值，单位为rad/s；为空时读取车辆配置。
        /// </summary>
        public double? MaximumGcpAngleRateRadiansPerSecond;

        /// <summary>
        /// 获取或设置本次动作的终点距离容差覆盖值，单位为m；为空时读取车辆配置。
        /// </summary>
        public double? FinishDistanceMeters;

        /// <summary>
        /// 获取或设置本次动作的终点速度容差覆盖值，单位为m/s；为空时读取车辆配置。
        /// </summary>
        public double? FinishSpeedMetersPerSecond;

        /// <summary>
        /// 获取或设置本次动作的终点航向容差覆盖值，单位为rad；为空时读取车辆配置。
        /// </summary>
        public double? FinishHeadingToleranceRadians;

        /// <summary>
        /// 获取或设置本次动作的终点制动预瞄距离覆盖值，单位为m；为空时读取车辆配置。
        /// </summary>
        public double? TerminalBrakingPreviewMeters;

        /// <summary>
        /// 获取或设置本次动作进入终点单向低速逼近的剩余弧长覆盖值，单位为m；为空时读取车辆配置。
        /// </summary>
        public double? TerminalApproachDistanceMeters;

        /// <summary>
        /// 获取或设置本次动作由终点纵向剩余距离生成低速参考的比例增益覆盖值，单位为1/s；为空时读取车辆配置。
        /// </summary>
        public double? TerminalApproachGainPerSecond;

        /// <summary>
        /// 获取或设置本次动作终点单向逼近参考速度的最大绝对值覆盖值，单位为m/s；为空时读取车辆配置。
        /// </summary>
        public double? MaximumTerminalApproachSpeedMetersPerSecond;

        /// <summary>
        /// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
        /// </summary>
        public double? MaximumDistanceToTrajectoryMeters;

        /// <summary>
        /// 获取或设置本次动作的执行超时覆盖值，单位为s；为空时读取车辆配置。
        /// </summary>
        public double? ExecutionTimeoutSeconds;

        /// <summary>
        /// 获取本次动作创建的控制器，尚未开始时为空。
        /// </summary>
        public ParkingGeometricController Controller { get; private set; }

        /// <summary>
        /// 等待舵轮稳定回正后创建控制器并持续执行，直到轨迹完成、失败或动作被取消。
        /// </summary>
        public override IEnumerable<bool> Get()
        {
            var config = PilotDefinition.Conf;
            var stanleyCrossTrackGainPerSecond =
                StanleyCrossTrackGainPerSecond ??
                config.ParkingStanleyCrossTrackGain;
            var stanleyHeadingErrorGain =
                StanleyHeadingErrorGain ??
                config.ParkingStanleyHeadingGain;
            var stanleyMinimumSpeedMetersPerSecond =
                StanleyMinimumSpeedMetersPerSecond ??
                config.ParkingStanleyMinimumSpeed;
            var stanleyUsesActualSpeed =
                StanleyUsesActualSpeed ??
                config.ParkingStanleyUseActualSpeed;
            var stanleyCurvaturePreviewSeconds =
                StanleyCurvaturePreviewSeconds ??
                config.ParkingStanleyCurvaturePreviewSeconds;
            var stanleyMaximumCurvaturePreviewMeters =
                StanleyMaximumCurvaturePreviewMeters ??
                config.ParkingStanleyMaximumCurvaturePreviewMeters;
            var maximumCrossTrackCorrectionRadians =
                MaximumCrossTrackCorrectionRadians ??
                AngleMath.DegreesToRadians(
                    config.ParkingMaximumCrossTrackCorrectionDegrees);
            var maximumHeadingCorrectionRadians =
                MaximumHeadingCorrectionRadians ??
                AngleMath.DegreesToRadians(
                    config.ParkingMaximumHeadingCorrectionDegrees);
            var longitudinalKp =
                LongitudinalKp ??
                config.ParkingLongitudinalKp;
            var longitudinalKiPerSecond =
                LongitudinalKiPerSecond ??
                config.ParkingLongitudinalKi;
            var longitudinalKdSeconds =
                LongitudinalKdSeconds ??
                config.ParkingLongitudinalKd;
            var maximumIntegralCorrectionMetersPerSecond =
                MaximumIntegralCorrectionMetersPerSecond ??
                config.ParkingMaximumIntegralCorrection;
            var maximumCommandSpeedMetersPerSecond =
                MaximumCommandSpeedMetersPerSecond ??
                config.ParkingMaximumCommandSpeed;
            var longitudinalSpeedErrorDeadbandMetersPerSecond =
                LongitudinalSpeedErrorDeadbandMetersPerSecond ??
                config.ParkingLongitudinalSpeedErrorDeadband;
            var maximumGcpAngleRadians =
                MaximumGcpAngleRadians ??
                AngleMath.DegreesToRadians(
                    config.ParkingMaximumGcpAngleDegrees);
            var maximumGcpAngleRateRadiansPerSecond =
                MaximumGcpAngleRateRadiansPerSecond ??
                AngleMath.DegreesToRadians(
                    config.ParkingMaximumGcpAngleRateDegreesPerSecond);
            var finishDistanceMeters =
                FinishDistanceMeters ??
                config.ParkingFinishDistance;
            var finishSpeedMetersPerSecond =
                FinishSpeedMetersPerSecond ??
                config.ParkingFinishSpeed;
            var finishHeadingToleranceRadians =
                FinishHeadingToleranceRadians ??
                AngleMath.DegreesToRadians(
                    config.ParkingFinishHeadingToleranceDegrees);
            var terminalBrakingPreviewMeters =
                TerminalBrakingPreviewMeters ??
                config.ParkingTerminalBrakingPreview;
            var terminalApproachDistanceMeters =
                TerminalApproachDistanceMeters ??
                config.ParkingTerminalApproachDistance;
            var terminalApproachGainPerSecond =
                TerminalApproachGainPerSecond ??
                config.ParkingTerminalApproachGain;
            var maximumTerminalApproachSpeedMetersPerSecond =
                MaximumTerminalApproachSpeedMetersPerSecond ??
                config.ParkingTerminalMaximumApproachSpeed;
            var maximumDistanceToTrajectoryMeters =
                MaximumDistanceToTrajectoryMeters ??
                config.ParkingMaximumDistanceToTrajectory;
            var executionTimeoutSeconds =
                ExecutionTimeoutSeconds ??
                config.ParkingExecutionTimeoutSeconds;

            ValidateParameters(executionTimeoutSeconds);
            var motionDirectionInBodyRadians =
                MotionDirectionInBodyRadians ??
                ResolveFixedMotionDirectionInBodyRadians(
                    Trajectory);
            ResolvedMotionDirectionInBodyRadians =
                motionDirectionInBodyRadians;

            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
            {
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
            }

            var wheelPreparation =
                new PrepareWheelsForward
                {
                    DirectionRadians =
                        motionDirectionInBodyRadians
                };
            foreach (var keepRunning in wheelPreparation.Get())
            {
                if (!keepRunning)
                {
                    break;
                }

                yield return true;
            }

            if (!wheelPreparation.Completed)
            {
                throw new InvalidOperationException(
                    "轨迹跟踪开始前舵轮未能稳定到达目标运动方向。");
            }

            var adapter = new MultiWheelChassisAdapter(
                chassis,
                PilotDefinition.Self.CarNum);

            adapter.ActivateMotionFrame(
                motionDirectionInBodyRadians);

            var stateProvider =
                StateProvider ??
                ParkingVehicleStateProviderFactory.Create(
                    chassis,
                    config);
            var controlPointRadiusMeters =
                chassis.ControlPointRadius / 1000.0;

            var lateralController =
                LateralControllerFactory == null
                    ? new StanleyLateralController(
                        controlPointRadiusMeters,
                        stanleyCrossTrackGainPerSecond,
                        stanleyHeadingErrorGain,
                        stanleyMinimumSpeedMetersPerSecond,
                        stanleyUsesActualSpeed,
                        maximumCrossTrackCorrectionRadians,
                        maximumHeadingCorrectionRadians)
                    : LateralControllerFactory(
                        controlPointRadiusMeters);
            if (lateralController == null)
            {
                throw new InvalidOperationException(
                    "横向控制器创建委托不能返回空值。");
            }
            var longitudinalController =
                new PidLongitudinalController(
                    longitudinalKp,
                    longitudinalKiPerSecond,
                    longitudinalKdSeconds,
                    maximumIntegralCorrectionMetersPerSecond,
                    maximumCommandSpeedMetersPerSecond,
                    longitudinalSpeedErrorDeadbandMetersPerSecond);
            var gcpAllocator =
                new GcpCommandAllocator(
                    maximumGcpAngleRadians);
            var commandExecutor =
                new GcpCommandExecutor(
                    adapter,
                    maximumGcpAngleRateRadiansPerSecond,
                    motionDirectionInBodyRadians);

            Controller = new ParkingGeometricController(
                stateProvider,
                lateralController,
                longitudinalController,
                gcpAllocator,
                commandExecutor,
                finishDistanceMeters,
                finishSpeedMetersPerSecond,
                finishHeadingToleranceRadians,
                maximumDistanceToTrajectoryMeters,
                terminalBrakingPreviewMeters,
                terminalApproachDistanceMeters,
                terminalApproachGainPerSecond,
                maximumTerminalApproachSpeedMetersPerSecond,
                stanleyCurvaturePreviewSeconds,
                stanleyMaximumCurvaturePreviewMeters,
                motionDirectionInBodyRadians);

            var clock = Stopwatch.StartNew();
            var previousCycleSeconds =
                clock.Elapsed.TotalSeconds;
            Controller.Start(Trajectory);

            try
            {
                while (true)
                {
                    if (clock.Elapsed.TotalSeconds >
                        executionTimeoutSeconds)
                    {
                        throw new TimeoutException(
                            $"新版轨迹跟踪超过{executionTimeoutSeconds:F1}s仍未完成。");
                    }

                    var currentCycleSeconds =
                        clock.Elapsed.TotalSeconds;
                    var deltaTimeSeconds =
                        currentCycleSeconds -
                        previousCycleSeconds;
                    previousCycleSeconds =
                        currentCycleSeconds;

                    // 极短首周期不参与PID和GCP角速度限制，等待调度器进入下一周期。
                    if (deltaTimeSeconds <= 1e-6)
                    {
                        yield return true;
                        continue;
                    }

                    var result =
                        Controller.ExecuteCycle(
                            deltaTimeSeconds);

                    // 诊断观察器按真实控制周期触发，即使本周期状态不可用，
                    // 也允许记录状态读取和主动停车所消耗的时间。
                    CycleObserver?.Invoke(Controller);

                    if (result ==
                        ParkingControlCycleResult.Completed)
                    {
                        break;
                    }

                    if (result ==
                        ParkingControlCycleResult.Faulted)
                    {
                        throw new InvalidOperationException(
                            string.IsNullOrWhiteSpace(
                                Controller.LastFailureReason)
                                ? "新版轨迹跟踪控制器发生未知故障。"
                                : Controller.LastFailureReason,
                            Controller.LastException);
                    }

                    if (result ==
                        ParkingControlCycleResult.Inactive)
                    {
                        throw new InvalidOperationException(
                            "新版轨迹跟踪控制器在轨迹完成前意外停止活动。");
                    }

                    // CommandSent和短暂StateUnavailable均继续下一控制周期；
                    // 后者已经由控制器主动停车，等待Detour恢复。
                    yield return true;
                }
            }
            finally
            {
                Controller.Cancel();
            }

            if (ReturnWheelsForwardAfterCompletion)
            {
                var forwardPreparation =
                    new PrepareWheelsForward();
                foreach (var keepRunning in
                         forwardPreparation.Get())
                {
                    if (!keepRunning)
                    {
                        break;
                    }

                    yield return true;
                }

                if (!forwardPreparation.Completed)
                {
                    throw new InvalidOperationException(
                        "轨迹完成后舵轮未能稳定回到车头方向。");
                }
            }

            yield return false;
        }

        /// <summary>
        /// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
        /// </summary>
        private void ValidateParameters(
            double executionTimeoutSeconds)
        {
            if (Trajectory == null)
            {
                throw new InvalidOperationException(
                    "新版轨迹跟踪动作没有设置Trajectory。");
            }

            if (MotionDirectionInBodyRadians.HasValue)
            {
                NumericGuard.EnsureFinite(
                    MotionDirectionInBodyRadians.Value,
                    nameof(MotionDirectionInBodyRadians));
            }

            if (double.IsNaN(executionTimeoutSeconds) ||
                double.IsInfinity(executionTimeoutSeconds) ||
                executionTimeoutSeconds <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(ExecutionTimeoutSeconds),
                    "轨迹跟踪超时时间必须是正有限值。");
            }
        }

        /// <summary>
        /// 根据轨迹切线、参考车身航向和速度符号推导整段轨迹共同使用的固定运动方向。
        /// </summary>
        private static double ResolveFixedMotionDirectionInBodyRadians(
            Trajectory2D trajectory)
        {
            double? resolvedDirectionRadians = null;

            for (var index = 0;
                 index < trajectory.Count - 1;
                 index++)
            {
                var segmentStart = trajectory[index];
                var segmentEnd = trajectory[index + 1];
                var travelDirection = ResolveSegmentTravelDirection(
                    segmentStart.ReferenceSpeedMetersPerSecond,
                    segmentEnd.ReferenceSpeedMetersPerSecond,
                    index);
                if (travelDirection == 0.0)
                {
                    continue;
                }

                var tangentYawRadians = Math.Atan2(
                    segmentEnd.PoseInWorld.YMeters -
                        segmentStart.PoseInWorld.YMeters,
                    segmentEnd.PoseInWorld.XMeters -
                        segmentStart.PoseInWorld.XMeters);
                var positiveMotionAxisYawRadians =
                    travelDirection > 0.0
                        ? tangentYawRadians
                        : AngleMath.NormalizeRadians(
                            tangentYawRadians + Math.PI);
                var referenceBodyYawRadians =
                    AngleMath.LerpRadians(
                        segmentStart.PoseInWorld.YawRadians,
                        segmentEnd.PoseInWorld.YawRadians,
                        0.5);
                var candidateDirectionRadians =
                    AngleMath.ShortestDifferenceRadians(
                        positiveMotionAxisYawRadians,
                        referenceBodyYawRadians);

                if (!resolvedDirectionRadians.HasValue)
                {
                    resolvedDirectionRadians =
                        candidateDirectionRadians;
                    continue;
                }

                var directionDifferenceRadians = Math.Abs(
                    AngleMath.ShortestDifferenceRadians(
                        candidateDirectionRadians,
                        resolvedDirectionRadians.Value));
                if (directionDifferenceRadians >
                    FixedMotionDirectionToleranceRadians)
                {
                    throw new InvalidOperationException(
                        "轨迹无法由一个固定运动坐标系执行：" +
                        $"第{index + 1}段需要的方向与起始方向相差" +
                        $"{AngleMath.RadiansToDegrees(directionDifferenceRadians):F2}°。" +
                        "请拆分轨迹，或显式指定并验证MotionDirectionInBodyRadians。");
                }
            }

            if (!resolvedDirectionRadians.HasValue)
            {
                throw new InvalidOperationException(
                    "轨迹没有非零参考速度线段，无法自动确定运动坐标系方向。");
            }

            return resolvedDirectionRadians.Value;
        }

        /// <summary>
        /// 从相邻轨迹点的有符号参考速度确定该线段的执行方向。
        /// </summary>
        private static double ResolveSegmentTravelDirection(
            double startSpeedMetersPerSecond,
            double endSpeedMetersPerSecond,
            int segmentStartIndex)
        {
            var hasStartDirection =
                Math.Abs(startSpeedMetersPerSecond) >
                ReferenceSpeedDeadbandMetersPerSecond;
            var hasEndDirection =
                Math.Abs(endSpeedMetersPerSecond) >
                ReferenceSpeedDeadbandMetersPerSecond;

            if (hasStartDirection &&
                hasEndDirection &&
                Math.Sign(startSpeedMetersPerSecond) !=
                Math.Sign(endSpeedMetersPerSecond))
            {
                throw new InvalidOperationException(
                    $"轨迹第{segmentStartIndex + 1}段内参考速度发生正负切换，" +
                    "无法自动确定固定运动坐标系；请在零速点拆分动作段。");
            }

            if (hasStartDirection)
            {
                return Math.Sign(startSpeedMetersPerSecond);
            }

            return hasEndDirection
                ? Math.Sign(endSpeedMetersPerSecond)
                : 0.0;
        }
    }
}
