BQP/srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Execution/TuningNavigation.cs

using System.Threading.Channels;
using Microsoft.AspNetCore.SignalR;
using RobotNet10.NavigationTune.Hubs;
using RobotNet10.NavigationTune.Shared.Hubs;
using RobotNet10.NavigationTune.Shared.Interfaces;
using RobotNet10.NavigationTune.Shared.Models;
using RobotNet10.NavigationTune.Services;

namespace RobotNet10.NavigationTune.Execution;

/// <summary>
/// Tuning Navigation wrapper implementation.
/// Navigation algorithm runs in its own loop; telemetry is published on a dedicated publisher loop.
/// </summary>
public class TuningNavigation(
    ITestExecutor testExecutor,
    IMetricsCalculator metricsCalculator,
    IHubContext<TuningHub>? hubContext = null) : ITuningNavigation
{
    private readonly ITestExecutor _testExecutor = testExecutor;
    private readonly IMetricsCalculator _metricsCalculator = metricsCalculator;
    private readonly IHubContext<TuningHub>? _hubContext = hubContext;

    private TestScenario? _currentScenario;
    private NavigationParameterSet? _currentParameters;
    private TestExecutionResult? _currentResult;
    private Guid _currentTestRunId; // Set at start of run so telemetry can be pushed during execution
    private Task<TestExecutionResult>? _executionTask;
    private Task? _telemetryPublisherTask;
    private Channel<TelemetryData>? _telemetryChannel;

    public event Action<TelemetryData>? OnTelemetryUpdate;
    public event Action<SafetyViolation>? OnSafetyViolation;
    public event Action<TestStatus>? OnStatusChanged;

    // Safety violation handler
    private void HandleSafetyViolation(SafetyViolation violation)
    {
        OnSafetyViolation?.Invoke(violation);
        PublishSafetyEvent(violation);
    }

    public bool IsTestRunning => _executionTask != null && !_executionTask.IsCompleted;

    public async Task<TestExecutionResult> ExecuteTestAsync(
        TestScenario scenario,
        NavigationParameterSet parameters,
        Guid? testRunId = null,
        CancellationToken cancellationToken = default,
        Action<TestExecutionResult>? onComplete = null)
    {
        if (IsTestRunning)
            throw new InvalidOperationException("Test is already running");

        _currentScenario = scenario;
        _currentParameters = parameters;
        _currentTestRunId = testRunId ?? Guid.Empty;

        // Dedicated channel for telemetry: navigation loop only enqueues (no I/O in algorithm thread)
        _telemetryChannel = Channel.CreateUnbounded<TelemetryData>(new UnboundedChannelOptions
        {
            SingleReader = true,
            SingleWriter = true
        });

        // Start dedicated publisher loop (runs independently from navigation algorithm)
        _telemetryPublisherTask = RunTelemetryPublisherLoopAsync(cancellationToken);

        if (onComplete != null)
        {
            // Fire-and-forget: control loop runs on WatchThread; return immediately with Running
            _ = _testExecutor.ExecuteAsync(
                scenario,
                parameters,
                telemetry => _telemetryChannel.Writer.TryWrite(telemetry),
                cancellationToken,
                onComplete: result =>
                {
                    FinishExecution(result);
                    _telemetryChannel?.Writer.Complete();
                    _executionTask = null;
                    onComplete(result);
                });
            return new TestExecutionResult
            {
                TestRunId = _currentTestRunId,
                Status = TestStatus.Running,
                StartTime = DateTime.UtcNow
            };
        }

        // Await mode
        _executionTask = _testExecutor.ExecuteAsync(
            scenario,
            parameters,
            telemetry => _telemetryChannel.Writer.TryWrite(telemetry),
            cancellationToken
        );

        try
        {
            _currentResult = await _executionTask;
            FinishExecution(_currentResult);
            return _currentResult;
        }
        finally
        {
            _telemetryChannel?.Writer.Complete();
            if (_telemetryPublisherTask != null)
            {
                try
                {
                    await _telemetryPublisherTask;
                }
                catch (OperationCanceledException) { }
            }
            _telemetryPublisherTask = null;
            _telemetryChannel = null;
            _executionTask = null;
        }
    }

    private void FinishExecution(TestExecutionResult result)
    {
        if (_currentTestRunId != Guid.Empty)
            result.TestRunId = _currentTestRunId;

        if (result.TelemetryData.Count > 0 && _currentScenario != null)
        {
            var pathPoints = _currentScenario.GenerateReferencePath();
            var referencePath = new ReferencePath
            {
                Points = pathPoints,
                TotalLength = pathPoints.Count > 0 ? pathPoints[^1].DistanceFromStart : 0
            };
            result.Metrics = _metricsCalculator.CalculateMetrics(result.TelemetryData, referencePath);
        }

        OnStatusChanged?.Invoke(result.Status);
        PublishTestStatus(result.Status, result.TestRunId);
    }

    /// <summary>
    /// Dedicated loop for publishing telemetry (and raising events). Runs independently from navigation algorithm.
    /// </summary>
    private async Task RunTelemetryPublisherLoopAsync(CancellationToken cancellationToken)
    {
        if (_telemetryChannel == null) return;

        var reader = _telemetryChannel.Reader;
        try
        {
            await foreach (var telemetry in reader.ReadAllAsync(cancellationToken))
            {
                OnTelemetryUpdate?.Invoke(telemetry);
                PublishTelemetryUpdate(telemetry);
            }
        }
        catch (OperationCanceledException) { }
    }

    public TestProgress GetProgress()
    {
        return _testExecutor.GetProgress();
    }

    public void Pause()
    {
        _testExecutor.Pause();
        OnStatusChanged?.Invoke(TestStatus.Paused);
        PublishTestStatus(TestStatus.Paused, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
    }

    public void Resume()
    {
        _testExecutor.Resume();
        OnStatusChanged?.Invoke(TestStatus.Running);
        PublishTestStatus(TestStatus.Running, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
    }

    public void Stop()
    {
        _testExecutor.Stop();
        OnStatusChanged?.Invoke(TestStatus.Aborted);
        PublishTestStatus(TestStatus.Aborted, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
    }

    public void EmergencyStop()
    {
        _testExecutor.EmergencyStop();
        OnStatusChanged?.Invoke(TestStatus.EmergencyStopped);
        PublishTestStatus(TestStatus.EmergencyStopped, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
    }

    private void PublishTelemetryUpdate(TelemetryData telemetry)
    {
        if (_hubContext == null) return;

        var groupId = _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty;
        if (groupId == Guid.Empty) return;

        var dto = new TelemetryUpdateDto
        {
            TimestampMs = telemetry.TimestampMs,
            X = telemetry.RobotPose.X,
            Y = telemetry.RobotPose.Y,
            Theta = telemetry.RobotPose.Theta,
            LinearVelocity = telemetry.RobotTwist.Linear,
            AngularVelocity = telemetry.RobotTwist.Angular,
            CrossTrackError = telemetry.CrossTrackError,
            HeadingError = telemetry.HeadingError,
            DistanceToGoal = telemetry.DistanceToGoal,
            CommandedLinearVelocity = telemetry.CommandTwist.Linear,
            CommandedAngularVelocity = telemetry.CommandTwist.Angular
        };

        _ = _hubContext.Clients.Group($"test_{groupId}")
            .SendAsync("ReceiveTelemetry", dto);
    }

    private void PublishTestStatus(TestStatus status, Guid testRunId)
    {
        if (_hubContext == null) return;

        var progress = GetProgress();
        var dto = new TestStatusUpdateDto
        {
            TestRunId = testRunId,
            Status = status,
            ProgressPercent = progress.ProgressPercent,
            Message = status.ToString()
        };

        _ = _hubContext.Clients.Group($"test_{testRunId}")
            .SendAsync("ReceiveTestStatus", dto);
    }

    private void PublishSafetyEvent(SafetyViolation violation)
    {
        if (_hubContext == null) return;

        var groupId = _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty;
        if (groupId == Guid.Empty) return;

        var dto = new SafetyEventDto
        {
            TestRunId = groupId,
            Type = violation.Type,
            Severity = violation.Severity,
            Message = violation.Message,
            Timestamp = violation.Timestamp
        };

        _ = _hubContext.Clients.Group($"test_{groupId}")
            .SendAsync("ReceiveSafetyEvent", dto);
    }
}