using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Converts immutable execution state into a controller-neutral longitudinal and yaw command.</summary>
public sealed class TrajectoryControlAdapter
{
    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);
    }
}