ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPlanningDeadline.cs

using System;
using System.Diagnostics;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.EMPlanner;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

/// <summary>Owns one monotonic time budget shared by every phase of an observation planning cycle.</summary>
public sealed class TrajectoryObservationPlanningDeadline : IDisposable
{
    private static readonly TimeSpan MaximumTimerDelay = TimeSpan.FromMilliseconds(uint.MaxValue - 1d);
    private readonly object authorizationGate = new object();
    private readonly TimeSpan limit;
    private readonly Stopwatch stopwatch;
    private readonly TimeSpan? injectedElapsed;
    private readonly CancellationToken callerToken;
    private readonly CancellationTokenSource expirationCancellation;
    private readonly CancellationTokenSource linkedCancellation;
    private readonly Timer expirationTimer;
    private readonly IEmPlanningPublicationAuthorization publicationAuthorization;
    private bool callerCancellationObserved;
    private bool deadlineExpired;
    private bool disposed;

    public TrajectoryObservationPlanningDeadline(TimeSpan limit, CancellationToken callerToken = default)
        : this(limit, null, callerToken)
    {
    }

    private TrajectoryObservationPlanningDeadline(TimeSpan limit, TimeSpan? injectedElapsed,
        CancellationToken callerToken)
    {
        if (limit < TimeSpan.Zero)
            throw new ArgumentOutOfRangeException(nameof(limit), "The planning deadline cannot be negative.");
        if (injectedElapsed.HasValue && injectedElapsed.Value < TimeSpan.Zero)
            throw new ArgumentOutOfRangeException(nameof(injectedElapsed));

        this.limit = limit;
        this.injectedElapsed = injectedElapsed;
        this.callerToken = callerToken;
        stopwatch = injectedElapsed.HasValue ? null : Stopwatch.StartNew();
        expirationCancellation = new CancellationTokenSource();
        TimeSpan initialRemaining = ComputeRemaining();
        deadlineExpired = initialRemaining <= TimeSpan.Zero;
        if (deadlineExpired)
            expirationCancellation.Cancel();
        linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
            callerToken, expirationCancellation.Token);
        publicationAuthorization = new DeadlinePublicationAuthorization(this);
        if (!deadlineExpired)
        {
            TimeSpan timerDelay = initialRemaining < MaximumTimerDelay
                ? initialRemaining
                : MaximumTimerDelay;
            expirationTimer = new Timer(ExpireFromTimer, null, timerDelay, Timeout.InfiniteTimeSpan);
        }
    }

    public TimeSpan Remaining
    {
        get
        {
            bool cancelExpiration = false;
            TimeSpan remaining;
            lock (authorizationGate)
            {
                remaining = ComputeRemaining();
                if (!deadlineExpired && remaining <= TimeSpan.Zero)
                {
                    deadlineExpired = true;
                    cancelExpiration = true;
                }
                if (deadlineExpired)
                    remaining = TimeSpan.Zero;
            }
            if (cancelExpiration)
                CancelExpirationToken();
            return remaining;
        }
    }

    public bool IsExpired => callerToken.IsCancellationRequested || DeadlineExpired;

    public CancellationToken Token => linkedCancellation.Token;

    internal bool CallerCancellationRequested => callerToken.IsCancellationRequested;

    internal CancellationToken CallerToken => callerToken;

    internal CancellationToken DeadlineToken => expirationCancellation.Token;

    internal IEmPlanningPublicationAuthorization PublicationAuthorization => publicationAuthorization;

    internal static TrajectoryObservationPlanningDeadline CreateForTesting(TimeSpan limit,
        TimeSpan elapsed, CancellationToken callerToken)
    {
        return new TrajectoryObservationPlanningDeadline(limit, elapsed, callerToken);
    }

    public TimeSpan Clamp(TimeSpan configuredLimit)
    {
        if (configuredLimit < TimeSpan.Zero)
            throw new ArgumentOutOfRangeException(nameof(configuredLimit));
        TimeSpan remaining = Remaining;
        return configuredLimit < remaining ? configuredLimit : remaining;
    }

    public void Dispose()
    {
        lock (authorizationGate)
            disposed = true;
        expirationTimer?.Dispose();
        linkedCancellation.Dispose();
        expirationCancellation.Dispose();
    }

    private bool DeadlineExpired
    {
        get
        {
            _ = Remaining;
            lock (authorizationGate)
                return deadlineExpired;
        }
    }

    private TimeSpan ComputeRemaining()
    {
        TimeSpan elapsed = injectedElapsed ?? stopwatch.Elapsed;
        TimeSpan remaining = limit - elapsed;
        return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
    }

    private EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
        CancellationToken coordinatorCallerCancellationToken, Action publish)
    {
        if (publish == null)
            throw new ArgumentNullException(nameof(publish));

        bool cancelExpiration = false;
        using CancellationTokenRegistration ownerCallerRegistration =
            callerToken.Register(ObserveCallerCancellation);
        using CancellationTokenRegistration requestCallerRegistration =
            requestCallerCancellationToken.Register(ObserveCallerCancellation);
        using CancellationTokenRegistration coordinatorCallerRegistration =
            coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
        EmPlanningPublicationDecision decision;
        lock (authorizationGate)
        {
            if (callerToken.IsCancellationRequested ||
                requestCallerCancellationToken.IsCancellationRequested ||
                coordinatorCallerCancellationToken.IsCancellationRequested)
            {
                callerCancellationObserved = true;
            }
            if (!deadlineExpired && ComputeRemaining() <= TimeSpan.Zero)
            {
                deadlineExpired = true;
                cancelExpiration = true;
            }
            if (callerCancellationObserved)
                decision = EmPlanningPublicationDecision.CallerCancelled;
            else if (deadlineExpired || disposed)
                decision = EmPlanningPublicationDecision.DeadlineExpired;
            else
            {
                publish();
                decision = EmPlanningPublicationDecision.Published;
            }
        }
        if (cancelExpiration)
            CancelExpirationToken();
        return decision;
    }

    private void ObserveCallerCancellation()
    {
        lock (authorizationGate)
            callerCancellationObserved = true;
    }

    private void ExpireFromTimer(object state)
    {
        bool cancelExpiration = false;
        lock (authorizationGate)
        {
            if (!disposed && !deadlineExpired)
            {
                deadlineExpired = true;
                cancelExpiration = true;
            }
        }
        if (cancelExpiration)
            CancelExpirationToken();
    }

    private void CancelExpirationToken()
    {
        try
        {
            expirationCancellation.Cancel();
        }
        catch (ObjectDisposedException)
        {
            // Disposal can race a timer callback only after the owning cycle has completed.
        }
    }

    private sealed class DeadlinePublicationAuthorization : IEmPlanningPublicationAuthorization
    {
        private readonly TrajectoryObservationPlanningDeadline owner;

        public DeadlinePublicationAuthorization(TrajectoryObservationPlanningDeadline owner)
        {
            this.owner = owner;
        }

        public TimeSpan Remaining => owner.Remaining;

        public bool IsExpired => owner.DeadlineExpired;

        public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
            CancellationToken coordinatorCallerCancellationToken, Action publish) =>
            owner.TryPublish(requestCallerCancellationToken, coordinatorCallerCancellationToken, publish);
    }
}