using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>
/// 从已验证轨迹执行状态导出的不可变、控制器中立运动命令。
/// 只表示带符号纵向速度和 yaw rate；不包含横向车体速度、蟹行、原地旋转、UI 或硬件字段。
/// </summary>
public sealed class TrajectoryControlCommand
{
    /// <summary>创建控制器中立命令。</summary>
    /// <param name="signedLongitudinalVelocity">车体纵向速度，单位 m/s；正数前进、负数倒车，保持制动时必须为 0。</param>
    /// <param name="yawRate">车辆 yaw rate，单位 rad/s；保持制动或完成时必须为 0。</param>
    /// <param name="direction">命令关联的实际行驶方向。</param>
    /// <param name="requestDirectionChange">仅在零速 dwell 完成后的单个更新周期请求方向变更。</param>
    /// <param name="holdBrake">为 <see langword="true"/> 时调用方必须保持零速度和零 yaw rate。</param>
    /// <param name="isTrajectoryComplete">到达 Goal 或 RollingSafetyStop 后为 <see langword="true"/>。</param>
    public TrajectoryControlCommand(double signedLongitudinalVelocity, double yawRate, TravelDirection direction,
        bool requestDirectionChange, bool holdBrake, bool isTrajectoryComplete)
    {
        if (double.IsNaN(signedLongitudinalVelocity) || double.IsInfinity(signedLongitudinalVelocity))
            throw new ArgumentOutOfRangeException(nameof(signedLongitudinalVelocity));
        if (double.IsNaN(yawRate) || double.IsInfinity(yawRate))
            throw new ArgumentOutOfRangeException(nameof(yawRate));
        if (!Enum.IsDefined(typeof(TravelDirection), direction))
            throw new ArgumentOutOfRangeException(nameof(direction));

        SignedLongitudinalVelocity = signedLongitudinalVelocity;
        YawRate = yawRate;
        Direction = direction;
        RequestDirectionChange = requestDirectionChange;
        HoldBrake = holdBrake;
        IsTrajectoryComplete = isTrajectoryComplete;
    }

    /// <summary>权威带符号纵向速度，单位 m/s。</summary>
    public double SignedLongitudinalVelocity { get; }

    /// <summary>逆时针为正的 yaw rate，单位 rad/s。</summary>
    public double YawRate { get; }

    public TravelDirection Direction { get; }

    /// <summary>是否在本次更新请求一次方向切换。</summary>
    public bool RequestDirectionChange { get; }

    /// <summary>是否要求调用方保持制动，不允许轨迹运动。</summary>
    public bool HoldBrake { get; }

    public bool IsTrajectoryComplete { get; }
}