using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>将不可变执行状态转换为控制器中立的纵向速度和 yaw rate 命令，并在停稳/完成时强制零运动。</summary>
public sealed class TrajectoryControlAdapter
{
    /// <summary>依据选中轨迹点和换向状态创建安全控制命令。</summary>
    /// <param name="trajectoryPoint">已验证轨迹中的选中点，提供带符号速度、yaw rate 和方向。</param>
    /// <param name="executionState">不可变执行状态；停零、完成或不允许运动时优先于轨迹点运动字段。</param>
    /// <returns>常规跟随时保留轨迹点 m/s 和 rad/s；保持或完成时返回零速度、零 yaw rate 和制动命令。</returns>
    public TrajectoryControlCommand CreateCommand(EmTrajectoryPoint trajectoryPoint,
        TrajectoryExecutionState executionState)
    {
        if (trajectoryPoint == null)
            throw new ArgumentNullException(nameof(trajectoryPoint));
        if (executionState == null)
            throw new ArgumentNullException(nameof(executionState));

        bool holdBrake = executionState.HoldZero || executionState.IsTrajectoryComplete ||
            !executionState.AllowsTrajectoryMotion;
        if (holdBrake)
        {
            return new TrajectoryControlCommand(0d, 0d, trajectoryPoint.Direction,
                executionState.RequestDirectionChange, true, executionState.IsTrajectoryComplete);
        }

        return new TrajectoryControlCommand(trajectoryPoint.SignedLongitudinalVelocity, trajectoryPoint.YawRate,
            trajectoryPoint.Direction, false, false, false);
    }
}