using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable generic motion command derived from a validated trajectory execution state.</summary>
public sealed class TrajectoryControlCommand
{
    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;
    }

    public double SignedLongitudinalVelocity { get; }

    public double YawRate { get; }

    public TravelDirection Direction { get; }

    public bool RequestDirectionChange { get; }

    public bool HoldBrake { get; }

    public bool IsTrajectoryComplete { get; }
}