BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Hubs/XlocPoseHub.cs

using Microsoft.AspNetCore.SignalR;
using System.Threading.Tasks;

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// SignalR Hub for streaming XLOC pose data in realtime
/// AND controlling SLAM operations manually (start/stop localization/mapping)
/// </summary>
public class XlocPoseHub : Hub
{
    private readonly ILogger<XlocPoseHub> _logger;
    private readonly Xloc.XlocIntegrationService? _xlocService;

    public XlocPoseHub(ILogger<XlocPoseHub> logger, Xloc.XlocIntegrationService? xlocService = null)
    {
        _logger = logger;
        _xlocService = xlocService;
    }

    public override async Task OnConnectedAsync()
    {
        _logger.LogInformation("Client connected to XlocPoseHub: {ConnectionId}", Context.ConnectionId);
        await base.OnConnectedAsync();
    }

    public override async Task OnDisconnectedAsync(Exception? exception)
    {
        _logger.LogInformation("Client disconnected from XlocPoseHub: {ConnectionId}", Context.ConnectionId);
        await base.OnDisconnectedAsync(exception);
    }

    /// <summary>
    /// Client requests current pose (on-demand)
    /// </summary>
    public async Task RequestCurrentPose()
    {
        _logger.LogDebug("Client {ConnectionId} requested current pose", Context.ConnectionId);
        await Task.CompletedTask;
    }

    #region Manual SLAM Control Methods

    /// <summary>
    /// Activate a map for localization
    /// </summary>
    public async Task<bool> ActivateMap(string mapFilePath)
    {
        if (_xlocService == null)
        {
            _logger.LogWarning("XlocIntegrationService not available");
            return false;
        }

        _logger.LogInformation("Client {ConnectionId} activating map: {MapPath}", Context.ConnectionId, mapFilePath);
        return _xlocService.ActivateMap(mapFilePath);
    }

    /// <summary>
    /// Start localization mode
    /// </summary>
    public async Task<bool> StartLocalization()
    {
        if (_xlocService == null)
        {
            _logger.LogWarning("XlocIntegrationService not available");
            return false;
        }

        _logger.LogInformation("Client {ConnectionId} starting localization", Context.ConnectionId);
        return _xlocService.StartLocalization();
    }

    /// <summary>
    /// Stop localization mode
    /// </summary>
    public async Task<bool> StopLocalization()
    {
        if (_xlocService == null)
        {
            _logger.LogWarning("XlocIntegrationService not available");
            return false;
        }

        _logger.LogInformation("Client {ConnectionId} stopping localization", Context.ConnectionId);
        return _xlocService.StopLocalization();
    }

    /// <summary>
    /// Start mapping mode
    /// </summary>
    public async Task<bool> StartMapping()
    {
        if (_xlocService == null)
        {
            _logger.LogWarning("XlocIntegrationService not available");
            return false;
        }

        _logger.LogInformation("Client {ConnectionId} starting mapping", Context.ConnectionId);
        return _xlocService.StartMapping();
    }

    /// <summary>
    /// Stop mapping and save to file
    /// </summary>
    public async Task<bool> StopMapping(string saveMapFilePath)
    {
        if (_xlocService == null)
        {
            _logger.LogWarning("XlocIntegrationService not available");
            return false;
        }

        _logger.LogInformation("Client {ConnectionId} stopping mapping, saving to: {MapPath}", 
            Context.ConnectionId, saveMapFilePath);
        return _xlocService.StopMapping(saveMapFilePath);
    }

    /// <summary>
    /// Get current 2D pose (x, y, yaw)
    /// </summary>
    public async Task<object?> GetCurrentPose2D()
    {
        if (_xlocService == null)
            return null;

        var pose = _xlocService.GetCurrentPose2D();
        if (!pose.HasValue)
            return null;

        var (x, y, yaw) = pose.Value;
        return new { x, y, yaw, yawDegrees = yaw * 180.0 / Math.PI };
    }

    /// <summary>
    /// Get sampled laser scan data (1 degree intervals) for web visualization
    /// Returns array of {angle, range} objects
    /// </summary>
    public async Task<object?> GetSampledLaserScan()
    {
        if (_xlocService == null)
            return null;

        var laserData = _xlocService.GetSampledLaserScan();
        if (laserData == null || laserData.Count == 0)
            return null;

        // Convert to JSON-friendly format
        var points = laserData.Select(p => new { angle = p.angle, range = p.range }).ToArray();
        
        return new { 
            pointCount = points.Length,
            points = points,
            timestamp = DateTime.UtcNow
        };
    }

    /// <summary>
    /// Get XLOC diagnostics data
    /// </summary>
    public async Task<object?> GetDiagnostics()
    {
        if (_xlocService == null)
            return null;

        var diagnostics = _xlocService.GetDiagnostics();
        if (diagnostics == null)
            return null;

        return new
        {
            header = new
            {
                seq = diagnostics.HeaderSeq,
                stamp = new
                {
                    sec = diagnostics.HeaderStampSec,
                    nsec = diagnostics.HeaderStampNsec
                },
                frameId = diagnostics.HeaderFrameId
            },
            xlocState = diagnostics.XlocState,
            stateString = diagnostics.StateString,
            currentActiveMap = diagnostics.CurrentActiveMap,
            reliability = diagnostics.Reliability,
            matchingScore = diagnostics.MatchingScore
        };
    }

    #endregion
}

/// <summary>
/// Pose data for SignalR clients
/// </summary>
public class XlocPoseData
{
    /// <summary>
    /// X position in meters
    /// </summary>
    public double X { get; set; }

    /// <summary>
    /// Y position in meters
    /// </summary>
    public double Y { get; set; }

    /// <summary>
    /// Z position in meters
    /// </summary>
    public double Z { get; set; }

    /// <summary>
    /// Yaw angle in radians
    /// </summary>
    public double Yaw { get; set; }

    /// <summary>
    /// Yaw angle in degrees
    /// </summary>
    public double YawDegrees => Yaw * 180.0 / Math.PI;

    /// <summary>
    /// Quaternion X
    /// </summary>
    public double QuaternionX { get; set; }

    /// <summary>
    /// Quaternion Y
    /// </summary>
    public double QuaternionY { get; set; }

    /// <summary>
    /// Quaternion Z
    /// </summary>
    public double QuaternionZ { get; set; }

    /// <summary>
    /// Quaternion W
    /// </summary>
    public double QuaternionW { get; set; }

    /// <summary>
    /// Timestamp of pose
    /// </summary>
    public DateTime Timestamp { get; set; }

    /// <summary>
    /// Is localization active
    /// </summary>
    public bool IsActive { get; set; }

    /// <summary>
    /// Xloc state (0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR)
    /// </summary>
    public byte XlocState { get; set; }

    /// <summary>
    /// Current active map name
    /// </summary>
    public string MapName { get; set; } = string.Empty;

    /// <summary>
    /// Localization reliability (0.0 to 1.0)
    /// </summary>
    public double Reliability { get; set; }

    /// <summary>
    /// SLAM matching score
    /// </summary>
    public double MatchingScore { get; set; }
}