using System;
using MultiWheelC.Control.Abstractions;

namespace MultiWheelC.Control.Lateral
{
    /// <summary>
    /// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。
    /// </summary>
    public sealed class StanleyLateralController : ILateralController
    {
        /// <summary>
        /// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。
        /// </summary>
        public StanleyLateralController(
            double controlPointRadiusMeters,
            double crossTrackGainPerSecond,
            double headingErrorGain,
            double minimumSpeedMetersPerSecond,
            bool useActualSpeedForGain = true,
            double maximumCrossTrackCorrectionRadians =
                10.0 * Math.PI / 180.0,
            double maximumHeadingCorrectionRadians =
                10.0 * Math.PI / 180.0)
        {
            EnsureFinitePositive(
                controlPointRadiusMeters,
                nameof(controlPointRadiusMeters));
            EnsureFiniteNonNegative(
                crossTrackGainPerSecond,
                nameof(crossTrackGainPerSecond));
            EnsureFiniteNonNegative(
                headingErrorGain,
                nameof(headingErrorGain));
            EnsureFinitePositive(
                minimumSpeedMetersPerSecond,
                nameof(minimumSpeedMetersPerSecond));
            EnsureFinitePositive(
                maximumCrossTrackCorrectionRadians,
                nameof(maximumCrossTrackCorrectionRadians));
            EnsureFinitePositive(
                maximumHeadingCorrectionRadians,
                nameof(maximumHeadingCorrectionRadians));

            ControlPointRadiusMeters = controlPointRadiusMeters;
            CrossTrackGainPerSecond = crossTrackGainPerSecond;
            HeadingErrorGain = headingErrorGain;
            MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
            UseActualSpeedForGain = useActualSpeedForGain;
            MaximumCrossTrackCorrectionRadians =
                maximumCrossTrackCorrectionRadians;
            MaximumHeadingCorrectionRadians =
                maximumHeadingCorrectionRadians;
        }

        /// <summary>
        /// 获取车体中心到前、后GCP的距离，单位为m。
        /// </summary>
        public double ControlPointRadiusMeters { get; }

        /// <summary>
        /// 获取横向误差增益，单位为1/s。
        /// </summary>
        public double CrossTrackGainPerSecond { get; }

        /// <summary>
        /// 获取航向误差的无量纲增益。
        /// </summary>
        public double HeadingErrorGain { get; }

        /// <summary>
        /// 获取Stanley分母使用的最小速度绝对值，单位为m/s。
        /// </summary>
        public double MinimumSpeedMetersPerSecond { get; }

        /// <summary>
        /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
        /// </summary>
        public bool UseActualSpeedForGain { get; }

        /// <summary>
        /// 获取横向误差共同转角分量的最大绝对值，单位为rad。
        /// </summary>
        public double MaximumCrossTrackCorrectionRadians { get; }

        /// <summary>
        /// 获取航向误差差动转角分量的最大绝对值，单位为rad。
        /// </summary>
        public double MaximumHeadingCorrectionRadians { get; }

        /// <summary>
        /// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。
        /// </summary>
        public LateralControlCommand Compute(
            PathTrackingContext context)
        {
            var speedForGain = SelectSpeedForGain(context);
            var speedMagnitude = Math.Max(
                Math.Abs(speedForGain),
                MinimumSpeedMetersPerSecond);
            var travelDirection = SelectTravelDirection(context);

            // 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
            var feedforwardAngleRadians = Math.Atan(
                context.ReferenceCurvaturePerMeter *
                ControlPointRadiusMeters);

            // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
            var crossTrackCorrectionRadians =
                ClampSymmetric(
                    Math.Atan(
                        CrossTrackGainPerSecond *
                        context.LateralErrorMeters /
                        speedMagnitude),
                    MaximumCrossTrackCorrectionRadians);

            // 航向误差生成前后反向的差动转角，只负责调整车身朝向。
            var headingCorrectionRadians =
                ClampSymmetric(
                    HeadingErrorGain *
                    context.HeadingErrorRadians,
                    MaximumHeadingCorrectionRadians);

            var commonAngleRadians =
                travelDirection *
                crossTrackCorrectionRadians;
            var differentialAngleRadians =
                feedforwardAngleRadians +
                travelDirection *
                headingCorrectionRadians;

            return new LateralControlCommand(
                commonAngleRadians +
                differentialAngleRadians,
                commonAngleRadians -
                differentialAngleRadians);
        }

        /// <summary>
        /// 清除横向控制器状态；当前Stanley实现没有跨周期状态。
        /// </summary>
        public void Reset()
        {
        }

        /// <summary>
        /// 选择Stanley横向误差项使用的实际速度或参考速度。
        /// </summary>
        private double SelectSpeedForGain(
            PathTrackingContext context)
        {
            if (UseActualSpeedForGain &&
                context.HasValidVelocityEstimate)
            {
                return context
                    .ActualLongitudinalSpeedMetersPerSecond;
            }

            return context.ReferenceSpeedMetersPerSecond;
        }

        /// <summary>
        /// 根据有符号参考速度确定前进或倒车时的反馈修正方向。
        /// </summary>
        private static double SelectTravelDirection(
            PathTrackingContext context)
        {
            const double directionDeadbandMetersPerSecond = 1e-6;

            if (Math.Abs(context.ReferenceSpeedMetersPerSecond) >
                directionDeadbandMetersPerSecond)
            {
                return Math.Sign(
                    context.ReferenceSpeedMetersPerSecond);
            }

            if (context.HasValidVelocityEstimate &&
                Math.Abs(
                    context.ActualLongitudinalSpeedMetersPerSecond) >
                directionDeadbandMetersPerSecond)
            {
                return Math.Sign(
                    context.ActualLongitudinalSpeedMetersPerSecond);
            }

            return 1.0;
        }

        /// <summary>
        /// 将数值按正负对称方式限制在指定绝对值内。
        /// </summary>
        private static double ClampSymmetric(
            double value,
            double maximumAbsoluteValue)
        {
            return Math.Max(
                -maximumAbsoluteValue,
                Math.Min(maximumAbsoluteValue, value));
        }

        /// <summary>
        /// 检查控制参数是否为正有限值。
        /// </summary>
        private static void EnsureFinitePositive(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "Stanley控制器的几何尺寸、速度和角度限制必须是正有限值。");
            }
        }

        /// <summary>
        /// 检查控制增益是否为非负有限值。
        /// </summary>
        private static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "Stanley控制增益必须是非负有限值。");
            }
        }

        /// <summary>
        /// 检查控制参数是否为有限值。
        /// </summary>
        private static void EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "Stanley控制参数必须是有限值。");
            }
        }
    }
}