using System;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC.Control.Abstractions
{
    /// <summary>
    /// 保存一次轨迹跟踪控制周期使用的刚体速度、轨迹投影和真实时间间隔。
    /// </summary>
    public readonly struct PathTrackingContext
    {
        /// <summary>
        /// 创建横向和纵向控制器共享的只读控制输入快照。
        /// </summary>
        public PathTrackingContext(
            Twist2D actualTwistInBody,
            bool hasValidVelocityEstimate,
            TrajectoryProjection projection,
            double controlReferenceSpeedMetersPerSecond,
            double feedforwardCurvaturePerMeter,
            double deltaTimeSeconds,
            double motionDirectionInBodyRadians = 0.0)
        {
            EnsureFinite(
                controlReferenceSpeedMetersPerSecond,
                nameof(controlReferenceSpeedMetersPerSecond));
            EnsureFinite(
                feedforwardCurvaturePerMeter,
                nameof(feedforwardCurvaturePerMeter));
            EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));
            EnsureFinite(
                motionDirectionInBodyRadians,
                nameof(motionDirectionInBodyRadians));
            NumericGuard.EnsureFinite(
                actualTwistInBody,
                nameof(actualTwistInBody));

            ActualTwistInBody = actualTwistInBody;
            HasValidVelocityEstimate =
                hasValidVelocityEstimate;
            Projection = projection;
            ControlReferenceSpeedMetersPerSecond =
                controlReferenceSpeedMetersPerSecond;
            FeedforwardCurvaturePerMeter =
                feedforwardCurvaturePerMeter;
            DeltaTimeSeconds = deltaTimeSeconds;
            MotionDirectionInBodyRadians =
                AngleMath.NormalizeRadians(
                    motionDirectionInBodyRadians);
        }

        /// <summary>
        /// 获取受控刚体坐标系下的实际速度。
        /// </summary>
        public Twist2D ActualTwistInBody { get; }

        /// <summary>
        /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
        /// </summary>
        public TrajectoryProjection Projection { get; }

        /// <summary>
        /// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。
        /// </summary>
        public double DeltaTimeSeconds { get; }

        /// <summary>
        /// 获取轨迹原始速度经过起步释放和制动预瞄处理后，本控制周期实际使用的有符号参考速度，单位为m/s。
        /// </summary>
        public double ControlReferenceSpeedMetersPerSecond { get; }

        /// <summary>
        /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
        /// </summary>
        public double ActualLongitudinalSpeedMetersPerSecond =>
            Math.Cos(MotionDirectionInBodyRadians) *
                ActualTwistInBody.VxMetersPerSecond +
            Math.Sin(MotionDirectionInBodyRadians) *
                ActualTwistInBody.VyMetersPerSecond;

        /// <summary>
        /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
        /// </summary>
        public double MotionDirectionInBodyRadians { get; }

        /// <summary>
        /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
        /// </summary>
        public double ReferenceCurvaturePerMeter =>
            Projection.ReferencePoint
                .CurvaturePerMeter;

        /// <summary>
        /// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。
        /// </summary>
        public double FeedforwardCurvaturePerMeter { get; }

        /// <summary>
        /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
        /// </summary>
        public double LateralErrorMeters =>
            Projection.LateralErrorMeters;

        /// <summary>
        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
        /// </summary>
        public double HeadingErrorRadians =>
            Projection.HeadingErrorRadians;

        /// <summary>
        /// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。
        /// </summary>
        public double RemainingDistanceMeters =>
            Projection.RemainingDistanceMeters;

        /// <summary>
        /// 获取本周期实际速度估计是否可供闭环控制使用。
        /// </summary>
        public bool HasValidVelocityEstimate { get; }

        /// <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 EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "轨迹跟踪参考速度必须是有限值。");
            }
        }
    }
}