BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Hik/HikVisionQr.cs

using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared.Geometry;
using System.Diagnostics;
using System.Net;
using System.Net.Sockets;
using System.Text;

namespace RobotNet10.RobotApp.Drivers.Hik;

/// <summary>
/// Driver Hik cho Camera QR
/// </summary>
[Device(DeviceType.CameraQr, "Hik", "HikVisionQr", "1.0.0", Description = "Camera QR Code Detection Hik Driver")]
public class HikVisionQr : DeviceBase, ICameraQr
{
    private readonly ILogger _logger;
    private readonly Lock _dataLock = new();
    private Thread? _receiveThread;
    private CancellationTokenSource? _receiveCts;
    private UdpClient? _udpClient;
    private IPEndPoint? _localEndPoint;
    private IPEndPoint? _remoteEndPoint;

    // Camera parameters
    private readonly int _cameraWidth;
    private readonly int _cameraHeight;
    private readonly double _distanceToQr;
    private readonly double _fovRangeWidthMm;
    private readonly double _fovRangeHeightMm;
    private readonly double _fovRangeDistanceMm;
    private readonly double _focalLengthPixels;
    private readonly string _localIp;
    private readonly int _localPort;
    private readonly double _qrDataTimeoutSeconds;

    // QR data dictionary (PoseStamped already contains timestamp in Header.Stamp)
    private readonly Dictionary<string, PoseStamped> _qrDictionary = [];

    // Data rate tracking
    private int _packetCount = 0;
    private double _currentDataRate = 0.0;

    public Dictionary<string, PoseStamped> Codes
    {
        get
        {
            lock (_dataLock)
            {
                var validCodes = new Dictionary<string, PoseStamped>();
                var now = DateTime.UtcNow;
                var expiredKeys = new List<string>();

                foreach (var (code, poseStamped) in _qrDictionary)
                {
                    var elapsed = (now - poseStamped.Header.Stamp).TotalSeconds;
                    if (elapsed <= _qrDataTimeoutSeconds)
                    {
                        // Data is still valid
                        validCodes[code] = poseStamped;
                    }
                    else
                    {
                        // Data expired, mark for removal
                        expiredKeys.Add(code);
                    }
                }

                // Clean up expired entries
                foreach (var key in expiredKeys)
                {
                    _qrDictionary.Remove(key);
                }

                return validCodes;
            }
        }
    }

    public HikVisionQr(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
        : base(deviceId, deviceName, DeviceType.CameraQr)
    {
        
        _logger = serviceProvider.GetRequiredService<ILoggerFactory>().CreateLogger<HikVisionQr>();
        // Read configuration
        var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
        var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
        var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");

        // Camera parameters
        _cameraWidth = connection.GetValue<int?>("CameraWidth") ?? 1920;
        _cameraHeight = connection.GetValue<int?>("CameraHeight") ?? 1080;
        _distanceToQr = connection.GetValue<double?>("DistanceToQr") ?? 0.1;
        _fovRangeWidthMm = connection.GetValue<double?>("FovRangeWidthMm") ?? 170.0;
        _fovRangeHeightMm = connection.GetValue<double?>("FovRangeHeightMm") ?? 130.0;
        _fovRangeDistanceMm = connection.GetValue<double?>("FovRangeDistanceMm") ?? 100.0;
        _qrDataTimeoutSeconds = connection.GetValue<double?>("QrDataTimeoutSeconds") ?? 3.0;

        // Compute focal length in pixels from FOV range (pinhole camera model)
        // f = CameraWidth * FovRangeDistance / FovRangeWidth
        _focalLengthPixels = _cameraWidth * _fovRangeDistanceMm / _fovRangeWidthMm;

        // UDP parameters
        _localIp = connection.GetValue<string>("LocalIP") ?? "192.168.254.100";
        _localPort = connection.GetValue<int?>("LocalPort") ?? 1024;

        if (autoReconnectEnabled.HasValue)
            AutoReconnectEnabled = autoReconnectEnabled.Value;

        if (reconnectDelayMs.HasValue)
            ReconnectDelayMs = reconnectDelayMs.Value;

        if (maxReconnectAttempts.HasValue)
            MaxReconnectAttempts = maxReconnectAttempts.Value;

        // Initialize properties
        UpdateProperties();
    }

    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
    {
        yield return new PropertyDescription("DataRate", "Data Rate", "Tần số nhận dữ liệu (packets/s)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 1,
            Category = "Trạng thái",
            DefaultValue = "0"
        };
    }

    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        await Task.CompletedTask;
    }

    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
    {
        return await Task.FromResult(_udpClient != null && _receiveThread != null && _receiveThread.IsAlive);
    }

    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
    {
        // Create local endpoint and UDP client
        _localEndPoint = new IPEndPoint(IPAddress.Parse(_localIp), _localPort);
        _udpClient = new UdpClient(_localEndPoint);

        // Create cancellation token source for receive thread
        _receiveCts = new CancellationTokenSource();

        // Create and start high-priority thread for receiving data
        _receiveThread = new Thread(() => ReceiveDataLoop(_receiveCts.Token))
        {
            IsBackground = true,
            Priority = ThreadPriority.Highest,
            Name = $"HikQr-{DeviceId}-ReceiveThread"
        };
        _receiveThread.Start();

        await Task.CompletedTask;
    }

    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        // Stop receive thread
        _receiveCts?.Cancel();

        // Wait for thread to finish (with timeout)
        if (_receiveThread != null && _receiveThread.IsAlive)
        {
            var joined = _receiveThread.Join(TimeSpan.FromSeconds(5));
            if (!joined)
            {
                // Thread didn't finish gracefully
                System.Diagnostics.Debug.WriteLine($"[HikQr] Receive thread did not finish in time");
            }
        }

        _receiveCts?.Dispose();
        _receiveCts = null;
        _receiveThread = null;

        // Close and dispose UDP client
        _udpClient?.Close();
        _udpClient?.Dispose();
        _udpClient = null;
        _localEndPoint = null;
        _remoteEndPoint = null;

        await Task.CompletedTask;
    }

    protected override async Task OnResetAsync(CancellationToken cancellationToken)
    {
        lock (_dataLock)
        {
            // Clear QR dictionary
            _qrDictionary.Clear();
            _packetCount = 0;
            _currentDataRate = 0.0;
            UpdateProperties();
        }

        await Task.CompletedTask;
    }

    private void ReceiveDataLoop(CancellationToken cancellationToken)
    {
        Thread.BeginThreadAffinity();
        try
        {
            var stopwatch = Stopwatch.StartNew();
            var packetCountInSecond = 0;

            while (!cancellationToken.IsCancellationRequested)
            {
                try
                {
                    // Check if 1 second has elapsed
                    if (stopwatch.Elapsed.TotalSeconds >= 1.0)
                    {
                        lock (_dataLock)
                        {
                            _currentDataRate = packetCountInSecond / stopwatch.Elapsed.TotalSeconds;
                            UpdateProperties();
                        }

                        // Reset counter and stopwatch
                        packetCountInSecond = 0;
                        stopwatch.Restart();
                    }

                    // Check if UDP client is available
                    if (_udpClient == null)
                        break;

                    // Set receive timeout to allow checking cancellation token
                    _udpClient.Client.ReceiveTimeout = 100;

                    // Receive UDP packet
                    var receivedData = _udpClient.Receive(ref _remoteEndPoint);

                    if (receivedData.Length > 0)
                    {
                        packetCountInSecond++;
                        lock (_dataLock)
                        {
                            _packetCount++;
                        }

                        // Process received data
                        UpdateData(receivedData);
                    }
                }
                catch (SocketException ex) when (ex.SocketErrorCode == SocketError.TimedOut)
                {
                    // Timeout is expected, continue loop
                    continue;
                }
                catch (OperationCanceledException)
                {
                    break;
                }
                catch (Exception ex)
                {
                    // Log error and continue
                    _logger.LogError(ex, "[HikVisionQr] ReceiveDataLoop error");
                }
            }

            stopwatch.Stop();
        }
        finally
        {
            Thread.EndThreadAffinity();
        }
    }

    private void UpdateData(byte[] receivedData)
    {
        lock (_dataLock)
        {
            if (receivedData.Length == 0)
            {
                // No data, clear dictionary
                _qrDictionary.Clear();
            }
            else
            {
                try
                {
                    // Extract message (40 bytes from position 37)
                    if (receivedData.Length < 77)
                    {
                        // Invalid packet size
                        return;
                    }

                    byte[] messageBytes = new byte[40];
                    Array.Copy(receivedData, 37, messageBytes, 0, 40);
                    string message = Encoding.UTF8.GetString(messageBytes);

                    // Extract QR data (9 bytes)
                    byte[] dataBytes = new byte[9];
                    Array.Copy(messageBytes, 0, dataBytes, 0, 9);
                    string qrCode = Encoding.UTF8.GetString(dataBytes);

                    // Extract X position (3 bytes from position 11)
                    byte[] xBytes = new byte[3];
                    Array.Copy(messageBytes, 11, xBytes, 0, 3);
                    int qrPositionX = int.Parse(Encoding.ASCII.GetString(xBytes));

                    // Extract Y position (3 bytes from position 19)
                    byte[] yBytes = new byte[3];
                    Array.Copy(messageBytes, 19, yBytes, 0, 3);
                    int qrPositionY = int.Parse(Encoding.ASCII.GetString(yBytes));

                    // Extract angle (between ')' and '@')
                    double qrAngle = 0;
                    int start = message.IndexOf(')') + 1;
                    int end = message.IndexOf('@', start);
                    if (start > 0 && end > start)
                    {
                        string numberAngle = message[start..end];
                        qrAngle = double.Parse(numberAngle);
                    }

                    // Calculate pose immediately
                    var pose = CreatePoseFromPixels(
                        qrPositionX,
                        qrPositionY,
                        qrAngle,
                        _cameraWidth,
                        _cameraHeight,
                        _distanceToQr,
                        _focalLengthPixels);

                    if (pose.HasValue)
                    {
                        // Console.WriteLine($"[HikQr] Detected QR Code: {qrCode}, X: {pose.Value.Pose.Position.X}, Y: {pose.Value.Pose.Position.Y}, Angle: {pose.Value.Pose.Orientation.ToYawDegrees()} degrees");
                        // Add or update dictionary (timestamp is in pose.Header.Stamp)
                        _qrDictionary[qrCode] = pose.Value;
                    }
                }
                catch (Exception ex)
                {
                    System.Diagnostics.Debug.WriteLine($"[HikQr] UpdateData parsing error: {ex.Message}");
                }
            }

            UpdateProperties();
        }
    }

    private void UpdateProperties()
    {
        lock (_dataLock)
        {
            SetProperty("DataRate", _currentDataRate.ToString("F2"));
        }
    }

    protected override void Dispose(bool disposing)
    {
        if (disposing)
        {
            _receiveCts?.Cancel();
            _receiveCts?.Dispose();
            _udpClient?.Close();
            _udpClient?.Dispose();
        }
        base.Dispose(disposing);
    }

    #region ICameraQr Implementation

    public PoseStamped? this[string code]
    {
        get
        {
            lock (_dataLock)
            {
                if (!_qrDictionary.TryGetValue(code, out var poseStamped))
                    return null;

                // Check if data is still valid (within timeout)
                var elapsed = (DateTime.UtcNow - poseStamped.Header.Stamp).TotalSeconds;
                if (elapsed > _qrDataTimeoutSeconds)
                {
                    // Data expired, remove from dictionary
                    _qrDictionary.Remove(code);
                    return null;
                }

                // Data is valid, return pose
                return poseStamped;
            }
        }
    }

    private static PoseStamped? CreatePoseFromPixels(
        int? px,
        int? py,
        double? angleDeg,
        int cameraWidth,
        int cameraHeight,
        double distanceMeters,
        double focalLengthPixels)
    {
        if (!px.HasValue || !py.HasValue || !angleDeg.HasValue)
            return null;

        // Convert pixel coordinates to meters using pinhole camera model
        // Same focal length for both axes (square pixels: 4.8μm × 4.8μm)
        double xMeters = (px.Value - cameraWidth / 2.0) * distanceMeters / focalLengthPixels;
        double yMeters = -(py.Value - cameraHeight / 2.0) * distanceMeters / focalLengthPixels; // Invert Y axis
        double zMeters = distanceMeters;

        // Convert angle to quaternion
        double angleRad = angleDeg.Value * Math.PI / 180.0;
        var quat = RobotNet10.Shared.Numbers.Quaternion.FromYawRadian(angleRad);

        // Create header
        var header = new RobotNet10.Shared.Header
        {
            Seq = 0,
            Stamp = DateTime.UtcNow,
            FrameId = "camera"
        };

        // Create pose
        var pose = new Pose
        {
            Position = new Point { X = xMeters, Y = yMeters, Z = zMeters },
            Orientation = new Quaternion(quat.X, quat.Y, quat.Z, quat.W)
        };

        return new PoseStamped(header, pose);
    }

    #endregion
}