BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/XlocAsyncDispatcher.cs

using System.Threading.Channels;

namespace RobotNet10.RobotApp.Xloc;

/// <summary>
/// Single-thread async dispatcher for xloc sensor data — mirrors ROS ros::spin() model.
/// All sensor types (IMU, Odom, Scan) are dispatched sequentially by ONE worker thread,
/// eliminating lock contention on the native xloc library.
///
/// ROS model: ros::spin() processes all callbacks in a single thread, no mutex needed.
/// This dispatcher replicates that pattern: one queue, one worker, zero lock contention.
/// </summary>
public class XlocAsyncDispatcher : IDisposable
{
    private readonly XlocClient _xlocClient;
    private readonly ILogger? _logger;

    // Single unified queue for ALL sensor types — like ROS callback queue
    private readonly Channel<SensorDispatchEvent> _dispatchQueue;
    private readonly Task _workerTask;
    private readonly CancellationTokenSource _cts;
    private bool _disposed = false;

    private const int DefaultQueueCapacity = 300;

    private readonly int _queueCapacity;
    private long _dropCount;

    /// <summary>
    /// Sensor dispatch event wrapper
    /// </summary>
    private record SensorDispatchEvent(
        string SensorType,
        string SensorId,
        Action DispatchAction,
        long EnqueueTimestamp);

    public XlocAsyncDispatcher(
        XlocClient xlocClient,
        int queueCapacity = DefaultQueueCapacity,
        ILogger? logger = null)
    {
        _xlocClient = xlocClient ?? throw new ArgumentNullException(nameof(xlocClient));
        _logger = logger;
        _queueCapacity = queueCapacity;

        _dispatchQueue = Channel.CreateBounded<SensorDispatchEvent>(new BoundedChannelOptions(queueCapacity)
        {
            FullMode = BoundedChannelFullMode.DropOldest
        });

        _cts = new CancellationTokenSource();
        _workerTask = RunWorker(_cts.Token);
    }

    public ValueTask EnqueueOdometryAsync(Action dispatchAction, string sensorId = "odom")
    {
        ThrowIfDisposed();
        return EnqueueAsync("odometry", sensorId, dispatchAction);
    }

    public ValueTask EnqueueImuAsync(Action dispatchAction, string sensorId = "imu")
    {
        ThrowIfDisposed();
        return EnqueueAsync("imu", sensorId, dispatchAction);
    }

    public ValueTask EnqueueLaserScanAsync(Action dispatchAction, string sensorId)
    {
        ThrowIfDisposed();
        return EnqueueAsync("laserscan", sensorId, dispatchAction);
    }

    private ValueTask EnqueueAsync(string sensorType, string sensorId, Action dispatchAction)
    {
        var enqueueTs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
        var @event = new SensorDispatchEvent(sensorType, sensorId, dispatchAction, enqueueTs);

        if (_dispatchQueue.Reader.Count >= _queueCapacity)
        {
            Interlocked.Increment(ref _dropCount);
        }

        try
        {
            return _dispatchQueue.Writer.WriteAsync(@event, _cts.Token);
        }
        catch (ChannelClosedException)
        {
            throw new InvalidOperationException("XlocAsyncDispatcher has been disposed.");
        }
    }

    private async Task RunWorker(CancellationToken ct)
    {
        _logger?.LogInformation("[XLOC-ASYNC] Single dispatch worker started (ROS spin model)");

        try
        {
            await foreach (var @event in _dispatchQueue.Reader.ReadAllAsync(ct))
            {
                ExecuteDispatchEvent(@event);
            }
        }
        catch (OperationCanceledException)
        {
            _logger?.LogInformation("[XLOC-ASYNC] Dispatch worker cancelled");
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[XLOC-ASYNC-FATAL] Dispatch worker crashed: {Message}", ex.Message);
        }
        finally
        {
            _logger?.LogInformation("[XLOC-ASYNC] Dispatch worker stopped");
        }
    }

    private void ExecuteDispatchEvent(SensorDispatchEvent @event)
    {
        var queueWaitMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - @event.EnqueueTimestamp;

        try
        {
            var dispatchStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

            @event.DispatchAction();

            var dispatchElapsedMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - dispatchStart;

            _logger?.LogTrace(
                "[XLOC-ASYNC] {SensorType} sensor={SensorId} queue_wait={QueueWaitMs}ms dispatch={DispatchMs}ms",
                @event.SensorType,
                @event.SensorId,
                queueWaitMs,
                dispatchElapsedMs);

            if (queueWaitMs >= 10 || dispatchElapsedMs >= 10)
            {
                // _logger?.LogWarning(
                //     "[XLOC-DIAG] AsyncDispatcher {SensorType} sensor={SensorId} queue_wait={QueueWaitMs}ms dispatch={DispatchMs}ms",
                //     @event.SensorType,
                //     @event.SensorId,
                //     queueWaitMs,
                //     dispatchElapsedMs);
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex,
                "[XLOC-ASYNC-ERROR] Failed to dispatch {SensorType} sensor={SensorId}: {Message}",
                @event.SensorType,
                @event.SensorId,
                ex.Message);
        }
    }

    public void Dispose()
    {
        Dispose(true);
        GC.SuppressFinalize(this);
    }

    protected virtual void Dispose(bool disposing)
    {
        if (_disposed)
            return;

        _logger?.LogInformation("[XLOC-ASYNC] Shutting down dispatcher");

        _dispatchQueue.Writer.TryComplete();
        _cts.Cancel();

        try
        {
            _workerTask.Wait(TimeSpan.FromSeconds(5));
        }
        catch (AggregateException ex)
        {
            _logger?.LogWarning(ex, "[XLOC-ASYNC] Worker task did not complete gracefully");
        }

        _cts.Dispose();

        _disposed = true;
        _logger?.LogInformation("[XLOC-ASYNC] Dispatcher shut down");
    }

    private void ThrowIfDisposed()
    {
        if (_disposed)
            throw new InvalidOperationException("XlocAsyncDispatcher has been disposed.");
    }

    ~XlocAsyncDispatcher()
    {
        Dispose(false);
    }
}