using System;
using System.Collections.Generic;
using System.Diagnostics;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
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>
        /// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
        /// </summary>
        public Action<ParkingGeometricController> CycleObserver;

        /// <summary>
        /// Stanley横向误差增益，单位为1/s。
        /// </summary>
        public double StanleyCrossTrackGainPerSecond = 0.4;

        /// <summary>
        /// Stanley航向误差增益。
        /// </summary>
        public double StanleyHeadingErrorGain = 1.0;

        /// <summary>
        /// Stanley低速分母保护速度，单位为m/s。
        /// </summary>
        public double StanleyMinimumSpeedMetersPerSecond = 0.15;

        /// <summary>
        /// 获取或设置Stanley是否优先使用Detour估算的实际速度。
        /// </summary>
        public bool StanleyUsesActualSpeed = true;

        /// <summary>
        /// 纵向速度外环比例增益。
        /// </summary>
        public double LongitudinalKp = 0.5;

        /// <summary>
        /// 纵向速度外环积分增益，单位为1/s。
        /// </summary>
        public double LongitudinalKiPerSecond;

        /// <summary>
        /// 纵向速度外环微分增益，单位为s。
        /// </summary>
        public double LongitudinalKdSeconds;

        /// <summary>
        /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
        /// </summary>
        public double MaximumIntegralCorrectionMetersPerSecond = 0.05;

        /// <summary>
        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
        /// </summary>
        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
            0.025;

        /// <summary>
        /// 底盘纵向命令速度绝对值上限，单位为m/s。
        /// </summary>
        public double MaximumCommandSpeedMetersPerSecond = 0.50;

        /// <summary>
        /// 前后GCP允许的最大转角绝对值，单位为rad。
        /// </summary>
        public double MaximumGcpAngleRadians =
            AngleMath.DegreesToRadians(45.0);

        /// <summary>
        /// 前后GCP目标转角最大变化率，单位为rad/s。
        /// </summary>
        public double MaximumGcpAngleRateRadiansPerSecond =
            AngleMath.DegreesToRadians(15.0);

        /// <summary>
        /// 终点位置和剩余弧长的完成容差，单位为m。
        /// </summary>
        public double FinishDistanceMeters = 0.03;

        /// <summary>
        /// 终点停稳判定允许的实际线速度，单位为m/s。
        /// </summary>
        public double FinishSpeedMetersPerSecond = 0.02;

        /// <summary>
        /// 终点航向完成容差，单位为rad。
        /// </summary>
        public double FinishHeadingToleranceRadians =
            AngleMath.DegreesToRadians(3.0);

        /// <summary>
        /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
        /// </summary>
        public double MaximumDistanceToTrajectoryMeters = 0.50;

        /// <summary>
        /// 单次轨迹动作允许的最长执行时间，单位为s。
        /// </summary>
        public double ExecutionTimeoutSeconds = 120.0;

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

        /// <summary>
        /// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
        /// </summary>
        public override IEnumerable<bool> Get()
        {
            ValidateParameters();

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

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

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

            var stateProvider =
                StateProvider ??
                new DetourVehicleStateProvider();
            var controlPointRadiusMeters =
                chassis.ControlPointRadius / 1000.0;

            var lateralController =
                new StanleyLateralController(
                    controlPointRadiusMeters,
                    StanleyCrossTrackGainPerSecond,
                    StanleyHeadingErrorGain,
                    StanleyMinimumSpeedMetersPerSecond,
                    StanleyUsesActualSpeed);
            var longitudinalController =
                new PidLongitudinalController(
                    LongitudinalKp,
                    LongitudinalKiPerSecond,
                    LongitudinalKdSeconds,
                    MaximumIntegralCorrectionMetersPerSecond,
                    MaximumCommandSpeedMetersPerSecond,
                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
            var gcpAllocator =
                new AckermannGcpAllocator(
                    controlPointRadiusMeters,
                    MaximumGcpAngleRadians);
            var commandExecutor =
                new GcpCommandExecutor(
                    adapter,
                    MaximumGcpAngleRateRadiansPerSecond);

            Controller = new ParkingGeometricController(
                stateProvider,
                lateralController,
                longitudinalController,
                gcpAllocator,
                commandExecutor,
                FinishDistanceMeters,
                FinishSpeedMetersPerSecond,
                FinishHeadingToleranceRadians,
                MaximumDistanceToTrajectoryMeters);

            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()
        {
            if (Trajectory == null)
            {
                throw new InvalidOperationException(
                    "新版轨迹跟踪动作没有设置Trajectory。");
            }

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