ParkingRobot/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs

using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public enum TrajectoryHandoffSource
{
    MeasuredState,
    PreviousTrajectory,
}

public enum TrajectoryHandoffRejectionReason
{
    None,
    MissingTrajectory,
    InvalidConfiguration,
    TrajectoryNotYetEffective,
    TrajectoryTooOld,
    SegmentMismatch,
    DirectionMismatch,
    TrackingErrorExceeded,
    GearBoundary,
    TerminalBoundary,
    HandoffBeyondTrajectory,
    SampleUnavailable,
}

/// <summary>Immutable handoff decision for the next one-shot planning request.</summary>
public sealed class TrajectoryHandoffSelection
{
    internal TrajectoryHandoffSelection(TrajectoryHandoffSource source, VehicleMotionState startState,
        EmTrajectory previousTrajectory, EmTrajectoryPoint sampledPoint, TrajectoryHandoffRejectionReason rejectionReason)
    {
        Source = source;
        StartState = startState ?? throw new ArgumentNullException(nameof(startState));
        PreviousTrajectory = previousTrajectory;
        SampledPoint = sampledPoint;
        RejectionReason = rejectionReason;
    }

    public TrajectoryHandoffSource Source { get; }

    public VehicleMotionState StartState { get; }

    public EmTrajectory PreviousTrajectory { get; }

    public EmTrajectoryPoint SampledPoint { get; }

    public TrajectoryHandoffRejectionReason RejectionReason { get; }
}

/// <summary>Selects a future same-segment seed or preserves the caller's measured state.</summary>
public sealed class TrajectoryHandoffSelector
{
    private const double TimeEpsilonSeconds = 1e-9d;
    private readonly TrajectorySampler sampler;

    public TrajectoryHandoffSelector()
        : this(new TrajectorySampler())
    {
    }

    internal TrajectoryHandoffSelector(TrajectorySampler sampler)
    {
        this.sampler = sampler ?? throw new ArgumentNullException(nameof(sampler));
    }

    public TrajectoryHandoffSelection Select(EmTrajectory previousTrajectory, VehicleMotionState measuredState,
        int expectedSegmentIndex, TravelDirection expectedDirection, DateTimeOffset now,
        EmPlannerConfiguration configuration)
    {
        if (measuredState == null)
            throw new ArgumentNullException(nameof(measuredState));
        if (expectedSegmentIndex < 0 || !Enum.IsDefined(typeof(TravelDirection), expectedDirection))
            throw new ArgumentOutOfRangeException(nameof(expectedSegmentIndex));
        if (previousTrajectory == null)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.MissingTrajectory);
        if (!TryReadConfiguration(configuration, out double maximumAgeSeconds, out double lookaheadSeconds,
                out double spatialToleranceMeters, out double kinematicTolerance))
        {
            return Measured(measuredState, TrajectoryHandoffRejectionReason.InvalidConfiguration);
        }
        if (previousTrajectory.Metadata.SegmentIndex != expectedSegmentIndex)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.SegmentMismatch);
        if (previousTrajectory.Metadata.Direction != expectedDirection)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.DirectionMismatch);

        double trajectoryAgeSeconds = (now - previousTrajectory.Metadata.EffectiveAtUtc).TotalSeconds;
        if (trajectoryAgeSeconds < -TimeEpsilonSeconds)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryNotYetEffective);
        if (trajectoryAgeSeconds > maximumAgeSeconds + TimeEpsilonSeconds)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrajectoryTooOld);
        if (!sampler.TrySample(previousTrajectory, trajectoryAgeSeconds, out EmTrajectoryPoint currentPoint))
            return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);
        if (!Tracks(currentPoint, measuredState, spatialToleranceMeters, kinematicTolerance))
            return Measured(measuredState, TrajectoryHandoffRejectionReason.TrackingErrorExceeded);

        double handoffTime = trajectoryAgeSeconds + lookaheadSeconds;
        double terminalTime = previousTrajectory.Points[previousTrajectory.Points.Count - 1].TimeFromStart;
        if (handoffTime > terminalTime + TimeEpsilonSeconds)
            return Measured(measuredState, TrajectoryHandoffRejectionReason.HandoffBeyondTrajectory);
        TrajectoryHandoffRejectionReason boundaryReason = BoundaryInInterval(previousTrajectory, trajectoryAgeSeconds,
            handoffTime);
        if (boundaryReason != TrajectoryHandoffRejectionReason.None)
            return Measured(measuredState, boundaryReason);
        if (!sampler.TrySample(previousTrajectory, handoffTime, out EmTrajectoryPoint handoffPoint))
            return Measured(measuredState, TrajectoryHandoffRejectionReason.SampleUnavailable);

        var startState = new VehicleMotionState(new Pose2D(handoffPoint.X, handoffPoint.Y, handoffPoint.Yaw),
            handoffPoint.SignedLongitudinalVelocity, handoffPoint.LongitudinalAcceleration,
            previousTrajectory.Metadata.EffectiveAtUtc.AddSeconds(handoffTime), measuredState.SequenceId);
        return new TrajectoryHandoffSelection(TrajectoryHandoffSource.PreviousTrajectory, startState,
            previousTrajectory, handoffPoint, TrajectoryHandoffRejectionReason.None);
    }

    private static TrajectoryHandoffSelection Measured(VehicleMotionState measuredState,
        TrajectoryHandoffRejectionReason rejectionReason)
    {
        return new TrajectoryHandoffSelection(TrajectoryHandoffSource.MeasuredState, measuredState, null, null,
            rejectionReason);
    }

    private static bool TryReadConfiguration(EmPlannerConfiguration configuration, out double maximumAgeSeconds,
        out double lookaheadSeconds, out double spatialToleranceMeters, out double kinematicTolerance)
    {
        maximumAgeSeconds = configuration?.Scheduling?.MaximumVehicleStateAgeSeconds ?? double.NaN;
        lookaheadSeconds = configuration?.Scheduling?.HandoffLookaheadSeconds ?? double.NaN;
        spatialToleranceMeters = configuration?.Validation?.SpatialToleranceMeters ?? double.NaN;
        kinematicTolerance = configuration?.Validation?.KinematicTolerance ?? double.NaN;
        return IsNonNegativeFinite(maximumAgeSeconds) && IsNonNegativeFinite(lookaheadSeconds) &&
            IsNonNegativeFinite(spatialToleranceMeters) && IsNonNegativeFinite(kinematicTolerance);
    }

    private static bool Tracks(EmTrajectoryPoint trajectoryPoint, VehicleMotionState measuredState,
        double spatialToleranceMeters, double kinematicTolerance)
    {
        double dx = trajectoryPoint.X - measuredState.Pose.X;
        double dy = trajectoryPoint.Y - measuredState.Pose.Y;
        if (Math.Sqrt(dx * dx + dy * dy) > spatialToleranceMeters)
            return false;
        double yawError = Math.Atan2(Math.Sin(trajectoryPoint.Yaw - measuredState.Pose.Heading),
            Math.Cos(trajectoryPoint.Yaw - measuredState.Pose.Heading));
        return Math.Abs(yawError) <= kinematicTolerance &&
            Math.Abs(trajectoryPoint.SignedLongitudinalVelocity -
                measuredState.SignedLongitudinalSpeedMetersPerSecond) <= kinematicTolerance;
    }

    private static TrajectoryHandoffRejectionReason BoundaryInInterval(EmTrajectory trajectory, double startTime,
        double endTime)
    {
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            if (point.TimeFromStart <= startTime + TimeEpsilonSeconds || point.TimeFromStart > endTime + TimeEpsilonSeconds)
                continue;
            if (point.BoundaryType == EmBoundaryType.GearSwitchApproach ||
                point.BoundaryType == EmBoundaryType.GearSwitchDeparture)
            {
                return TrajectoryHandoffRejectionReason.GearBoundary;
            }
            if (point.BoundaryType == EmBoundaryType.RollingSafetyStop || point.BoundaryType == EmBoundaryType.Goal)
                return TrajectoryHandoffRejectionReason.TerminalBoundary;
        }
        return TrajectoryHandoffRejectionReason.None;
    }

    private static bool IsNonNegativeFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d;
    }
}