using System;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Common;

namespace MultiWheelC.Control.Longitudinal
{
    /// <summary>
    /// 将轨迹参考速度前馈与通用PID速度反馈组合为有符号底盘命令速度。
    /// </summary>
    public sealed class PidLongitudinalController
        : ILongitudinalController
    {
        private const double ReferenceStopDeadbandMetersPerSecond =
            1e-6;

        private readonly PidController _feedbackPid;

        /// <summary>
        /// 创建具有积分抗饱和和命令速度限幅的纵向速度外环。
        /// </summary>
        public PidLongitudinalController(
            double proportionalGain,
            double integralGainPerSecond,
            double derivativeGainSeconds,
            double maximumIntegralCorrectionMetersPerSecond,
            double maximumCommandSpeedMetersPerSecond,
            double speedErrorDeadbandMetersPerSecond = 0.025)
        {
            EnsureFinitePositive(
                maximumCommandSpeedMetersPerSecond,
                nameof(maximumCommandSpeedMetersPerSecond));
            EnsureFiniteNonNegative(
                speedErrorDeadbandMetersPerSecond,
                nameof(speedErrorDeadbandMetersPerSecond));

            _feedbackPid = new PidController(
                proportionalGain,
                integralGainPerSecond,
                derivativeGainSeconds,
                maximumIntegralCorrectionMetersPerSecond,
                derivativeOnMeasurement: true);
            MaximumCommandSpeedMetersPerSecond =
                maximumCommandSpeedMetersPerSecond;
            SpeedErrorDeadbandMetersPerSecond =
                speedErrorDeadbandMetersPerSecond;
        }

        /// <summary>
        /// 获取负责计算速度误差修正量的通用PID控制器。
        /// </summary>
        public PidController FeedbackPid => _feedbackPid;

        /// <summary>
        /// 获取底盘命令速度的最大绝对值，单位为m/s。
        /// </summary>
        public double MaximumCommandSpeedMetersPerSecond { get; }

        /// <summary>
        /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
        /// </summary>
        public double SpeedErrorDeadbandMetersPerSecond { get; }

        /// <summary>
        /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
        /// </summary>
        public double LastSpeedErrorMetersPerSecond =>
            _feedbackPid.LastError;

        /// <summary>
        /// 获取最近一次比例项产生的速度修正，单位为m/s。
        /// </summary>
        public double LastProportionalCorrectionMetersPerSecond =>
            _feedbackPid.LastProportionalOutput;

        /// <summary>
        /// 获取最近一次积分项产生的速度修正，单位为m/s。
        /// </summary>
        public double LastIntegralCorrectionMetersPerSecond =>
            _feedbackPid.LastIntegralOutput;

        /// <summary>
        /// 获取最近一次微分项产生的速度修正，单位为m/s。
        /// </summary>
        public double LastDerivativeCorrectionMetersPerSecond =>
            _feedbackPid.LastDerivativeOutput;

        /// <summary>
        /// 根据轨迹参考速度和Detour实际纵向速度计算底盘命令速度。
        /// </summary>
        public double ComputeSpeedMetersPerSecond(
            PathTrackingContext context)
        {
            var referenceSpeedMetersPerSecond =
                context.ReferenceSpeedMetersPerSecond;

            // 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
            if (Math.Abs(referenceSpeedMetersPerSecond) <=
                ReferenceStopDeadbandMetersPerSecond)
            {
                Reset();
                return 0.0;
            }

            // 定位速度尚不可用时只透传参考速度，不使用无效反馈更新PID状态。
            if (!context.HasValidVelocityEstimate)
            {
                Reset();
                return LimitReferenceSpeed(
                    referenceSpeedMetersPerSecond);
            }

            var speedErrorMetersPerSecond =
                referenceSpeedMetersPerSecond -
                context.ActualLongitudinalSpeedMetersPerSecond;

            // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
            // 同时清除PID历史，避免噪声持续积累后产生突发修正。
            if (Math.Abs(speedErrorMetersPerSecond) <=
                SpeedErrorDeadbandMetersPerSecond)
            {
                Reset();
                return LimitReferenceSpeed(
                    referenceSpeedMetersPerSecond);
            }

            GetCorrectionOutputRange(
                referenceSpeedMetersPerSecond,
                out var minimumCorrectionMetersPerSecond,
                out var maximumCorrectionMetersPerSecond);

            var correctionMetersPerSecond =
                _feedbackPid.Update(
                    referenceSpeedMetersPerSecond,
                    context
                        .ActualLongitudinalSpeedMetersPerSecond,
                    context.DeltaTimeSeconds,
                    minimumCorrectionMetersPerSecond,
                    maximumCorrectionMetersPerSecond);

            return referenceSpeedMetersPerSecond +
                   correctionMetersPerSecond;
        }

        /// <summary>
        /// 清除纵向速度外环的积分、历史测量值和诊断输出。
        /// </summary>
        public void Reset()
        {
            _feedbackPid.Reset();
        }

        /// <summary>
        /// 根据参考行驶方向计算PID修正量允许使用的动态输出范围。
        /// </summary>
        private void GetCorrectionOutputRange(
            double referenceSpeedMetersPerSecond,
            out double minimumCorrectionMetersPerSecond,
            out double maximumCorrectionMetersPerSecond)
        {
            if (referenceSpeedMetersPerSecond > 0.0)
            {
                minimumCorrectionMetersPerSecond =
                    -referenceSpeedMetersPerSecond;
                maximumCorrectionMetersPerSecond =
                    MaximumCommandSpeedMetersPerSecond -
                    referenceSpeedMetersPerSecond;
                return;
            }

            minimumCorrectionMetersPerSecond =
                -MaximumCommandSpeedMetersPerSecond -
                referenceSpeedMetersPerSecond;
            maximumCorrectionMetersPerSecond =
                -referenceSpeedMetersPerSecond;
        }

        /// <summary>
        /// 在没有有效速度反馈时限制参考速度的绝对值。
        /// </summary>
        private double LimitReferenceSpeed(
            double referenceSpeedMetersPerSecond)
        {
            return Math.Max(
                -MaximumCommandSpeedMetersPerSecond,
                Math.Min(
                    MaximumCommandSpeedMetersPerSecond,
                    referenceSpeedMetersPerSecond));
        }

        /// <summary>
        /// 检查最大命令速度是否为正有限值。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value) ||
                value <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "纵向控制器最大命令速度必须是正有限值。");
            }
        }

        /// <summary>
        /// 检查速度误差死区是否为非负有限值。
        /// </summary>
        private static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value) ||
                value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "纵向控制器速度误差死区必须是非负有限值。");
            }
        }
    }
}