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
    {
        /// <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>
        /// 获取或设置本次动作的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横向修正上限覆盖值，单位为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? 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 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 maximumDistanceToTrajectoryMeters =
                MaximumDistanceToTrajectoryMeters ??
                config.ParkingMaximumDistanceToTrajectory;
            var executionTimeoutSeconds =
                ExecutionTimeoutSeconds ??
                config.ParkingExecutionTimeoutSeconds;

            ValidateParameters(executionTimeoutSeconds);

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

            var wheelPreparation =
                new PrepareWheelsForward();
            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);

            // 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
            adapter.ResetToBodyFrame();

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

            Controller = new ParkingGeometricController(
                stateProvider,
                lateralController,
                longitudinalController,
                gcpAllocator,
                commandExecutor,
                finishDistanceMeters,
                finishSpeedMetersPerSecond,
                finishHeadingToleranceRadians,
                maximumDistanceToTrajectoryMeters,
                terminalBrakingPreviewMeters);

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

                    if (Controller.LastVehicleState.HasValue)
                    {
                        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();
            }

            yield return false;
        }

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

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