using System;
using MultiWheelC.Control.Abstractions;

namespace MultiWheelC.Control.Lateral
{
    /// <summary>
    /// 使用参考曲率前馈、航向误差和横向误差计算车体中心目标曲率。
    /// </summary>
    public sealed class StanleyLateralController : ILateralController
    {
        private const double MaximumMathematicalAngleRadians =
            Math.PI / 2.0 - 1e-3;

        /// <summary>
        /// 创建使用指定GCP几何、Stanley增益和低速保护参数的横向控制器。
        /// </summary>
        public StanleyLateralController(
            double controlPointRadiusMeters,
            double crossTrackGainPerSecond,
            double headingErrorGain,
            double minimumSpeedMetersPerSecond,
            bool useActualSpeedForGain = true)
        {
            EnsureFinitePositive(
                controlPointRadiusMeters,
                nameof(controlPointRadiusMeters));
            EnsureFiniteNonNegative(
                crossTrackGainPerSecond,
                nameof(crossTrackGainPerSecond));
            EnsureFiniteNonNegative(
                headingErrorGain,
                nameof(headingErrorGain));
            EnsureFinitePositive(
                minimumSpeedMetersPerSecond,
                nameof(minimumSpeedMetersPerSecond));

            ControlPointRadiusMeters = controlPointRadiusMeters;
            CrossTrackGainPerSecond = crossTrackGainPerSecond;
            HeadingErrorGain = headingErrorGain;
            MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
            UseActualSpeedForGain = useActualSpeedForGain;
        }

        /// <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>
        /// 获取是否优先使用Detour估算的实际纵向速度计算横向误差项。
        /// </summary>
        public bool UseActualSpeedForGain { get; }

        /// <summary>
        /// 根据参考曲率、航向误差和横向误差计算车体中心目标曲率。
        /// </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 = Math.Atan(
                CrossTrackGainPerSecond *
                context.LateralErrorMeters /
                speedMagnitude);

            // 倒车时需要反转反馈修正方向；参考曲率前馈仍由轨迹本身决定。
            var feedbackAngleRadians = travelDirection *
                (HeadingErrorGain * context.HeadingErrorRadians +
                 crossTrackCorrectionRadians);

            // 这里只避开tan奇点，实际GCP机械限制由AckermannGcpAllocator处理。
            var targetEquivalentAngleRadians = Clamp(
                feedforwardAngleRadians + feedbackAngleRadians,
                -MaximumMathematicalAngleRadians,
                MaximumMathematicalAngleRadians);
            var targetCurvaturePerMeter = Math.Tan(
                targetEquivalentAngleRadians) /
                ControlPointRadiusMeters;

            return new LateralControlCommand(
                targetCurvaturePerMeter);
        }

        /// <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 Clamp(
            double value,
            double minimum,
            double maximum)
        {
            return Math.Max(
                minimum,
                Math.Min(maximum, 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控制参数必须是有限值。");
            }
        }
    }
}