using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class VehicleMotionState
{
    public VehicleMotionState(
        Pose2D pose,
        double signedLongitudinalSpeedMetersPerSecond,
        double? longitudinalAccelerationMetersPerSecondSquared,
        DateTimeOffset capturedAtUtc,
        long sequenceId)
    {
        if (pose == null)
            throw new ArgumentNullException(nameof(pose));
        ContractNumeric.RequireFinite(pose.X, nameof(pose));
        ContractNumeric.RequireFinite(pose.Y, nameof(pose));
        ContractNumeric.RequireFinite(pose.Heading, nameof(pose));
        ContractNumeric.RequireFinite(signedLongitudinalSpeedMetersPerSecond, nameof(signedLongitudinalSpeedMetersPerSecond));
        if (longitudinalAccelerationMetersPerSecondSquared.HasValue)
            ContractNumeric.RequireFinite(longitudinalAccelerationMetersPerSecondSquared.Value, nameof(longitudinalAccelerationMetersPerSecondSquared));
        if (sequenceId < 0)
            throw new ArgumentOutOfRangeException(nameof(sequenceId));

        Pose = pose;
        SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond;
        LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared;
        CapturedAtUtc = capturedAtUtc;
        SequenceId = sequenceId;
    }

    public Pose2D Pose { get; }

    public double SignedLongitudinalSpeedMetersPerSecond { get; }

    public double? LongitudinalAccelerationMetersPerSecondSquared { get; }

    public DateTimeOffset CapturedAtUtc { get; }

    public long SequenceId { get; }
}