Denso/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Hubs/LidarHub.cs

using Microsoft.AspNetCore.Authorization;
using Microsoft.AspNetCore.SignalR;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// SignalR Hub cho Lidar device - cung cấp real-time lidar scan data
/// </summary>
[Authorize]
public class LidarHub(IDeviceProvider deviceProvider) : Hub
{
    /// <summary>
    /// Độ phân giải mới: 1 độ (1 tia/độ) tính bằng radian
    /// </summary>
    private const double TARGET_ANGLE_INCREMENT_RAD = (Math.PI / 180.0); // 1 độ = π/180 radian

    /// <summary>
    /// Lấy thông tin device (DeviceName) theo device ID
    /// </summary>
    public async Task<DeviceInfoDto?> GetDeviceInfo(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ILidar)
        {
            return null;
        }

        return new DeviceInfoDto
        {
            DeviceId = device.DeviceId,
            DeviceName = device.DeviceName
        };
    }

    /// <summary>
    /// Lấy thông tin lidar scan data theo device ID
    /// Trả về LaserScan với độ phân giải mới 1 độ (1 tia/độ)
    /// </summary>
    public async Task<LaserScan?> GetLidarData(string deviceId)
    {
        var device = deviceProvider.GetDevice(deviceId);
        if (device is not ILidar lidar || lidar.CurrentMeasurementData == null)
        {
            return null;
        }

        var originalScan = lidar.CurrentMeasurementData.Value;

        // Tạo LaserScan mới với độ phân giải 1°
        var newScan = new LaserScan
        {
            Header = originalScan.Header,
            AngleMin = originalScan.AngleMin,
            AngleMax = originalScan.AngleMax,
            AngleIncrement = TARGET_ANGLE_INCREMENT_RAD, // 1° spacing (π/180 rad)
            TimeIncrement = originalScan.TimeIncrement,
            ScanTime = originalScan.ScanTime,
            RangeMin = originalScan.RangeMin,
            RangeMax = originalScan.RangeMax
        };

        // Tính số điểm mới với độ phân giải 1 độ
        var angleSpan = originalScan.AngleMax - originalScan.AngleMin;
        var newPointCount = (int)Math.Round(angleSpan / TARGET_ANGLE_INCREMENT_RAD) + 1;
        newScan.Ranges = new double[newPointCount];
        newScan.Intensities = new double[newPointCount];

        // Tính toán lại Ranges và Intensities với độ phân giải mới
        for (int i = 0; i < newPointCount; i++)
        {
            // Tính góc bắt đầu và kết thúc cho bin hiện tại
            var targetAngleStart = originalScan.AngleMin + i * TARGET_ANGLE_INCREMENT_RAD;
            var targetAngleEnd = Math.Min(
                originalScan.AngleMin + (i + 1) * TARGET_ANGLE_INCREMENT_RAD,
                originalScan.AngleMax
            );

            // Tìm các index trong khoảng [targetAngleStart, targetAngleEnd]
            var indexStart = (targetAngleStart - originalScan.AngleMin) / originalScan.AngleIncrement;
            var indexEnd = (targetAngleEnd - originalScan.AngleMin) / originalScan.AngleIncrement;

            var index0 = (int)Math.Floor(indexStart);
            var index1 = (int)Math.Ceiling(indexEnd);

            // Clamp indices to valid range
            index0 = Math.Max(0, Math.Min(index0, originalScan.Ranges.Length - 1));
            index1 = Math.Max(0, Math.Min(index1, originalScan.Ranges.Length - 1));

            // Tính trung bình các điểm có intensity > 0 trong khoảng [index0, index1]
            double sumRange = 0;
            double sumIntensity = 0;
            int validPointCount = 0;

            for (int idx = index0; idx <= index1; idx++)
            {
                // Kiểm tra intensity để xác định điểm hợp lệ
                bool hasValidIntensity = originalScan.Intensities != null &&
                                        idx < originalScan.Intensities.Length &&
                                        originalScan.Intensities[idx] > 0;

                if (hasValidIntensity)
                {
                    var range = originalScan.Ranges[idx];

                    // Chỉ lấy các điểm không phải NaN/Infinity
                    if (!double.IsNaN(range) && !double.IsInfinity(range))
                    {
                        sumRange += range;
                        sumIntensity += originalScan.Intensities![idx];
                        validPointCount++;
                    }
                }
            }

            // Tính giá trị trung bình hoặc set -1 nếu không có điểm hợp lệ
            if (validPointCount > 0)
            {
                newScan.Ranges[i] = sumRange / validPointCount;
                newScan.Intensities[i] = sumIntensity / validPointCount;
            }
            else
            {
                newScan.Ranges[i] = -1.0;  // No valid data
                newScan.Intensities[i] = 0.0;
            }
        }

        // Final safety pass: ensure no NaN/Infinity values remain
        // This should ideally never catch anything if the logic above is correct
        for (int i = 0; i < newScan.Ranges.Length; i++)
        {
            if (double.IsNaN(newScan.Ranges[i]) || double.IsInfinity(newScan.Ranges[i]))
            {
                newScan.Ranges[i] = -1.0;
            }
        }

        return newScan;
    }
}