using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;
using Sick.SafetyScanners;
using Sick.SafetyScanners.DataStructures;
using Sick.SafetyScanners.Types;
using System.Reactive;
using UdpScanDataEventArgs = Sick.SafetyScanners.DataStructures.UdpScanDataEventArgs;

namespace RobotNet10.RobotApp.Drivers.Sick;

/// <summary>
/// Cấu hình cho SICK Safety Scanner LiDAR Driver
/// </summary>
public class SickLidarDriverConfig
{
    /// <summary>
    /// Địa chỉ IP của LiDAR (required)
    /// </summary>
    public string IpAddress { get; set; } = string.Empty;

    /// <summary>
    /// TCP port (COLA2) - mặc định: 2122
    /// Chỉ dùng để config scanner nếu cần, không dùng để poll scan data
    /// </summary>
    public ushort TcpPort { get; set; } = 2122;

    /// <summary>
    /// UDP local port (for streaming mode) - mặc định: 0 (auto-assign)
    /// </summary>
    public ushort UdpLocalPort { get; set; } = 0;

    /// <summary>
    /// Host IP address (local machine IP for UDP streaming) - optional
    /// Nếu null hoặc empty, sẽ tự động detect IP cùng subnet với scanner IP khi connect
    /// </summary>
    public string? HostIp { get; set; }

    /// <summary>
    /// Start angle (degrees) - optional
    /// Nếu null, sẽ lấy từ scanner hoặc dùng all angles
    /// Lưu ý: Giá trị trong config là DEGREES, sẽ được convert sang RADIANS
    /// </summary>
    public double? StartAngle { get; set; }

    /// <summary>
    /// End angle (degrees) - optional
    /// Nếu null, sẽ lấy từ scanner hoặc dùng all angles
    /// Lưu ý: Giá trị trong config là DEGREES, sẽ được convert sang RADIANS
    /// </summary>
    public double? EndAngle { get; set; }

    /// <summary>
    /// Connect timeout (milliseconds) - mặc định: 5000
    /// </summary>
    public int ConnectTimeoutMs { get; set; } = 5000;

    /// <summary>
    /// Command timeout (milliseconds) - mặc định: 5000
    /// </summary>
    public int CommandTimeoutMs { get; set; } = 5000;

    /// <summary>
    /// Bật/tắt tự động reconnect - mặc định: true
    /// </summary>
    public bool AutoReconnectEnabled { get; set; } = true;

    /// <summary>
    /// Thời gian chờ trước khi reconnect (milliseconds) - mặc định: 3000
    /// </summary>
    public int ReconnectDelayMs { get; set; } = 3000;

    /// <summary>
    /// Số lần reconnect tối đa - mặc định: 0 (unlimited)
    /// </summary>
    public int MaxReconnectAttempts { get; set; } = 0;

    /// <summary>
    /// Topic cho scan data (ROS-style), dùng trong LaserScan header hoặc publish - mặc định: /scan
    /// </summary>
    public string ScanTopic { get; set; } = "/scan";

    /// <summary>
    /// Frame ID cho LaserScan header (coordinate frame) - mặc định: lidar_frame
    /// </summary>
    public string FrameId { get; set; } = "lidar_frame";

    /// <summary>
    /// Offset góc (degrees) áp dụng lên mọi góc báo ra trong LaserScan.
    /// Ví dụ: lidar gửi -45°..225°, cần nhận -135°..135° thì đặt AngleOffsetDeg = -90.
    /// Mặc định: 0 (không offset).
    /// </summary>
    public double AngleOffsetDeg { get; set; } = 0;

    /// <summary>
    /// Khoảng cách đo tối thiểu (mét) - mặc định: 0.05m (5cm)
    /// SICK nanoScan3 hỗ trợ min range ~5cm.
    /// </summary>
    public double RangeMin { get; set; } = 0.05;

    /// <summary>
    /// Khoảng cách đo tối đa (mét) - mặc định: 40m
    /// SICK nanoScan3 hỗ trợ max range 40m (measurement data output).
    /// Giá trị này được dùng làm RangeMax trong LaserScan, ảnh hưởng tới
    /// SLAM, định vị XLOC, navigation và lọc điểm quét.
    /// </summary>
    public double RangeMax { get; set; } = 40.0;

    /// <summary>
    /// Parse cấu hình từ IConfigurationSection
    /// </summary>
    public static SickLidarDriverConfig Parse(IConfigurationSection configuration)
    {
        var config = new SickLidarDriverConfig();

        // IpAddress (required)
        config.IpAddress = configuration["IpAddress"] ?? throw new InvalidOperationException(
            "IpAddress is required in connection configuration");

        // TCP port - mặc định: 2122, hỗ trợ cả "TcpPort" và "Port"
        var tcpPortStr = configuration["TcpPort"] ?? configuration["Port"] ?? "2122";
        if (!ushort.TryParse(tcpPortStr, out var tcpPort) || tcpPort == 0)
        {
            throw new InvalidOperationException($"Invalid TcpPort: {tcpPortStr}. Must be between 1 and 65535");
        }
        config.TcpPort = tcpPort;

        // UDP local port - mặc định: 0 (auto-assign)
        var udpPortStr = configuration["UdpLocalPort"] ?? "0";
        if (!ushort.TryParse(udpPortStr, out var udpPort))
        {
            throw new InvalidOperationException($"Invalid UdpLocalPort: {udpPortStr}. Must be between 0 and 65535");
        }
        config.UdpLocalPort = udpPort;

        // Host IP - optional, null = auto-detect
        config.HostIp = configuration["HostIp"];

        // Start angle - optional (degrees)
        var startAngleStr = configuration["StartAngle"];
        if (startAngleStr != null && double.TryParse(startAngleStr, out var startAngleDeg))
        {
            config.StartAngle = startAngleDeg;
        }

        // End angle - optional (degrees)
        var endAngleStr = configuration["EndAngle"];
        if (endAngleStr != null && double.TryParse(endAngleStr, out var endAngleDeg))
        {
            config.EndAngle = endAngleDeg;
        }

        // Connect timeout - mặc định: 5000ms
        var connectTimeoutStr = configuration["ConnectTimeoutMs"] ?? "5000";
        if (!int.TryParse(connectTimeoutStr, out var connectTimeout) || connectTimeout < 1000)
        {
            connectTimeout = 5000; // Default 5 seconds
        }
        config.ConnectTimeoutMs = connectTimeout;

        // Command timeout - mặc định: 5000ms
        var commandTimeoutStr = configuration["CommandTimeoutMs"] ?? "5000";
        if (!int.TryParse(commandTimeoutStr, out var commandTimeout) || commandTimeout < 1000)
        {
            commandTimeout = 5000; // Default 5 seconds
        }
        config.CommandTimeoutMs = commandTimeout;

        // Auto reconnect settings
        var autoReconnectEnabled = configuration.GetValue<bool?>("AutoReconnectEnabled");
        config.AutoReconnectEnabled = autoReconnectEnabled ?? true; // Default true

        var reconnectDelayMs = configuration.GetValue<int?>("ReconnectDelayMs");
        config.ReconnectDelayMs = reconnectDelayMs ?? 3000; // Default 3 seconds

        var maxReconnectAttempts = configuration.GetValue<int?>("MaxReconnectAttempts");
        config.MaxReconnectAttempts = maxReconnectAttempts ?? 0; // Default 0 (unlimited)

        // ScanTopic - mặc định: /scan (hỗ trợ cả "ScanTopic" và "Topic")
        config.ScanTopic = configuration["ScanTopic"] ?? configuration["Topic"] ?? "/scan";
        if (string.IsNullOrWhiteSpace(config.ScanTopic))
            config.ScanTopic = "/scan";

        // FrameId - mặc định: lidar_frame
        config.FrameId = configuration["FrameId"] ?? "lidar_frame";
        if (string.IsNullOrWhiteSpace(config.FrameId))
            config.FrameId = "lidar_frame";

        // AngleOffsetDeg - mặc định: 0 (lidar -45°..225° → -135°..135° thì đặt -90)
        var angleOffsetStr = configuration["AngleOffsetDeg"];
        if (angleOffsetStr != null && double.TryParse(angleOffsetStr, out var angleOffsetDeg))
        {
            config.AngleOffsetDeg = angleOffsetDeg;
        }

        // RangeMin - mặc định: 0.05m (SICK nanoScan3 min range)
        var rangeMin = configuration.GetValue<double?>("RangeMin");
        if (rangeMin.HasValue && rangeMin.Value >= 0)
        {
            config.RangeMin = rangeMin.Value;
        }

        // RangeMax - mặc định: 40m (SICK nanoScan3 max measurement range)
        var rangeMax = configuration.GetValue<double?>("RangeMax");
        if (rangeMax.HasValue && rangeMax.Value > 0)
        {
            config.RangeMax = rangeMax.Value;
        }

        if (config.RangeMax <= config.RangeMin)
        {
            throw new InvalidOperationException(
                $"RangeMax must be greater than RangeMin. RangeMin={config.RangeMin}, RangeMax={config.RangeMax}");
        }

        return config;
    }
}

/// <summary>
/// Driver cho thiết bị LiDAR SICK Safety Scanner
/// Tập trung vào cung cấp dữ liệu Measurement qua UDP streaming
/// Kế thừa DeviceBase và implement ILidar
/// </summary>
[Device(DeviceType.Lidar, "SICK AG", "SickLidarDriver", "1.0.0",
    Description = "SICK Safety Scanner LiDAR Driver - UDP Streaming Mode")]
public class SickLidarDriver : DeviceBase, ILidar
{
    private readonly string _ipAddress;
    private readonly ushort _tcpPort;
    private readonly ushort _udpLocalPort;
    private readonly string? _hostIp; // Local IP address for UDP streaming (from config or auto-detected)
    private readonly double? _startAngle; // Start angle in radians (from config, optional)
    private readonly double? _endAngle; // End angle in radians (from config, optional)
    private readonly int _connectTimeoutMs;
    private readonly int _commandTimeoutMs;
    private readonly string _scanTopic;
    private readonly string _frameId;
    private readonly double _angleOffsetRad;
    private readonly double _rangeMinM;
    private readonly double _rangeMaxM;

    private SafetyScanner? _scanner;
    private readonly Lock _scannerLock = new();

    // Cached device information (optional - only for display purposes)
    private string? _serialNumber;
    private string? _firmwareVersion;
    private string? _deviceName;
    private string? _orderNumber;
    private DateTime _lastUpdateTime = DateTime.UtcNow;
    private readonly Lock _infoLock = new();

    // Cached scan data from UDP streaming
    private UdpScanData? _lastScanData;
    private DateTime? _lastScanDataTimestamp;
    private readonly Lock _scanDataLock = new();

    /// <summary>
    /// Constructor
    /// </summary>
    public SickLidarDriver(string deviceId, string deviceName, IConfigurationSection configuration)
        : base(deviceId, deviceName, DeviceType.Lidar)
    {
        // Parse cấu hình từ IConfigurationSection
        var config = SickLidarDriverConfig.Parse(configuration);

        // Lưu các giá trị vào private fields
        _ipAddress = config.IpAddress;
        _tcpPort = config.TcpPort;
        _udpLocalPort = config.UdpLocalPort;
        _hostIp = string.IsNullOrWhiteSpace(config.HostIp) ? null : config.HostIp;

        // Convert StartAngle và EndAngle từ degrees sang radians
        if (config.StartAngle.HasValue)
        {
            _startAngle = (config.StartAngle.Value * Math.PI / 180.0);
        }
        else
        {
            _startAngle = null;
        }

        if (config.EndAngle.HasValue)
        {
            _endAngle = (config.EndAngle.Value * Math.PI / 180.0);
        }
        else
        {
            _endAngle = null;
        }

        _connectTimeoutMs = config.ConnectTimeoutMs;
        _commandTimeoutMs = config.CommandTimeoutMs;
        _scanTopic = config.ScanTopic;
        _frameId = config.FrameId;
        _angleOffsetRad = config.AngleOffsetDeg * Math.PI / 180.0;
        _rangeMinM = config.RangeMin;
        _rangeMaxM = config.RangeMax;

        // Set auto reconnect properties
        AutoReconnectEnabled = config.AutoReconnectEnabled;
        ReconnectDelayMs = config.ReconnectDelayMs;
        MaxReconnectAttempts = config.MaxReconnectAttempts;

        // Khởi tạo giá trị properties
        SetProperty("IpAddress", _ipAddress);
        SetProperty("TcpPort", _tcpPort.ToString());
        SetProperty("UdpLocalPort", _udpLocalPort == 0 ? "Auto" : _udpLocalPort.ToString());
        SetProperty("HostIp", _hostIp ?? "Auto-detect");
        SetProperty("ConnectTimeoutMs", _connectTimeoutMs.ToString());
        SetProperty("CommandTimeoutMs", _commandTimeoutMs.ToString());
        // Hiển thị góc trong config (degrees) hoặc "Auto"
        SetProperty("StartAngle", _startAngle.HasValue ? $"{(_startAngle.Value * 180.0 / Math.PI):F1}°" : "Auto");
        SetProperty("EndAngle", _endAngle.HasValue ? $"{(_endAngle.Value * 180.0 / Math.PI):F1}°" : "Auto");
        SetProperty("ScanTopic", _scanTopic);
        SetProperty("FrameId", _frameId);
        SetProperty("AngleOffsetDeg", _angleOffsetRad == 0 ? "0" : $"{config.AngleOffsetDeg:F1}°");
        SetProperty("RangeMin", _rangeMinM.ToString("F3"));
        SetProperty("RangeMax", _rangeMaxM.ToString("F3"));
        SetProperty("ConnectionStatus", "Disconnected");
        SetProperty("SessionId", "");
        SetProperty("SerialNumber", "");
        SetProperty("FirmwareVersion", "");
        SetProperty("DeviceName", "");
        SetProperty("OrderNumber", "");
        SetProperty("LastUpdateTime", "");
        SetProperty("LastScanDataTimestamp", "");
    }

    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
    {
        yield return new PropertyDescription("IpAddress", "IP Address", "Địa chỉ IP của LiDAR")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 1,
            Category = "Kết nối",
            DefaultValue = ""
        };

        yield return new PropertyDescription("TcpPort", "TCP Port", "Port TCP của LiDAR (COLA2, chỉ dùng để config scanner)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 2,
            Category = "Kết nối",
            DefaultValue = "2122"
        };

        yield return new PropertyDescription("UdpLocalPort", "UDP Local Port", "Port UDP local để nhận scan data (0 = auto-assign)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 3,
            Category = "Kết nối",
            DefaultValue = "0"
        };

        yield return new PropertyDescription("HostIp", "Host IP", "Địa chỉ IP local để nhận UDP packets (auto-detect nếu không chỉ định)")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 4,
            Category = "Kết nối",
            DefaultValue = "Auto-detect"
        };

        yield return new PropertyDescription("ConnectTimeoutMs", "Connect Timeout (ms)",
            "Thời gian timeout khi kết nối (milliseconds)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 5,
            Category = "Cấu hình",
            DefaultValue = "5000"
        };

        yield return new PropertyDescription("CommandTimeoutMs", "Command Timeout (ms)",
            "Thời gian timeout cho COLA2 commands (milliseconds)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 6,
            Category = "Cấu hình",
            DefaultValue = "5000"
        };

        yield return new PropertyDescription("StartAngle", "Start Angle (degrees)",
            "Góc bắt đầu quét (độ). Nếu không chỉ định, sẽ lấy từ scanner hoặc dùng all angles. Giá trị trong config là degrees.")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 7,
            Category = "Cấu hình",
            DefaultValue = "Auto"
        };

        yield return new PropertyDescription("EndAngle", "End Angle (degrees)",
            "Góc kết thúc quét (độ). Nếu không chỉ định, sẽ lấy từ scanner hoặc dùng all angles. Giá trị trong config là degrees.")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 8,
            Category = "Cấu hình",
            DefaultValue = "Auto"
        };

        yield return new PropertyDescription("ScanTopic", "Scan Topic",
            "Topic cho scan data (ROS-style, mặc định /scan). Có thể dùng cho FrameId hoặc publish.")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 9,
            Category = "Cấu hình",
            DefaultValue = "/scan"
        };

        yield return new PropertyDescription("FrameId", "Frame ID",
            "Frame ID cho LaserScan header (coordinate frame).")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 10,
            Category = "Cấu hình",
            DefaultValue = "lidar_frame"
        };

        yield return new PropertyDescription("AngleOffsetDeg", "Angle Offset (deg)",
            "Offset góc (độ) áp dụng lên góc báo ra. Lidar -45°..225° → -135°..135° thì đặt -90.")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 11,
            Category = "Cấu hình",
            DefaultValue = "0"
        };

        yield return new PropertyDescription("RangeMin", "Range Min (m)",
            "Khoảng cách đo tối thiểu (mét). Mặc định 0.05m cho SICK nanoScan3.")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 12,
            Category = "Cấu hình",
            DefaultValue = "0.05"
        };

        yield return new PropertyDescription("RangeMax", "Range Max (m)",
            "Khoảng cách đo tối đa (mét). Mặc định 40m cho SICK nanoScan3 (measurement data output).")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 13,
            Category = "Cấu hình",
            DefaultValue = "40.0"
        };

        yield return new PropertyDescription("ConnectionStatus", "Connection Status", "Trạng thái kết nối")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 11,
            Category = "Trạng thái",
            DefaultValue = "Disconnected"
        };

        yield return new PropertyDescription("SessionId", "Session ID", "COLA2 Session ID hiện tại (nếu có)")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 12,
            Category = "Trạng thái",
            DefaultValue = ""
        };

        yield return new PropertyDescription("SerialNumber", "Serial Number", "Serial number của thiết bị")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 8,
            Category = "Thông tin thiết bị",
            DefaultValue = ""
        };

        yield return new PropertyDescription("FirmwareVersion", "Firmware Version", "Phiên bản firmware")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 9,
            Category = "Thông tin thiết bị",
            DefaultValue = ""
        };

        yield return new PropertyDescription("DeviceName", "Device Name", "Tên thiết bị")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 10,
            Category = "Thông tin thiết bị",
            DefaultValue = ""
        };

        yield return new PropertyDescription("OrderNumber", "Order Number", "Order number của thiết bị")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 11,
            Category = "Thông tin thiết bị",
            DefaultValue = ""
        };

        yield return new PropertyDescription("LastUpdateTime", "Last Update Time",
            "Thời gian cập nhật thông tin cuối cùng")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 12,
            Category = "Trạng thái",
            DefaultValue = ""
        };

        yield return new PropertyDescription("LastScanDataTimestamp", "Last Scan Data Timestamp",
            "Thời gian cập nhật dữ liệu scan cuối cùng")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 13,
            Category = "Trạng thái",
            DefaultValue = ""
        };

        yield return new PropertyDescription("NumberOfBeams", "Number Of Beams",
            "Số lượng dữ liệu trong scan")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 13,
            Category = "Trạng thái",
            DefaultValue = ""
        };
    }

    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        lock (_scannerLock)
        {
            if (_scanner != null)
            {
                // Unsubscribe from events before disposing
                _scanner.ScanDataReceived -= OnSafetyScannerScanDataReceived;
                _scanner.Dispose();
                _scanner = null;
            }

            // Tạo SafetyScanner instance với UDP port để enable UDP streaming
            // Driver này chỉ tập trung vào UDP streaming cho measurement data
            _scanner = new SafetyScanner(_ipAddress, _tcpPort, _udpLocalPort);
        }

        await Task.CompletedTask;
    }

    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
    {
        SafetyScanner? scanner;
        lock (_scannerLock)
        {
            scanner = _scanner;
        }

        if (scanner == null)
            throw new InvalidOperationException("Scanner not initialized. Call InitializeAsync first.");

        // Subscribe to SafetyScanner events để nhận scan data từ UDP
        scanner.ScanDataReceived += OnSafetyScannerScanDataReceived;

        // Kết nối TCP trước (chưa mở COLA2 session)
        // UDP streaming không cần COLA2 session, nhưng ChangeCommSettings cần
        scanner.Connect(openCola2Session: false);

        // Lấy local UDP port (sau khi UDP streaming đã start)
        if (_udpLocalPort == 0)
        {
            throw new InvalidOperationException("Failed to get local UDP port after starting UDP streaming");
        }

        // Xác định Host IP: từ config hoặc auto-detect
        string hostIp = _hostIp ?? DetectLocalIpAddress(_ipAddress);
        SetProperty("HostIp", hostIp);

        // Mở COLA2 session để có thể gọi ChangeCommSettings và RequestCurrentConfig
        scanner.Session.Open();

        // Cập nhật session ID
        var sessionId = scanner.Session.SessionId;
        SetProperty("SessionId", sessionId?.ToString() ?? "");

        // Xác định StartAngle và EndAngle: từ config, hoặc request từ scanner, hoặc default (all angles)
        double startAngle = 0.0;
        double endAngle = 0.0;

        if (_startAngle.HasValue && _endAngle.HasValue)
        {
            // Có trong config, sử dụng giá trị từ config
            startAngle = _startAngle.Value;
            endAngle = _endAngle.Value;
        }
        else
        {
            // Không có trong config, thử request từ scanner
            try
            {
                var configTimeoutDuration = TimeDuration.FromMilliseconds(_commandTimeoutMs);
                var currentConfig = scanner.RequestCurrentConfig(configTimeoutDuration);
                startAngle = currentConfig.StartAngle;
                endAngle = currentConfig.EndAngle;
                // Nếu cả hai đều 0, nghĩa là all angles (giữ nguyên)
            }
            catch (Exception ex)
            {
                // Nếu không request được, dùng default (0.0 = all angles)
                // Log error nhưng không throw - sẽ dùng all angles
                LastError = ex;
                OnErrorOccurred(ex, $"Failed to request current config from scanner, using default angles (all angles). Error: {ex.Message}");
                startAngle = 0.0;
                endAngle = 0.0;
            }
        }

        // Cấu hình scanner để gửi UDP packets về đúng IP và port
        // Sử dụng ChangeCommSettingsAsync để đảm bảo lidar gửi dữ liệu về UDP server đúng
        var timeoutDuration = TimeDuration.FromMilliseconds(_commandTimeoutMs);
        var commSettings = CommSettings.Create(
            channel: 0,
            hostIp: hostIp,
            hostUdpPort: _udpLocalPort,
            generalSystemState: false,
            derivedSettings: true,
            measurementData: true, // Quan trọng: enable measurement data
            intrusionData: false,
            applicationData: false,
            publishingFrequency: 1, // Publish every scan
            startAngle: 0, // Từ config (radians), scanner (radians), hoặc default (all angles nếu = 0)
            endAngle: 0,
            interfaceType: InterfaceType.NonSafeEthernet,
            enabled: true
        );

        scanner.ChangeCommSettings(commSettings, timeoutDuration);
        // Scanner đã được config để gửi UDP packets về {hostIp}:{localUdpPort}

        // Start UDP streaming TRƯỚC để lấy local UDP port
        // Sau đó sẽ dùng port này để config scanner
        scanner.StartUdpStreaming();

        // Update properties với angles đã sử dụng (từ config, scanner, hoặc default)
        // Hiển thị góc ở dạng degrees cho dễ đọc
        if (startAngle == 0.0 && endAngle == 0.0)
        {
            SetProperty("StartAngle", "All angles (0°)");
            SetProperty("EndAngle", "All angles (0°)");
        }
        else
        {
            SetProperty("StartAngle", $"{startAngle * 180.0 / Math.PI:F1}°");
            SetProperty("EndAngle", $"{endAngle * 180.0 / Math.PI:F1}°");
        }

        SetProperty("ConnectionStatus", "Connected");
    }

    /// <summary>
    /// Tự động detect local IP address cùng subnet với scanner IP
    /// </summary>
    private static string DetectLocalIpAddress(string scannerIpAddress)
    {
        try
        {
            // Parse scanner IP để lấy subnet
            if (!System.Net.IPAddress.TryParse(scannerIpAddress, out var scannerIp))
            {
                throw new ArgumentException($"Invalid scanner IP address: {scannerIpAddress}", nameof(scannerIpAddress));
            }

            var scannerBytes = scannerIp.GetAddressBytes();
            if (scannerBytes.Length != 4)
            {
                throw new ArgumentException($"Only IPv4 is supported. Scanner IP: {scannerIpAddress}", nameof(scannerIpAddress));
            }

            // Lấy tất cả network interfaces
            var networkInterfaces = System.Net.NetworkInformation.NetworkInterface.GetAllNetworkInterfaces();

            foreach (var networkInterface in networkInterfaces)
            {
                // Chỉ lấy interfaces đang up và không phải loopback
                if (networkInterface.OperationalStatus != System.Net.NetworkInformation.OperationalStatus.Up ||
                    networkInterface.NetworkInterfaceType == System.Net.NetworkInformation.NetworkInterfaceType.Loopback)
                {
                    continue;
                }

                // Lấy IP properties
                var ipProperties = networkInterface.GetIPProperties();
                foreach (var ipAddress in ipProperties.UnicastAddresses)
                {
                    // Chỉ lấy IPv4 addresses
                    if (ipAddress.Address.AddressFamily != System.Net.Sockets.AddressFamily.InterNetwork)
                        continue;

                    var localIpBytes = ipAddress.Address.GetAddressBytes();
                    if (localIpBytes.Length != 4)
                        continue;

                    // Kiểm tra xem có cùng subnet không (giả định /24 subnet)
                    // So sánh 3 bytes đầu (subnet mask 255.255.255.0)
                    if (localIpBytes[0] == scannerBytes[0] &&
                        localIpBytes[1] == scannerBytes[1] &&
                        localIpBytes[2] == scannerBytes[2])
                    {
                        return ipAddress.Address.ToString();
                    }
                }
            }

            // Nếu không tìm thấy IP cùng subnet, throw exception
            throw new InvalidOperationException(
                $"Could not detect local IP address in the same subnet as scanner IP {scannerIpAddress}. " +
                $"Please specify HostIp in connection configuration.");
        }
        catch (Exception ex)
        {
            throw new InvalidOperationException(
                $"Failed to auto-detect local IP address for scanner {scannerIpAddress}: {ex.Message}", ex);
        }
    }

    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        SafetyScanner? scanner;
        lock (_scannerLock)
        {
            scanner = _scanner;
        }

        if (scanner != null)
        {
            try
            {
                // Stop UDP streaming trước
                scanner.StopUdpStreaming();

                // Unsubscribe from SafetyScanner events
                scanner.ScanDataReceived -= OnSafetyScannerScanDataReceived;

                // Disconnect TCP (COLA2 session nếu có)
                scanner.Disconnect();
            }
            catch (Exception ex)
            {
                // Log error nhưng không throw
                LastError = ex;
                OnErrorOccurred(ex, "Error during disconnect");
            }

            SetProperty("ConnectionStatus", "Disconnected");
            SetProperty("SessionId", "");
        }

        await Task.CompletedTask;
    }

    /// <summary>
    /// Event handler for SafetyScanner.ScanDataReceived (UDP streaming)
    /// Converts SICK-specific UdpScanData to LaserScan
    /// </summary>
    private void OnSafetyScannerScanDataReceived(object? sender, UdpScanDataEventArgs e)
    {
        try
        {
            var timestamp = e.ScanData.Timestamp ?? DateTime.UtcNow;

            lock (_scanDataLock)
            {
                _lastScanData = e.ScanData;
                _lastScanDataTimestamp = timestamp;
            }

            // Convert to LaserScan and fire ILidar event
            if (e.ScanData.MeasurementData != null && !e.ScanData.MeasurementData.IsEmpty)
            {
                var laserScan = ConvertToLaserScan(e.ScanData, _frameId, _angleOffsetRad, _rangeMinM, _rangeMaxM);
                var eventArgs = new LidarScanDataEventArgs(timestamp, laserScan);
                ScanDataReceived?.Invoke(this, eventArgs);
            }

            // Update properties for UI
            SetProperty("LastScanDataTimestamp", _lastScanDataTimestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
            if (e.ScanData.MeasurementData != null && !e.ScanData.MeasurementData.IsEmpty)
            {
                SetProperty("NumberOfBeams", e.ScanData.MeasurementData.NumberOfBeams.ToString());
            }
        }
        catch (Exception ex)
        {
            LastError = ex;
            OnErrorOccurred(ex, "Error converting scan data");
        }
    }

    protected override async Task OnResetAsync(CancellationToken cancellationToken)
    {
        // Clear cached device info
        lock (_infoLock)
        {
            _serialNumber = null;
            _firmwareVersion = null;
            _deviceName = null;
            _orderNumber = null;
        }

        SetProperty("SerialNumber", "");
        SetProperty("FirmwareVersion", "");
        SetProperty("DeviceName", "");
        SetProperty("OrderNumber", "");
        SetProperty("LastUpdateTime", "");

        await Task.CompletedTask;
    }

    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
    {
        SafetyScanner? scanner;
        lock (_scannerLock)
        {
            scanner = _scanner;
        }

        if (scanner == null)
        {
            OnErrorOccurred(new InvalidOperationException("Scanner is null"),
                "OnCheckConnectionAsync: Scanner is null");
            return false;
        }

        try
        {
            // Check if UDP streaming is active
            // Driver này chỉ tập trung vào UDP streaming, nên chỉ cần check UDP streaming status
            if (scanner.IsUdpStreaming)
            {
                return true; // UDP streaming is active, connection is OK
            }
            else
            {
                OnErrorOccurred(new InvalidOperationException("UDP streaming is not active"),
                    "OnCheckConnectionAsync: UDP streaming is not active");
                return false;
            }
        }
        catch (Exception ex)
        {
            OnErrorOccurred(ex,
                $"OnCheckConnectionAsync: Unexpected error. " +
                $"Exception: {ex.GetType().Name}, Message: {ex.Message}");
            return false;
        }
    }

    #region ILidar Implementation

    // ILidar Scan Data Properties Implementation (common for all Lidar brands)

    public LaserScan? CurrentMeasurementData
    {
        get
        {
            lock (_scanDataLock)
            {
                if (_lastScanData?.MeasurementData == null || _lastScanData.MeasurementData.IsEmpty)
                    return null;

                return ConvertToLaserScan(_lastScanData, _frameId, _angleOffsetRad, _rangeMinM, _rangeMaxM);
            }
        }
    }

    public DateTime? LastScanDataTimestamp
    {
        get
        {
            lock (_scanDataLock)
            {
                return _lastScanDataTimestamp ?? _lastScanData?.Timestamp;
            }
        }
    }

    // ILidar Device Specifications

    public double MinAngleRad
    {
        get
        {
            lock (_scanDataLock)
            {
                if (_lastScanData?.DerivedValues != null && !_lastScanData.DerivedValues.IsEmpty)
                {
                    return _lastScanData.DerivedValues.StartAngle + _angleOffsetRad;
                }
                return -Math.PI + _angleOffsetRad;
            }
        }
    }

    public double MaxAngleRad
    {
        get
        {
            lock (_scanDataLock)
            {
                if (_lastScanData?.DerivedValues != null && !_lastScanData.DerivedValues.IsEmpty)
                {
                    var startAngle = _lastScanData.DerivedValues.StartAngle;
                    var numberOfBeams = _lastScanData.DerivedValues.NumberOfBeams;
                    var resolution = _lastScanData.DerivedValues.AngularBeamResolution;
                    if (numberOfBeams > 0 && resolution != 0)
                    {
                        return startAngle + (numberOfBeams - 1) * resolution + _angleOffsetRad;
                    }
                }
                return Math.PI + _angleOffsetRad;
            }
        }
    }

    public double MinRangeM => _rangeMinM;

    public double MaxRangeM => _rangeMaxM;

    public double? AngularResolutionRad
    {
        get
        {
            lock (_scanDataLock)
            {
                if (_lastScanData?.DerivedValues != null && !_lastScanData.DerivedValues.IsEmpty)
                {
                    return _lastScanData.DerivedValues.AngularBeamResolution;
                }
                return null;
            }
        }
    }

    public double? ScanFrequencyHz
    {
        get
        {
            lock (_scanDataLock)
            {
                if (_lastScanData?.DerivedValues != null && !_lastScanData.DerivedValues.IsEmpty)
                {
                    var scanTimeMs = _lastScanData.DerivedValues.ScanTime;
                    if (scanTimeMs > 0)
                    {
                        return 1000.0 / scanTimeMs; // Convert ms to Hz
                    }
                }
                return null;
            }
        }
    }

    public double FieldOfViewRad => Math.Abs(MaxAngleRad - MinAngleRad);

    public bool SupportsIntensity => true; // SICK Safety Scanner supports reflectivity/intensity

    public double? AccuracyM => null; // SICK Safety Scanner accuracy ~1cm

    // Events

    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;

    #endregion

    // SICK-specific Properties (optional - for advanced usage)

    /// <summary>
    /// SICK-specific: Scan data gần nhất (parsed UDP packet)
    /// </summary>
    public UdpScanData? LastScanData
    {
        get
        {
            lock (_scanDataLock)
            {
                return _lastScanData;
            }
        }
    }

    /// <summary>
    /// SICK-specific: Derived values hiện tại (cấu hình scan: angles, resolution, number of beams)
    /// </summary>
    public DerivedValues? CurrentDerivedValues
    {
        get
        {
            lock (_scanDataLock)
            {
                return _lastScanData?.DerivedValues;
            }
        }
    }

    /// <summary>
    /// Converts SICK-specific MeasurementData to LaserScan structure
    /// Note: All angles in DerivedValues and ScanPoints are already in radians (converted by ParseDerivedValues and ParseMeasurementData)
    /// angleOffsetRad: offset (radians) applied to reported angles, e.g. -90° so that -45°..225° becomes -135°..135°
    /// </summary>
    private static LaserScan ConvertToLaserScan(UdpScanData scanData, string frameId, double angleOffsetRad, double configuredRangeMinM, double configuredRangeMaxM)
    {
        var sickMeasurementData = scanData.MeasurementData!;
        var sickDerivedValues = scanData.DerivedValues;

        // Convert SICK ScanPoints to ranges and intensities arrays
        var numberOfBeams = (int)sickMeasurementData.NumberOfBeams;
        var ranges = new double[numberOfBeams];
        var intensities = new double[numberOfBeams];

        for (int i = 0; i < numberOfBeams && i < sickMeasurementData.ScanPoints.Count; i++)
        {
            var sickPoint = sickMeasurementData.ScanPoints[i];

            // Convert distance from mm to meters
            var distanceM = sickPoint.Distance / 1000.0;

            // If invalid, set to NaN
            ranges[i] = sickPoint.IsValid ? distanceM : double.NaN;

            // Convert reflectivity (0-255) to normalized intensity (0-1)
            intensities[i] = sickPoint.Reflectivity;
        }

        // Extract derived values for metadata
        // Note: StartAngle and AngularBeamResolution from DerivedValues are already in radians
        // (converted by ParseDerivedValues from sensor units: radians = (sensorUnits / 4194304.0) * 2π)
        double angleMin = 0.0;
        double angleMax = 0.0;
        double angleIncrement = 0.0;
        double scanTime = 0.0;
        // Range min/max lấy từ config (mặc định 0.05m và 40m cho SICK nanoScan3).
        // DerivedValues không cung cấp StartDistance/EndDistance nên phải cấu hình theo thiết bị.
        double rangeMin = configuredRangeMinM;
        double rangeMax = configuredRangeMaxM;

        if (sickDerivedValues != null && !sickDerivedValues.IsEmpty)
        {
            angleMin = sickDerivedValues.StartAngle + angleOffsetRad; // Already in radians, apply offset
            // Calculate end angle: start + (numberOfBeams - 1) * resolution
            if (sickDerivedValues.NumberOfBeams > 0)
            {
                angleIncrement = sickDerivedValues.AngularBeamResolution; // unchanged
                angleMax = angleMin + (sickDerivedValues.NumberOfBeams - 1) * angleIncrement;
            }

            // Calculate scan time from scan frequency (ms to seconds)
            if (sickDerivedValues.ScanTime > 0)
            {
                scanTime = sickDerivedValues.ScanTime / 1000.0; // Convert ms to seconds
            }
        }

        // Ensure timestamp is UTC and properly formatted
        var timestamp = scanData.Timestamp ?? DateTime.UtcNow;
        // If timestamp is from SICK (has Kind = Unspecified), ensure it's treated as UTC
        if (timestamp.Kind == DateTimeKind.Unspecified)
        {
            timestamp = DateTime.SpecifyKind(timestamp, DateTimeKind.Utc);
        }
        else if (timestamp.Kind == DateTimeKind.Local)
        {
            timestamp = timestamp.ToUniversalTime();
        }

        return new LaserScan
        {
            Header = new Header
            {
                Stamp = timestamp,
                FrameId = frameId
            },
            AngleMin = angleMin,
            AngleMax = angleMax,
            AngleIncrement = angleIncrement,
            TimeIncrement = 0.0,
            ScanTime = scanTime,
            RangeMin = rangeMin,
            RangeMax = rangeMax,
            Ranges = ranges,
            Intensities = intensities
        };
    }

    // Optional: Device Information Methods (using SafetyScanner Request...Async methods)
    // These are optional because driver focuses on measurement data via UDP

    /// <summary>
    /// Gets the internal SafetyScanner instance (for advanced usage if needed)
    /// </summary>
    private SafetyScanner? GetScanner()
    {
        lock (_scannerLock)
        {
            return _scanner;
        }
    }

    /// <summary>
    /// Reads device information using SafetyScanner Request...Async methods
    /// Optional method - driver focuses on measurement data via UDP
    /// </summary>
    public async Task RefreshDeviceInfoAsync(CancellationToken cancellationToken = default)
    {
        var scanner = GetScanner();
        if (scanner == null || !scanner.IsConnected)
            throw new InvalidOperationException("Scanner is not connected");

        try
        {
            // Open COLA2 session if needed (for reading device info)
            bool needToOpenSession = !scanner.Session.IsOpen;
            if (needToOpenSession)
            {
                var timeoutDuration = TimeDuration.FromMilliseconds(_connectTimeoutMs);
                scanner.Connect(openCola2Session: true);
            }

            var timeout = TimeDuration.FromMilliseconds(_commandTimeoutMs);

            // Read device info using SafetyScanner Request...
            var serialNumber = scanner.RequestSerialNumber(timeout);
            var firmwareVersion = scanner.RequestFirmwareVersion(timeout);
            var deviceName = scanner.RequestDeviceName(timeout);
            var orderNumber = scanner.RequestOrderNumber(timeout);

            lock (_infoLock)
            {
                _serialNumber = serialNumber.Number;
                _firmwareVersion = firmwareVersion.Version;
                _deviceName = deviceName.Name;
                _orderNumber = orderNumber.Number;
                _lastUpdateTime = DateTime.UtcNow;
            }

            SetProperty("SerialNumber", _serialNumber);
            SetProperty("FirmwareVersion", _firmwareVersion);
            SetProperty("DeviceName", _deviceName);
            SetProperty("OrderNumber", _orderNumber);
            SetProperty("LastUpdateTime", _lastUpdateTime.ToString("yyyy-MM-dd HH:mm:ss"));
        }
        catch (Exception ex)
        {
            LastError = ex;
            OnErrorOccurred(ex, "Failed to refresh device info");
            throw;
        }
    }

    public string? SerialNumber
    {
        get
        {
            lock (_infoLock)
            {
                return _serialNumber;
            }
        }
    }

    public string? FirmwareVersion
    {
        get
        {
            lock (_infoLock)
            {
                return _firmwareVersion;
            }
        }
    }

    public new string? DeviceName
    {
        get
        {
            lock (_infoLock)
            {
                return _deviceName;
            }
        }
    }

    public string? OrderNumber
    {
        get
        {
            lock (_infoLock)
            {
                return _orderNumber;
            }
        }
    }

    public DateTime LastUpdateTime
    {
        get
        {
            lock (_infoLock)
            {
                return _lastUpdateTime;
            }
        }
    }

    protected override void Dispose(bool disposing)
    {
        if (disposing)
        {
            // Unsubscribe from events and dispose scanner
            SafetyScanner? scanner;
            lock (_scannerLock)
            {
                scanner = _scanner;
                _scanner = null;
            }

            if (scanner != null)
            {
                scanner.ScanDataReceived -= OnSafetyScannerScanDataReceived;
                scanner.StopUdpStreaming();
                scanner.Dispose();
            }
        }

        base.Dispose(disposing);
    }
}