ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationVisualizationPublisher.cs

using System;
using System.Runtime.CompilerServices;
using TrajectoryPlanningVisualization;

[assembly: InternalsVisibleTo("EMPlannerVerificationHost")]

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

internal interface ITrajectoryObservationVisualizationSink
{
    PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options,
        PlanningVisualizationStaticSnapshot snapshot);
    void Publish(PlanningVisualizationDynamicSnapshot snapshot);
    void Stop();
}

internal sealed class TrajectoryObservationVisualizationPublisher
{
    private readonly TrajectoryObservationSettings settings;
    private readonly ITrajectoryObservationVisualizationSink sink;
    private readonly Action<string> log;
    private DateTimeOffset nextPublishAtUtc;
    private bool started;
    private bool stopped;
    private bool sinkStopped;
    private string faultReason = string.Empty;

    internal TrajectoryObservationVisualizationPublisher(TrajectoryObservationSettings settings,
        ITrajectoryObservationVisualizationSink sink, Action<string> log)
    {
        this.settings = settings ?? throw new ArgumentNullException(nameof(settings));
        this.sink = sink ?? throw new ArgumentNullException(nameof(sink));
        this.log = log ?? throw new ArgumentNullException(nameof(log));
    }

    internal string FaultReason => faultReason;

    internal PlanningVisualizationSessionInfo Start(PlanningVisualizationStaticSnapshot snapshot)
    {
        if (!settings.EnableWebVisualization || stopped || faultReason.Length != 0)
            return null;
        if (started) return null;

        try
        {
            if (snapshot == null) throw new ArgumentNullException(nameof(snapshot));
            PlanningVisualizationSessionInfo session = sink.Start(new PlanningVisualizationOptions
            {
                Port = settings.WebVisualizationPort,
                RefreshRateHz = Math.Max(1, (int)Math.Round(settings.WebRefreshRateHz,
                    MidpointRounding.AwayFromZero)),
                HistoryCycleLimit = settings.VisualizationHistoryCycleLimit,
            }, snapshot);
            if (session == null) throw new InvalidOperationException("网页会话未返回访问地址。");
            started = true;
            return session;
        }
        catch (Exception exception)
        {
            Fuse(exception);
            return null;
        }
    }

    internal bool TryPublish(DateTimeOffset now, Func<PlanningVisualizationDynamicSnapshot> snapshotFactory)
    {
        if (snapshotFactory == null) throw new ArgumentNullException(nameof(snapshotFactory));
        if (!settings.EnableWebVisualization || !started || stopped || faultReason.Length != 0)
            return false;
        if (now < nextPublishAtUtc) return false;

        try
        {
            PlanningVisualizationDynamicSnapshot snapshot = snapshotFactory();
            if (snapshot == null) throw new InvalidOperationException("网页动态快照为空。");
            nextPublishAtUtc = now + TimeSpan.FromSeconds(1d / settings.WebRefreshRateHz);
            sink.Publish(snapshot);
            return true;
        }
        catch (Exception exception)
        {
            Fuse(exception);
            return false;
        }
    }

    internal void Disable(Exception exception)
    {
        if (exception == null) throw new ArgumentNullException(nameof(exception));
        Fuse(exception);
    }

    internal void Stop()
    {
        if (stopped) return;
        stopped = true;
        StopSink();
    }

    private void Fuse(Exception exception)
    {
        if (faultReason.Length != 0) return;
        faultReason = "网页可视化已关闭：" + exception.GetType().Name + "：" + exception.Message;
        StopSink();
        log(faultReason);
    }

    private void StopSink()
    {
        if (sinkStopped) return;
        sinkStopped = true;
        try
        {
            sink.Stop();
        }
        catch
        {
        }
    }
}

internal sealed class PlanningVisualizationSessionSink : ITrajectoryObservationVisualizationSink
{
    private PlanningVisualizationSession session;

    public PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options,
        PlanningVisualizationStaticSnapshot snapshot)
    {
        if (session != null) return session.Start(snapshot);
        session = new PlanningVisualizationSession(options);
        return session.Start(snapshot);
    }

    public void Publish(PlanningVisualizationDynamicSnapshot snapshot)
    {
        session?.Publish(snapshot);
    }

    public void Stop()
    {
        PlanningVisualizationSession current = session;
        session = null;
        current?.Dispose();
    }
}