BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Sick/SickScanXdLidarDriver.cs

using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;
using ColaB = Sick.Tim781s.Colab;
using ColaBCommands = Sick.Tim781s.Colab.Commands;
using ColaBComm = Sick.Tim781s.Colab.Communication;
using ColaBHelpers = Sick.Tim781s.Colab.Helpers;

namespace RobotNet10.RobotApp.Drivers.Sick;

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

    /// <summary>
    /// TCP port - mặc định: 2112 (SICK scanner default port)
    /// </summary>
    public ushort TcpPort { get; set; } = 2112;

    /// <summary>
    /// Scanner type - ví dụ: "sick_tim_5xx", "sick_mrs_1xxx", "sick_nav_350", etc.
    /// </summary>
    public string ScannerType { get; set; } = "sick_tim_7xxS";

    /// <summary>
    /// Sử dụng binary protocol hay ASCII protocol - mặc định: true (binary)
    /// Có thể là "ColaA", "ColaB", hoặc "Auto" (tự động phát hiện)
    /// </summary>
    public string Protocol { get; set; } = "Auto";
    
    /// <summary>
    /// Sử dụng binary protocol hay ASCII protocol - mặc định: true (binary)
    /// Deprecated: Sử dụng Protocol thay thế
    /// </summary>
    [Obsolete("Use Protocol property instead")]
    public bool UseBinaryProtocol { get; set; } = true;

    /// <summary>
    /// Frame ID cho scan data - mặc định: "lidar_frame"
    /// </summary>
    public string FrameId { get; set; } = "scan";

    /// <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>
    /// Read timeout (milliseconds) - mặc định: 5000
    /// </summary>
    public int ReadTimeoutMs { 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>
    /// Góc bắt đầu quét (degrees) - mặc định: -135.0° (tương đương -2.356 rad)
    /// </summary>
    public double StartAngle { get; set; } = -135.0;

    /// <summary>
    /// Góc kết thúc quét (degrees) - mặc định: 135.0° (tương đương 2.356 rad)
    /// </summary>
    public double EndAngle { get; set; } = 135.0;

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

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

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

        // Scanner type - mặc định: "sick_tim_5xx"
        config.ScannerType = connection["ScannerType"] ?? "sick_tim_5xx";

        // Protocol selection - mặc định: "Auto"
        var protocolStr = connection["Protocol"] ?? connection["UseBinaryProtocol"] ?? "Auto";
        protocolStr = protocolStr.Trim();
        
        if (string.Equals(protocolStr, "ColaA", StringComparison.OrdinalIgnoreCase) ||
            string.Equals(protocolStr, "Cola-A", StringComparison.OrdinalIgnoreCase) ||
            string.Equals(protocolStr, "ASCII", StringComparison.OrdinalIgnoreCase))
        {
            config.Protocol = "ColaA";
            config.UseBinaryProtocol = false;
        }
        else if (string.Equals(protocolStr, "ColaB", StringComparison.OrdinalIgnoreCase) ||
                 string.Equals(protocolStr, "Cola-B", StringComparison.OrdinalIgnoreCase) ||
                 string.Equals(protocolStr, "Binary", StringComparison.OrdinalIgnoreCase))
        {
            config.Protocol = "ColaB";
            config.UseBinaryProtocol = true;
        }
        else if (string.Equals(protocolStr, "Auto", StringComparison.OrdinalIgnoreCase))
        {
            config.Protocol = "Auto";
            // Default to ColaB for Auto mode
            config.UseBinaryProtocol = true;
        }
        else
        {
            // Fallback: try to parse as boolean for backward compatibility
            if (bool.TryParse(protocolStr, out var useBinary))
            {
                config.UseBinaryProtocol = useBinary;
                config.Protocol = useBinary ? "ColaB" : "ColaA";
            }
            else
            {
                config.Protocol = "Auto";
                config.UseBinaryProtocol = true;
            }
        }

        // Frame ID - mặc định: "lidar_frame"
        config.FrameId = connection["FrameId"] ?? "lidar_frame";

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

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

        // Read timeout - mặc định: 5000ms
        var readTimeoutStr = connection["ReadTimeoutMs"] ?? "5000";
        if (!int.TryParse(readTimeoutStr, out var readTimeout) || readTimeout < 1000)
        {
            readTimeout = 5000;
        }
        config.ReadTimeoutMs = readTimeout;

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

        var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
        config.ReconnectDelayMs = reconnectDelayMs ?? 3000;

        var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
        config.MaxReconnectAttempts = maxReconnectAttempts ?? 0;

        // StartAngle và EndAngle (degrees) - mặc định: -135° đến 135°
        var startAngle = connection.GetValue<double?>("StartAngle");
        config.StartAngle = startAngle ?? -135.0;

        var endAngle = connection.GetValue<double?>("EndAngle");
        config.EndAngle = endAngle ?? 135.0;

        return config;
    }
}

/// <summary>
/// Driver cho thiết bị LiDAR SICK Scan XD
/// Sử dụng TCP/IP và SOPAS protocol để giao tiếp với SICK scanners
/// Kế thừa DeviceBase và implement ILidar
/// </summary>
[Device(DeviceType.Lidar, "SICK AG", "SickScanXdLidarDriver", "1.0.0",
    Description = "SICK Scan XD LiDAR Driver - TCP/IP SOPAS Protocol")]
public class SickScanXdLidarDriver : DeviceBase, ILidar
{
    private readonly string _ipAddress;
    private readonly ushort _tcpPort;
    private readonly string _scannerType;
    private readonly bool _useBinaryProtocol; // Config preference
    private readonly string _protocolPreference; // "ColaA", "ColaB", or "Auto"
    private readonly string _frameId;
    private readonly int _connectTimeoutMs;
    private readonly int _commandTimeoutMs;
    private readonly int _readTimeoutMs;

    // Actual protocol being used (may differ from config if auto-detection switches)
    private bool _actualUseBinaryProtocol;
    private readonly Lock _protocolLock = new();

    private System.Net.Sockets.TcpClient? _tcpClient;
    private NetworkStream? _networkStream;
    private readonly Lock _connectionLock = new();

    // ColaB session for binary protocol
    private ColaB.ColaBSession? _colaBSession;
    private ColaBComm.TcpClient? _colaBTcpClient;

    // Scan data
    private LaserScan? _lastScanData;
    private DateTime? _lastScanDataTimestamp;
    private readonly Lock _scanDataLock = new();

    // Device specifications (cached from scanner)
    private double _minAngleRad = -Math.PI;
    private double _maxAngleRad = Math.PI;
    private double _minRangeM = 0.05;
    private double _maxRangeM = 25.0;
    private double? _angularResolutionRad;
    private double? _scanFrequencyHz;
    private bool _supportsIntensity = true;

    // Background task for receiving scan data
    private CancellationTokenSource? _receiveCts;
    private Task? _receiveTask;

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

        // Lưu các giá trị vào private fields
        _ipAddress = config.IpAddress;
        _tcpPort = config.TcpPort;
        _scannerType = config.ScannerType;
        _useBinaryProtocol = config.UseBinaryProtocol;
        _protocolPreference = config.Protocol;
        _frameId = config.FrameId;
        _connectTimeoutMs = config.ConnectTimeoutMs;
        _commandTimeoutMs = config.CommandTimeoutMs;
        _readTimeoutMs = config.ReadTimeoutMs;

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

        // Set scan angle range from config (convert from degrees to radians)
        _minAngleRad = config.StartAngle * Math.PI / 180.0;
        _maxAngleRad = config.EndAngle * Math.PI / 180.0;

        // Initialize actual protocol to config preference (will be auto-detected during connection)
        _actualUseBinaryProtocol = _useBinaryProtocol;

        // Khởi tạo giá trị properties
        SetProperty("IpAddress", _ipAddress);
        SetProperty("TcpPort", _tcpPort.ToString());
        SetProperty("ScannerType", _scannerType);
        SetProperty("Protocol", _protocolPreference);
        SetProperty("UseBinaryProtocol", _useBinaryProtocol.ToString());
        SetProperty("ActualProtocol", _useBinaryProtocol ? "ColaB" : "ColaA");
        SetProperty("FrameId", _frameId);
        SetProperty("StartAngle", config.StartAngle.ToString("F1"));
        SetProperty("EndAngle", config.EndAngle.ToString("F1"));
        SetProperty("ConnectionStatus", "Disconnected");
        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")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 2,
            Category = "Kết nối",
            DefaultValue = "2112"
        };

        yield return new PropertyDescription("ScannerType", "Scanner Type", "Loại scanner SICK")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 3,
            Category = "Cấu hình",
            DefaultValue = "sick_tim_5xx"
        };

        yield return new PropertyDescription("Protocol", "Protocol", "Protocol sử dụng: ColaA, ColaB, hoặc Auto (tự động phát hiện)")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 4,
            Category = "Cấu hình",
            DefaultValue = "Auto"
        };

        yield return new PropertyDescription("UseBinaryProtocol", "Use Binary Protocol", "Sử dụng binary protocol hay ASCII (cấu hình - deprecated)")
        {
            DataType = "boolean",
            IsReadOnly = true,
            DisplayOrder = 5,
            Category = "Cấu hình",
            DefaultValue = "true"
        };

        yield return new PropertyDescription("ActualProtocol", "Actual Protocol", "Protocol thực tế đang sử dụng (ColaA hoặc ColaB)")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 6,
            Category = "Trạng thái",
            DefaultValue = "ColaB"
        };

        yield return new PropertyDescription("FrameId", "Frame ID", "Frame ID cho scan data")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 5,
            Category = "Cấu hình",
            DefaultValue = "lidar_frame"
        };

        yield return new PropertyDescription("StartAngle", "Start Angle", "Góc bắt đầu quét (degrees)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 7,
            Category = "Cấu hình",
            DefaultValue = "-135.0"
        };

        yield return new PropertyDescription("EndAngle", "End Angle", "Góc kết thúc quét (degrees)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 8,
            Category = "Cấu hình",
            DefaultValue = "135.0"
        };

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

        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 = 10,
            Category = "Trạng thái",
            DefaultValue = ""
        };
    }

    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        // Không cần khởi tạo gì đặc biệt ở đây
        await Task.CompletedTask;
    }

    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
    {
        lock (_connectionLock)
        {
            if (_tcpClient != null && _tcpClient.Connected)
            {
                return; // Already connected
            }

            // Disconnect existing connection if any
            DisconnectInternal();
        }

        // Try to connect and auto-detect protocol
        bool connected = false;
        Exception? lastException = null;

        // Determine which protocol to try first
        bool tryColaBFirst = _protocolPreference == "Auto" ? _useBinaryProtocol : 
                            (_protocolPreference == "ColaB");

        // First, try with configured protocol preference
        try
        {
            if (tryColaBFirst)
            {
                await ConnectColaBAsync(cancellationToken);
            }
            else
            {
                await ConnectAsciiAsync(cancellationToken);
            }

            // Try to detect which protocol the scanner actually supports
            var detectedProtocol = await DetectProtocolAsync(cancellationToken);
            if (detectedProtocol.HasValue)
            {
                lock (_protocolLock)
                {
                    _actualUseBinaryProtocol = detectedProtocol.Value;
                }
                SetProperty("ActualProtocol", _actualUseBinaryProtocol ? "ColaB" : "ColaA");

                // If detected protocol differs from what we connected with, reconnect
                if (_actualUseBinaryProtocol != _useBinaryProtocol)
                {
                    DisconnectInternal();
                    
                    if (_actualUseBinaryProtocol)
                    {
                        await ConnectColaBAsync(cancellationToken);
                    }
                    else
                    {
                        await ConnectAsciiAsync(cancellationToken);
                    }
                }
            }

            connected = true;
        }
        catch (Exception ex)
        {
            lastException = ex;
        }

        // If connection failed and protocol is Auto, try the other protocol as fallback
        if (!connected && _protocolPreference == "Auto")
        {
            try
            {
                DisconnectInternal();
                
                if (!tryColaBFirst)
                {
                    await ConnectColaBAsync(cancellationToken);
                }
                else
                {
                    await ConnectAsciiAsync(cancellationToken);
                }

                // Try to detect protocol again
                var detectedProtocol = await DetectProtocolAsync(cancellationToken);
                if (detectedProtocol.HasValue)
                {
                    lock (_protocolLock)
                    {
                        _actualUseBinaryProtocol = detectedProtocol.Value;
                    }
                    SetProperty("ActualProtocol", _actualUseBinaryProtocol ? "ColaB" : "ColaA");
                }
                else
                {
                    // Use the protocol we connected with
                    lock (_protocolLock)
                    {
                        _actualUseBinaryProtocol = !_useBinaryProtocol;
                    }
                    SetProperty("ActualProtocol", _actualUseBinaryProtocol ? "ColaB" : "ColaA");
                }

                connected = true;
            }
            catch (Exception ex2)
            {
                throw new InvalidOperationException($"Failed to connect to SICK scanner at {_ipAddress}:{_tcpPort} with both ColaA and ColaB protocols. Last error: {ex2.Message}", ex2);
            }
        }

        // Initialize scanner (send SOPAS commands)
        // Don't fail if initialization has issues - some scanners may work without full initialization
        try
        {
            await InitializeScannerAsync(cancellationToken);
        }
        catch (Exception ex)
        {
            // Log error but continue - scanner may still work
            OnErrorOccurred(ex, "Scanner initialization had issues but continuing");
        }

        // Start receiving scan data in background
        StartReceiveTask();

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

    /// <summary>
    /// Connect using ColaB (binary) protocol
    /// </summary>
    private async Task ConnectColaBAsync(CancellationToken cancellationToken)
    {
        try
        {
            _colaBTcpClient = new ColaBComm.TcpClient(_ipAddress, _tcpPort);
            _colaBSession = new ColaB.ColaBSession(_colaBTcpClient);
            
            // Open session (connects to scanner)
            _colaBSession.Open(_connectTimeoutMs);
        }
        catch (Exception ex)
        {
            _colaBSession?.Dispose();
            _colaBTcpClient?.Dispose();
            _colaBSession = null;
            _colaBTcpClient = null;
            throw new InvalidOperationException($"Failed to connect to SICK scanner at {_ipAddress}:{_tcpPort} using ColaB: {ex.Message}", ex);
        }
    }

    /// <summary>
    /// Connect using ASCII protocol
    /// </summary>
    private async Task ConnectAsciiAsync(CancellationToken cancellationToken)
    {
        // Create new TCP connection
        var tcpClient = new System.Net.Sockets.TcpClient();
        try
        {
            // Connect with timeout
            var connectTask = tcpClient.ConnectAsync(_ipAddress, _tcpPort);
            var timeoutTask = Task.Delay(_connectTimeoutMs, cancellationToken);
            var completedTask = await Task.WhenAny(connectTask, timeoutTask);

            if (completedTask == timeoutTask)
            {
                tcpClient.Close();
                throw new TimeoutException($"Connection timeout after {_connectTimeoutMs}ms");
            }

            await connectTask; // Ensure connection completed successfully

            _networkStream = tcpClient.GetStream();
            _networkStream.ReadTimeout = _readTimeoutMs;
            _networkStream.WriteTimeout = _commandTimeoutMs;

            lock (_connectionLock)
            {
                _tcpClient = tcpClient;
            }
        }
        catch (Exception ex)
        {
            tcpClient?.Close();
            throw new InvalidOperationException($"Failed to connect to SICK scanner at {_ipAddress}:{_tcpPort}: {ex.Message}", ex);
        }
    }

    private void DisconnectInternal()
    {
        // Stop receive task
        _receiveCts?.Cancel();
        _receiveTask?.Wait(1000); // Wait max 1 second
        _receiveCts?.Dispose();
        _receiveCts = null;
        _receiveTask = null;

        // Close ColaB session if using binary protocol
        lock (_protocolLock)
        {
            if (_actualUseBinaryProtocol)
            {
                _colaBSession?.Dispose();
                _colaBTcpClient?.Dispose();
                _colaBSession = null;
                _colaBTcpClient = null;
            }
            else
            {
                // Close network stream
                _networkStream?.Close();
                _networkStream?.Dispose();
                _networkStream = null;

                // Close TCP client
                _tcpClient?.Close();
                _tcpClient?.Dispose();
                _tcpClient = null;
            }
        }
    }

    /// <summary>
    /// Detect which protocol (ColaA or ColaB) the scanner supports
    /// Returns true for ColaB, false for ColaA, null if detection failed
    /// </summary>
    private async Task<bool?> DetectProtocolAsync(CancellationToken cancellationToken)
    {
        try
        {
            // Try to send a simple command (sRN DeviceState) with current connection
            // If we're connected via ColaB, try ColaB first
            if (_colaBSession != null && _colaBSession.IsOpen)
            {
                try
                {
                    var readCmd = new ColaBCommands.ReadCommand("DeviceState");
                    _colaBSession.ExecuteCommand(readCmd, _commandTimeoutMs);
                    if (readCmd.WasSuccessful)
                    {
                        return true; // ColaB works
                    }
                }
                catch (Exception)
                {
                }
            }

            // If we're connected via ASCII, try ASCII
            if (_tcpClient != null && _tcpClient.Connected && _networkStream != null)
            {
                try
                {
                    var reply = await SendSopasCommandAsciiAsync("sRN DeviceState", cancellationToken);
                    if (!string.IsNullOrEmpty(reply) && !reply.Contains("sFA"))
                    {
                        return false; // ColaA works
                    }
                }
                catch (Exception)
                {
                }
            }
        }
        catch (Exception)
        {
        }

        return null; // Detection failed
    }

    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        lock (_connectionLock)
        {
            DisconnectInternal();
        }

        SetProperty("ConnectionStatus", "Disconnected");
        await Task.CompletedTask;
    }

    protected override async Task OnResetAsync(CancellationToken cancellationToken)
    {
        // Clear cached scan data
        lock (_scanDataLock)
        {
            _lastScanData = null;
            _lastScanDataTimestamp = null;
        }

        SetProperty("LastScanDataTimestamp", "");
        await Task.CompletedTask;
    }

    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
    {
        // Read protocol outside of lock to avoid await in lock
        bool useBinary;
        lock (_protocolLock)
        {
            useBinary = _actualUseBinaryProtocol;
        }

        if (useBinary)
        {
            // Check ColaB session - just verify TCP connection is still open
            // Don't send command here as it may fail if scanner is still initializing
            // The actual connection verification happens during OnConnectAsync
            try
            {
                if (_colaBSession == null || !_colaBSession.IsOpen)
                {
                    return false;
                }
                
                // Session is open, connection is valid
                return true;
            }
            catch
            {
                return false;
            }
        }
        else
        {
            lock (_connectionLock)
            {
                if (_tcpClient == null || !_tcpClient.Connected)
                {
                    return false;
                }
            }

            // Try to send a simple SOPAS command to check connection
            try
            {
                var reply = await SendSopasCommandAsync("sRN DeviceIdent", cancellationToken);
                return !string.IsNullOrEmpty(reply);
            }
            catch
            {
                return false;
            }
        }
    }

    #region SOPAS Protocol

    /// <summary>
    /// Initialize scanner by sending SOPAS commands
    /// </summary>
    private async Task InitializeScannerAsync(CancellationToken cancellationToken)
    {
        bool useBinary;
        lock (_protocolLock)
        {
            useBinary = _actualUseBinaryProtocol;
        }

        if (useBinary)
        {
            // Use ColaB protocol for binary communication
            await InitializeScannerColaBAsync(cancellationToken);
        }
        else
        {
            // Use ASCII protocol
            await InitializeScannerAsciiAsync(cancellationToken);
        }
    }

    /// <summary>
    /// Initialize scanner using ColaB (binary) protocol
    /// </summary>
    private async Task InitializeScannerColaBAsync(CancellationToken cancellationToken)
    {
        if (_colaBSession == null)
            throw new InvalidOperationException("ColaB session not initialized");

        // Set access mode first (authentication) - required for most SICK scanners
        // Level 3 with password hash "F4724744" (default for most scanners)
        // For TiM7xxS safety scanner, use "6FD62C05"
        try
        {
            string passwordHash = "F4724744"; // Default password hash
            if (_scannerType.Contains("7xxS", StringComparison.OrdinalIgnoreCase) || 
                _scannerType.Contains("safety", StringComparison.OrdinalIgnoreCase))
            {
                passwordHash = "6FD62C05"; // Safety scanner password hash
            }

            var methodCmd = new ColaBCommands.MethodCommand("SetAccessMode", "3", passwordHash);
            _colaBSession.ExecuteCommand(methodCmd, _commandTimeoutMs);
            if (!methodCmd.WasSuccessful)
            {
                OnErrorOccurred(new InvalidOperationException("Failed to set access mode"), 
                    "Failed to authenticate with scanner via ColaB");
                // Continue anyway - some scanners may not require authentication
            }
        }
        catch (Exception ex)
        {
            OnErrorOccurred(ex, "Error setting access mode via ColaB (may not be required)");
            // Continue anyway - some scanners may not require authentication
        }

        // Request device identification
        try
        {
            var readCmd = new ColaBCommands.ReadCommand("DeviceIdent");
            _colaBSession.ExecuteCommand(readCmd, _commandTimeoutMs);
            if (readCmd.WasSuccessful && !string.IsNullOrEmpty(readCmd.ReplyValue))
            {
                // Parse device info if needed
            }
            else
            {
                // Don't throw - some scanners may not respond to DeviceIdent immediately
                OnErrorOccurred(new InvalidOperationException("DeviceIdent command failed"), 
                    "Failed to read device identification via ColaB (may be normal)");
            }
        }
        catch (Exception ex)
        {
            // Don't throw - scanner may still work without DeviceIdent
            OnErrorOccurred(ex, "Failed to read device identification via ColaB (may be normal)");
        }

        // Start scan data transmission
        // For TiM781S, we try to enable scan data event
        // Note: Scanner may already be in measurement mode, so we skip LMCstartmeas if it fails
        try
        {
            // Check scanner state
            try
            {
                await SendSopasCommandAsync("sRN SCdevicestate", cancellationToken);
            }
            catch
            {
            }
            
            // Check scan data configuration
            try
            {
                var currentConfig = await SendSopasCommandAsync("sRN LMDscandatacfg", cancellationToken);
                Console.WriteLine($"[SICK-RSSI-CONFIG] Current LMDscandatacfg: {currentConfig}");
            }
            catch (Exception ex)
            {
                Console.WriteLine($"[SICK-RSSI-CONFIG] Failed to read LMDscandatacfg: {ex.Message}");
            }
            
            // Configure scan data output to include RSSI/remission values
            // Format: sWN LMDscandatacfg <output_channel> <remission> <resolution> <unit> <encoder>
            // Parameters:
            //   output_channel: 01 (standard output channel)
            //   remission: 01 (enable RSSI/remission data) - THIS IS KEY FOR INTENSITIES!
            //   resolution: 01 (8-bit resolution)
            //   unit: 00 (distance unit)
            //   encoder: 00 (no encoder)
            try
            {
                Console.WriteLine("[SICK-RSSI-CONFIG] Configuring LMDscandatacfg to enable RSSI/remission output...");
                var configResult = await SendSopasCommandAsync("sWN LMDscandatacfg 01 01 01 00 00", cancellationToken);
                Console.WriteLine($"[SICK-RSSI-CONFIG] LMDscandatacfg set successfully: {configResult}");
                _supportsIntensity = true;
                
                // Verify configuration was applied
                await Task.Delay(100, cancellationToken); // Give scanner time to apply settings
                var verifyConfig = await SendSopasCommandAsync("sRN LMDscandatacfg", cancellationToken);
                Console.WriteLine($"[SICK-RSSI-CONFIG] Verified LMDscandatacfg: {verifyConfig}");
            }
            catch (Exception ex)
            {
                Console.WriteLine($"[SICK-RSSI-CONFIG] ERROR: Failed to set LMDscandatacfg: {ex.Message}");
                OnErrorOccurred(ex, "Failed to enable RSSI output in LIDAR configuration");
                _supportsIntensity = false;
            }
            
            // Try to start measurement (may fail if already started - that's OK)
            try
            {
                await SendSopasCommandAsync("sMN LMCstartmeas", cancellationToken);
            }
            catch
            {
                // Continue - scanner may already be in measurement mode
            }
            
            // Apply settings (Run) - this is usually safe
            try
            {
                await SendSopasCommandAsync("sMN Run", cancellationToken);
            }
            catch
            {
                // Continue anyway
            }
            
            // Try to enable scan data event
            // Note: Some scanners may send scan data automatically without this command
            try
            {
                await SendSopasCommandAsync("sEN LMDscandata 1", cancellationToken);
            }
            catch
            {
                // Continue anyway - scanner may send scan data automatically
            }
            
            // Try to read scan data directly (polling mode) as fallback
            try
            {
                // Use ColaB session directly to get raw binary data
                if (_colaBSession != null)
                {
                    var readCmd = new ColaBCommands.ReadCommand("LMDscandata");
                    _colaBSession.ExecuteCommand(readCmd, _commandTimeoutMs);
                    
                    if (readCmd.WasSuccessful && readCmd.RawReplyData != null)
                    {
                        // Try to parse binary format
                        if (TryParseScanDataFromColaBBinary(readCmd.RawReplyData, out var parsedScanBinary))
                        {
                            UpdateScanData(parsedScanBinary);
                        }
                        else
                        {
                            // Fallback to ASCII parser
                            var replyString = System.Text.Encoding.ASCII.GetString(readCmd.RawReplyData);
                            var parsedScan = ParseLmdScandata(replyString);
                            if (parsedScan.HasValue && parsedScan.Value.Ranges != null)
                            {
                                UpdateScanData(parsedScan.Value);
                            }
                        }
                    }
                }
            }
            catch
            {
            }
        }
        catch (Exception ex)
        {
            // Continue anyway - scanner may still send scan data
            OnErrorOccurred(ex, "Some initialization commands failed, but continuing (scanner may send scan data automatically)");
        }
    }

    /// <summary>
    /// Initialize scanner using ASCII protocol
    /// </summary>
    private async Task InitializeScannerAsciiAsync(CancellationToken cancellationToken)
    {
        // Request device identification
        var deviceIdent = await SendSopasCommandAsync("sRN DeviceIdent", cancellationToken);
        if (!string.IsNullOrEmpty(deviceIdent))
        {
            // Parse device info if needed
        }

        // Start scan data transmission
        // Command depends on scanner type, but "sEN LMDscandata 1" is common
        try
        {
            await SendSopasCommandAsync("sEN LMDscandata 1", cancellationToken);
        }
        catch
        {
            // Some scanners may not support this command, continue anyway
        }
    }

    /// <summary>
    /// Send SOPAS command and wait for reply
    /// </summary>
    private async Task<string> SendSopasCommandAsync(string command, CancellationToken cancellationToken)
    {
        bool useBinary;
        lock (_protocolLock)
        {
            useBinary = _actualUseBinaryProtocol;
        }

        if (useBinary)
        {
            // Use ColaB protocol
            return await SendSopasCommandColaBAsync(command, cancellationToken);
        }
        else
        {
            // Use ASCII protocol
            return await SendSopasCommandAsciiAsync(command, cancellationToken);
        }
    }

    /// <summary>
    /// Send SOPAS command using ColaB (binary) protocol
    /// </summary>
    private async Task<string> SendSopasCommandColaBAsync(string command, CancellationToken cancellationToken)
    {
        if (_colaBSession == null)
            throw new InvalidOperationException("ColaB session not initialized");

        try
        {
            // Determine command type and create appropriate command
            ColaBCommands.IColaBCommand colaBCommand;
            if (command.StartsWith("sRN ", StringComparison.Ordinal))
            {
                // Read command
                var variableName = command.Substring(4).Trim();
                colaBCommand = new ColaBCommands.ReadCommand(variableName);
            }
            else if (command.StartsWith("sWN ", StringComparison.Ordinal))
            {
                // Write command
                var parts = command.Substring(4).Split(' ', 2);
                if (parts.Length == 2)
                {
                    colaBCommand = new ColaBCommands.WriteCommand(parts[0], parts[1]);
                }
                else
                {
                    throw new ArgumentException($"Invalid write command format: {command}");
                }
            }
            else if (command.StartsWith("sMN ", StringComparison.Ordinal))
            {
                // Method command
                var parts = command.Substring(4).Split(' ', StringSplitOptions.RemoveEmptyEntries);
                if (parts.Length > 0)
                {
                    var methodName = parts[0];
                    var parameters = parts.Skip(1).ToArray();
                    colaBCommand = new ColaBCommands.MethodCommand(methodName, parameters);
                }
                else
                {
                    throw new ArgumentException($"Invalid method command format: {command}");
                }
            }
            else if (command.StartsWith("sEN ", StringComparison.Ordinal))
            {
                // Event command (Set Event Name) - similar to write but for events
                // Format: "sEN <eventName> <value>"
                var parts = command.Substring(4).Split(' ', 2);
                if (parts.Length == 2)
                {
                    // Use WriteCommand for sEN as it's similar in structure
                    colaBCommand = new ColaBCommands.WriteCommand(parts[0], parts[1]);
                }
                else
                {
                    throw new ArgumentException($"Invalid event command format: {command}");
                }
            }
            else
            {
                throw new ArgumentException($"Unsupported command format: {command}");
            }

            // Execute command
            _colaBSession.ExecuteCommand(colaBCommand, _commandTimeoutMs);

            if (!colaBCommand.WasSuccessful)
            {
                return string.Empty;
            }

            // Get reply value
            if (colaBCommand is ColaBCommands.ReadCommand readCmd)
            {
                var reply = readCmd.ReplyValue ?? string.Empty;
                return reply;
            }
            else if (colaBCommand is ColaBCommands.MethodCommand methodCmd)
            {
                var reply = methodCmd.ReplyValue ?? string.Empty;
                return reply;
            }
            else if (colaBCommand is ColaBCommands.WriteCommand writeCmd)
            {
                // WriteCommand doesn't have ReplyValue, but WasSuccessful indicates success
                return "OK"; // Return OK to indicate success
            }
            else
            {
                return string.Empty;
            }
        }
        catch (Exception ex)
        {
            throw new InvalidOperationException($"Failed to send ColaB command: {command}", ex);
        }
    }

    /// <summary>
    /// Send SOPAS command using ASCII protocol
    /// </summary>
    private async Task<string> SendSopasCommandAsciiAsync(string command, CancellationToken cancellationToken)
    {
        NetworkStream? stream;
        lock (_connectionLock)
        {
            stream = _networkStream;
            if (stream == null || _tcpClient == null || !_tcpClient.Connected)
            {
                throw new InvalidOperationException("Not connected to scanner");
            }
        }

        // Format SOPAS command (ASCII mode)
        // STX + command + ETX
        var commandBytes = Encoding.ASCII.GetBytes(command);
        var message = new List<byte> { 0x02 }; // STX
        message.AddRange(commandBytes);
        message.Add(0x03); // ETX

        // Send command
        await stream.WriteAsync(message.ToArray(), 0, message.Count, cancellationToken);
        await stream.FlushAsync(cancellationToken);

        // Read reply
        var buffer = new byte[4096];
        var totalBytes = 0;
        var startTime = DateTime.UtcNow;

        while (DateTime.UtcNow - startTime < TimeSpan.FromMilliseconds(_commandTimeoutMs))
        {
            if (stream.DataAvailable)
            {
                var bytesRead = await stream.ReadAsync(buffer, totalBytes, buffer.Length - totalBytes, cancellationToken);
                if (bytesRead == 0)
                    break;

                totalBytes += bytesRead;

                // Check if we have complete message (ends with ETX)
                if (totalBytes > 0 && buffer[totalBytes - 1] == 0x03)
                {
                    break;
                }
            }
            else
            {
                await Task.Delay(10, cancellationToken);
            }
        }

        if (totalBytes == 0)
        {
            throw new TimeoutException("No reply from scanner");
        }

        // Parse reply (skip STX, remove ETX)
        var replyStart = 0;
        if (buffer[0] == 0x02) replyStart = 1; // Skip STX

        var replyEnd = totalBytes;
        if (buffer[totalBytes - 1] == 0x03) replyEnd = totalBytes - 1; // Remove ETX

        var replyLength = replyEnd - replyStart;
        if (replyLength <= 0)
        {
            return string.Empty;
        }

        return Encoding.ASCII.GetString(buffer, replyStart, replyLength);
    }

    #endregion

    #region Scan Data Reception

    /// <summary>
    /// Start background task to receive scan data
    /// </summary>
    private void StartReceiveTask()
    {
        _receiveCts?.Cancel();
        _receiveCts?.Dispose();

        _receiveCts = new CancellationTokenSource();
        _receiveTask = Task.Run(async () => 
        {
            try
            {
                await ReceiveScanDataLoopAsync(_receiveCts.Token);
            }
            catch (Exception ex)
            {
                OnErrorOccurred(ex, "ReceiveScanDataLoopAsync exception");
            }
        }, _receiveCts.Token);
    }

    /// <summary>
    /// Main loop to receive and parse scan data
    /// </summary>
    private async Task ReceiveScanDataLoopAsync(CancellationToken cancellationToken)
    {
        bool useBinary;
        lock (_protocolLock)
        {
            useBinary = _actualUseBinaryProtocol;
        }

        if (useBinary)
        {
            await ReceiveScanDataLoopColaBAsync(cancellationToken);
        }
        else
        {
            await ReceiveScanDataLoopAsciiAsync(cancellationToken);
        }
    }

    /// <summary>
    /// Receive scan data loop for ASCII protocol
    /// </summary>
    private async Task ReceiveScanDataLoopAsciiAsync(CancellationToken cancellationToken)
    {
        var buffer = new byte[480000]; // Large buffer for scan data
        var partialBuffer = new List<byte>();

        while (!cancellationToken.IsCancellationRequested)
        {
            try
            {
                NetworkStream? stream;
                bool shouldDelay = false;
                lock (_connectionLock)
                {
                    stream = _networkStream;
                    if (stream == null || _tcpClient == null || !_tcpClient.Connected)
                    {
                        shouldDelay = true;
                    }
                }
                
                if (shouldDelay)
                {
                    await Task.Delay(1000, cancellationToken);
                    continue;
                }

                if (stream == null) continue;

                // Read available data
                if (stream.DataAvailable)
                {
                    var bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length, cancellationToken);
                    if (bytesRead > 0)
                    {
                        partialBuffer.AddRange(buffer.Take(bytesRead));

                        // Try to parse scan data from buffer
                        var parsed = TryParseScanData(partialBuffer, out var scanData, out var consumedBytes);
                        if (parsed)
                        {
                            // Remove consumed bytes
                            partialBuffer.RemoveRange(0, consumedBytes);

                            // Update scan data
                            UpdateScanData(scanData);
                        }
                        else if (partialBuffer.Count > 100000)
                        {
                            // Buffer too large, clear it
                            partialBuffer.Clear();
                        }
                    }
                }
                else
                {
                    await Task.Delay(10, cancellationToken);
                }
            }
            catch (Exception ex)
            {
                LastError = ex;
                OnErrorOccurred(ex, "Error receiving scan data (ASCII)");
                await Task.Delay(1000, cancellationToken);
            }
        }
    }

    /// <summary>
    /// Receive scan data loop for ColaB (binary) protocol
    /// Uses polling mode since scanner doesn't send data automatically
    /// </summary>
    private async Task ReceiveScanDataLoopColaBAsync(CancellationToken cancellationToken)
    {
        var partialBuffer = new List<byte>();
        int loopCount = 0;
        DateTime lastPollTime = DateTime.MinValue;
        const int PollIntervalMs = 100; // Poll every 100ms (10 Hz)

        while (!cancellationToken.IsCancellationRequested)
        {
            loopCount++;
            
            try
            {
                ColaBComm.TcpClient? tcpClient;
                ColaB.ColaBSession? session;
                bool shouldDelay = false;
                lock (_connectionLock)
                {
                    tcpClient = _colaBTcpClient;
                    session = _colaBSession;
                    if (tcpClient == null || !tcpClient.IsConnected || session == null || !session.IsOpen)
                    {
                        shouldDelay = true;
                    }
                }

                if (shouldDelay)
                {
                    await Task.Delay(1000, cancellationToken);
                    continue;
                }

                if (tcpClient == null || session == null) 
                {
                    await Task.Delay(100, cancellationToken);
                    continue;
                }

                // Poll for scan data periodically (since scanner doesn't send automatically)
                var timeSinceLastPoll = (DateTime.UtcNow - lastPollTime).TotalMilliseconds;
                if (timeSinceLastPoll >= PollIntervalMs)
                {
                    try
                    {
                        // Poll for scan data using sRN LMDscandata command
                        var readCmd = new ColaBCommands.ReadCommand("LMDscandata");
                        session.ExecuteCommand(readCmd, _commandTimeoutMs);
                        
                        if (readCmd.WasSuccessful && readCmd.RawReplyData != null && readCmd.RawReplyData.Length > 0)
                        {
                            // Try to parse binary format
                            if (TryParseScanDataFromColaBBinary(readCmd.RawReplyData, out var parsedScanBinary))
                            {
                                UpdateScanData(parsedScanBinary);
                            }
                            else
                            {
                                // Fallback to ASCII parser
                                var replyString = System.Text.Encoding.ASCII.GetString(readCmd.RawReplyData);
                                var parsedScan = ParseLmdScandata(replyString);
                                if (parsedScan.HasValue && parsedScan.Value.Ranges != null)
                                {
                                    UpdateScanData(parsedScan.Value);
                                }
                            }
                            
                            lastPollTime = DateTime.UtcNow;
                        }
                    }
                    catch (Exception)
                    {
                    }
                }

                // Also try to receive any spontaneous data (in case scanner starts sending automatically)
                try
                {
                    var frame = tcpClient.Receive(50); // Very short timeout for non-blocking
                    
                    // Try to parse ColaB frame
                    if (ColaBHelpers.ColaBHelper.TryParseFrame(frame, out var commandData, out var consumedBytes))
                    {
                        // Decode command to check type
                        var commandString = ColaBHelpers.ColaBHelper.DecodeCommand(commandData);
                        
                        // Check if this is scan data (LMDscandata)
                        if (commandString.Contains("LMDscandata", StringComparison.Ordinal))
                        {
                            // Try to parse binary format first
                            var parsed = TryParseScanDataFromColaBBinary(commandData, out var scanData);
                            if (!parsed)
                            {
                                // Fallback to ASCII parser
                                parsed = TryParseScanDataFromColaB(commandData, out scanData);
                            }
                            
                            if (parsed && scanData.Ranges != null && scanData.Ranges.Length > 0)
                            {
                                UpdateScanData(scanData);
                            }
                        }
                    }
                }
                catch (TimeoutException)
                {
                    // No spontaneous data available - this is normal, we're using polling
                }
                catch (Exception ex)
                {
                    OnErrorOccurred(ex, "Error receiving scan data (ColaB)");
                    await Task.Delay(1000, cancellationToken);
                }
            }
            catch (Exception ex)
            {
                LastError = ex;
                OnErrorOccurred(ex, "Error receiving scan data (ColaB)");
                await Task.Delay(1000, cancellationToken);
            }
        }
    }

    /// <summary>
    /// Try to parse scan data from ColaB binary command data
    /// This parses the binary format directly
    /// </summary>
    private bool TryParseScanDataFromColaBBinary(ReadOnlySpan<byte> commandData, out LaserScan scanData)
    {
        scanData = default(LaserScan);
        try
        {
            // Binary LMDscandata format:
            // Starts with ASCII header: "sRA LMDscandata " or "sSN LMDscandata "
            // Then binary data follows:
            // - Various fields (version, device number, etc.)
            // - "DIST1" (5 bytes ASCII) followed by:
            //   - Number of items (2 bytes, big endian uint16)
            //   - Distance values (each 2 bytes, big endian uint16, units: mm)
            // - "RSSI1" (5 bytes ASCII) followed by:
            //   - Number of items (2 bytes, big endian uint16)
            //   - RSSI values (each 1 or 2 bytes depending on resolution)
            
            if (commandData.Length < 50)
            {
                return false;
            }
            
            // Check if it contains "LMDscandata"
            var commandStart = Encoding.ASCII.GetString(commandData.Slice(0, Math.Min(50, commandData.Length)));
            if (!commandStart.Contains("LMDscandata", StringComparison.Ordinal))
            {
                return false;
            }
            
            // Parse header fields before DIST1
            // According to SICK documentation, binary LMDscandata format has:
            // - Version, device number, serial number, status, etc. (variable length)
            // - Scanning frequency (field 16)
            // - Measurement frequency (field 17)
            // - Number of 16-bit channels (field 19)
            // - Measured data contents (field 20, e.g., "DIST1")
            // - Scaling factor (field 21, 4 bytes float)
            // - Scaling offset (field 22, 4 bytes float, usually 0)
            // - Starting angle (field 23, 4 bytes signed int, units: 1/10000 degree)
            // - Angular step width (field 24, 2 bytes unsigned short, units: 1/10000 degree)
            // - Number of data (field 25, 2 bytes unsigned short)
            
            float startAngleDeg = 0.0f;
            float angularStepDeg = 0.0f;
            float scaleFactor = 1.0f;
            float scaleOffset = 0.0f;
            
            // Find "DIST1" in the data
            var dist1Pattern = Encoding.ASCII.GetBytes("DIST1");
            var dist1Index = -1;
            for (int i = 0; i <= commandData.Length - dist1Pattern.Length; i++)
            {
                bool found = true;
                for (int j = 0; j < dist1Pattern.Length; j++)
                {
                    if (commandData[i + j] != dist1Pattern[j])
                    {
                        found = false;
                        break;
                    }
                }
                if (found)
                {
                    dist1Index = i;
                    break;
                }
            }
            
            if (dist1Index == -1)
            {
                return false;
            }
            
            // Try to parse header fields before DIST1
            // Look for scaling factor, starting angle, and angular step before DIST1
            // These are typically in the 20-30 bytes before DIST1
            int headerSearchStart = Math.Max(0, dist1Index - 40);
            for (int i = headerSearchStart; i < dist1Index - 10; i++)
            {
                // Try to find scaling factor (4 bytes float, big endian)
                if (i + 4 <= dist1Index)
                {
                    // Check if this looks like a float (scaling factor is usually 1.0 = 0x3F800000)
                    uint scaleFactorInt = (uint)((commandData[i] << 24) | (commandData[i + 1] << 16) | (commandData[i + 2] << 8) | commandData[i + 3]);
                    float testScale = BitConverter.ToSingle(BitConverter.GetBytes(scaleFactorInt), 0);
                    if (testScale > 0.1f && testScale < 10.0f)
                    {
                        scaleFactor = testScale;
                        
                        // Scaling offset is next (4 bytes, usually 0)
                        if (i + 8 <= dist1Index)
                        {
                            uint scaleOffsetInt = (uint)((commandData[i + 4] << 24) | (commandData[i + 5] << 16) | (commandData[i + 6] << 8) | commandData[i + 7]);
                            scaleOffset = BitConverter.ToSingle(BitConverter.GetBytes(scaleOffsetInt), 0);
                            
                            // Starting angle is next (4 bytes signed int, units: 1/10000 degree)
                            if (i + 12 <= dist1Index)
                            {
                                int startAngleInt = (int)((commandData[i + 8] << 24) | (commandData[i + 9] << 16) | (commandData[i + 10] << 8) | commandData[i + 11]);
                                startAngleDeg = startAngleInt / 10000.0f;
                                
                                // Angular step width is next (2 bytes unsigned short, units: 1/10000 degree)
                                if (i + 14 <= dist1Index)
                                {
                                    ushort angularStepInt = (ushort)((commandData[i + 12] << 8) | commandData[i + 13]);
                                    angularStepDeg = angularStepInt / 10000.0f;
                                    break;
                                }
                            }
                        }
                    }
                }
            }
            
            // According to SICK documentation and C++ reference code:
            // After "DIST1" (5 bytes), there are exactly 19 bytes of header fields:
            // - Scale factor (4 bytes float, big endian)
            // - Scale offset (4 bytes float, big endian)  
            // - Starting angle (4 bytes signed int, big endian, units: 1/10000 degree)
            // - Angular step width (2 bytes unsigned short, big endian, units: 1/10000 degree)
            // - Reserved (5 bytes)
            // Then: Count (2 bytes, big endian) at offset DIST1 + 5 + 19 = DIST1 + 24
            
            // Parse header fields at fixed offsets
            int headerStart = dist1Index + 5; // Right after "DIST1"
            
            // Scale factor (4 bytes float, big endian) at offset headerStart
            if (headerStart + 4 <= commandData.Length)
            {
                uint scaleFactorInt = (uint)((commandData[headerStart] << 24) | (commandData[headerStart + 1] << 16) | 
                                            (commandData[headerStart + 2] << 8) | commandData[headerStart + 3]);
                scaleFactor = BitConverter.ToSingle(BitConverter.GetBytes(scaleFactorInt), 0);
            }
            
            // Scale offset (4 bytes float, big endian) at offset headerStart + 4
            if (headerStart + 8 <= commandData.Length)
            {
                uint scaleOffsetInt = (uint)((commandData[headerStart + 4] << 24) | (commandData[headerStart + 5] << 16) | 
                                            (commandData[headerStart + 6] << 8) | commandData[headerStart + 7]);
                scaleOffset = BitConverter.ToSingle(BitConverter.GetBytes(scaleOffsetInt), 0);
            }
            
            // Starting angle (4 bytes signed int, big endian) at offset headerStart + 8
            if (headerStart + 12 <= commandData.Length)
            {
                int startAngleInt = (int)((commandData[headerStart + 8] << 24) | (commandData[headerStart + 9] << 16) | 
                                          (commandData[headerStart + 10] << 8) | commandData[headerStart + 11]);
                startAngleDeg = startAngleInt / 10000.0f;
            }
            
            // Angular step width (2 bytes unsigned short, big endian) at offset headerStart + 12
            if (headerStart + 14 <= commandData.Length)
            {
                ushort angularStepInt = (ushort)((commandData[headerStart + 12] << 8) | commandData[headerStart + 13]);
                angularStepDeg = angularStepInt / 10000.0f;
            }
            
            // Count (2 bytes, big endian) 
            // From hex data analysis: after DIST1 (5) + scale factor (4) + scale offset (4) + starting angle (4) + angular step (2) = 19 bytes
            // But actual data shows count at offset 75 from start, which is DIST1 (56) + 5 + 14 = 75
            // So it seems: DIST1 (5) + scale factor (4) + scale offset (4) + starting angle (4) + angular step (2) = 19, but count is at +14, not +19
            // Let's try both offsets and use the one that makes sense
            
            ushort distCount = 0;
            int distCountOffset = -1;
            bool countFound = false;
            
            // Try offset +14 first (matches actual data)
            int tryOffset1 = headerStart + 14; // DIST1 + 5 + 14
            if (tryOffset1 + 2 <= commandData.Length)
            {
                ushort testCount = (ushort)((commandData[tryOffset1] << 8) | commandData[tryOffset1 + 1]);
                if (testCount >= 50 && testCount <= 5000)
                {
                    int requiredBytes = tryOffset1 + 2 + (testCount * 2);
                    if (requiredBytes <= commandData.Length)
                    {
                        distCount = testCount;
                        distCountOffset = tryOffset1;
                        countFound = true;
                    }
                }
            }
            
            // Try offset +19 (per C++ reference code)
            if (!countFound)
            {
                int tryOffset2 = headerStart + 19; // DIST1 + 5 + 19
                if (tryOffset2 + 2 <= commandData.Length)
                {
                    ushort testCount = (ushort)((commandData[tryOffset2] << 8) | commandData[tryOffset2 + 1]);
                    if (testCount >= 50 && testCount <= 5000)
                    {
                        int requiredBytes = tryOffset2 + 2 + (testCount * 2);
                        if (requiredBytes <= commandData.Length)
                        {
                            distCount = testCount;
                            distCountOffset = tryOffset2;
                            countFound = true;
                        }
                    }
                }
            }
            
            // If fixed offset doesn't work, try searching (fallback)
            if (!countFound)
            {
                int searchStart = dist1Index + 5;
                int searchEnd = Math.Min(dist1Index + 5 + 30, commandData.Length - 2);
                
                for (int offset = searchStart; offset <= searchEnd && !countFound; offset++)
                {
                    // Try big endian
                    if (offset + 2 <= commandData.Length)
                    {
                        ushort testCount = (ushort)((commandData[offset] << 8) | commandData[offset + 1]);
                        
                        if (testCount >= 50 && testCount <= 5000)
                        {
                            int requiredBytes = offset + 2 + (testCount * 2);
                            if (requiredBytes <= commandData.Length)
                            {
                                distCount = testCount;
                                distCountOffset = offset;
                                countFound = true;
                                break;
                            }
                        }
                    }
                }
            }
            
            if (!countFound)
            {
                return false;
            }
            
            // Read distance values (each 2 bytes, big endian, units: mm)
            var distValuesOffset = distCountOffset + 2;
            if (distValuesOffset + (distCount * 2) > commandData.Length)
            {
                return false;
            }
            
            var ranges = new List<double>();
            
            for (int i = 0; i < distCount; i++)
            {
                var offset = distValuesOffset + (i * 2);
                if (offset + 2 > commandData.Length)
                {
                    break;
                }
                
                // Read uint16 big endian
                ushort distValue = (ushort)((commandData[offset] << 8) | commandData[offset + 1]);
                // Apply scale factor and convert from mm to meters
                float distanceM = (distValue * scaleFactor + scaleOffset) / 1000.0f;
                ranges.Add(distanceM);
            }
            
            // Find "RSSI1" for intensities (optional)
            var rssi1Pattern = Encoding.ASCII.GetBytes("RSSI1");
            var rssi1Index = -1;
            var intensities = new List<double>();
            
            // Search after DIST1 data
            var rssiSearchStart = distValuesOffset + (distCount * 2);
            for (int i = rssiSearchStart; i <= commandData.Length - rssi1Pattern.Length; i++)
            {
                bool found = true;
                for (int j = 0; j < rssi1Pattern.Length; j++)
                {
                    if (commandData[i + j] != rssi1Pattern[j])
                    {
                        found = false;
                        break;
                    }
                }
                if (found)
                {
                    rssi1Index = i;
                    break;
                }
            }
            
            if (rssi1Index != -1)
            {
                // RSSI data is BINARY after "RSSI1" header
                // Format appears to be: "RSSI1" + <2-byte scaling 3F80> + ... + 0x2B + <RSSI data>
                // Based on hex observation, RSSI values start after finding 0x2B separator
                int rssiDataOffset = rssi1Index + 5; // After "RSSI1"
                
                // Search for 0x2B separator (observed pattern in all telegrams)
                bool found2B = false;
                for (int searchOffset = rssiDataOffset; searchOffset < Math.Min(rssiDataOffset + 50, commandData.Length); searchOffset++)
                {
                    if (commandData[searchOffset] == 0x2B)
                    {
                        rssiDataOffset = searchOffset + 1; // Start after 0x2B
                        found2B = true;
                        break;
                    }
                }
                
                if (!found2B)
                {
                    rssiDataOffset = rssi1Index + 7; // Fallback
                }

                var rssiBytesAvailable = commandData.Length - rssiDataOffset;
                if (rssiBytesAvailable > 0)
                {
                    // RSSI can be encoded as 16-bit or 8-bit values.
                    // Use 16-bit when enough bytes are available; fallback to 8-bit otherwise.
                    var hasFull16BitPayload = rssiBytesAvailable >= (ranges.Count * 2);

                    if (hasFull16BitPayload)
                    {
                        var rssiCount16 = Math.Min(ranges.Count, rssiBytesAvailable / 2);
                        for (int i = 0; i < rssiCount16; i++)
                        {
                            var offset = rssiDataOffset + (i * 2);
                            ushort rssiValue = (ushort)((commandData[offset] << 8) | commandData[offset + 1]);
                            intensities.Add(rssiValue);
                        }
                    }
                    else
                    {
                        var rssiCount8 = Math.Min(ranges.Count, rssiBytesAvailable);
                        for (int i = 0; i < rssiCount8; i++)
                        {
                            byte rssiValue = commandData[rssiDataOffset + i];
                            intensities.Add(rssiValue);
                        }
                    }

                    if (ranges.Count - intensities.Count > 0)
                    {
                        Console.WriteLine($"[SICK-LIDAR-RSSI] WARNING: Only parsed {intensities.Count}/{ranges.Count} RSSI values (telegram truncated)");
                    }
                }
                else
                {
                    Console.WriteLine($"[SICK-LIDAR-RSSI] ERROR: No RSSI data available (offset: {rssiDataOffset}, length: {commandData.Length})");
                }
            }
            else
            {
                // RSSI1 field NOT found in telegram - log only once per 100 scans to avoid spam
                if (ranges.Count > 0 && (ranges.Count % 100 == 0))
                {
                    Console.WriteLine($"[SICK-LIDAR-RSSI] RSSI1 field NOT FOUND. Telegram size: {commandData.Length} bytes, DIST count: {ranges.Count}");
                }
            }
            
            if (ranges.Count == 0)
            {
                return false;
            }
            
            // Calculate angle parameters
            // Use parsed values if available, otherwise use defaults
            float angleMinRad, angleMaxRad, angleIncrementRad;
            
            if (angularStepDeg > 0.0f && startAngleDeg != 0.0f)
            {
                // Use parsed angle information
                angleMinRad = (float)(startAngleDeg * Math.PI / 180.0);
                angleIncrementRad = (float)(angularStepDeg * Math.PI / 180.0);
                angleMaxRad = angleMinRad + (ranges.Count - 1) * angleIncrementRad;
            }
            else
            {
                // Fallback to calculated values
                var angleSpan = _maxAngleRad - _minAngleRad;
                angleMinRad = (float)_minAngleRad;
                angleMaxRad = (float)_maxAngleRad;
                angleIncrementRad = ranges.Count > 1 ? (float)(angleSpan / (ranges.Count - 1)) : (float)(_angularResolutionRad ?? (Math.PI / 180.0));
            }
            
            // Create scan
            scanData = new LaserScan
            {
                Header = new Header
                {
                    Stamp = DateTime.UtcNow,
                    FrameId = _frameId
                },
                AngleMin = angleMinRad,
                AngleMax = angleMaxRad,
                AngleIncrement = angleIncrementRad,
                TimeIncrement = 0.0f,
                ScanTime = _scanFrequencyHz.HasValue ? (float)(1.0 / _scanFrequencyHz.Value) : 0.1f,
                RangeMin = (float)_minRangeM,
                RangeMax = (float)_maxRangeM,
                Ranges = ranges.ToArray(),
                Intensities = intensities.Count > 0 ? intensities.ToArray() : null
            };
            
            return true;
        }
        catch (Exception ex)
        {
            OnErrorOccurred(ex, "Error parsing binary scan data from ColaB");
            return false;
        }
    }

    /// <summary>
    /// Try to parse scan data from ColaB command data (ASCII fallback)
    /// </summary>
    private bool TryParseScanDataFromColaB(ReadOnlySpan<byte> commandData, out LaserScan scanData)
    {
        scanData = default(LaserScan);
        
        try
        {
            // Decode command string to check if it's scan data
            var commandString = ColaBHelpers.ColaBHelper.DecodeCommand(commandData);
            
            // Check if this is scan data telegram
            if (!commandString.Contains("LMDscandata", StringComparison.Ordinal))
            {
                return false;
            }

            // Try to parse using existing ASCII parser as fallback
            var commandStringBytes = commandData.ToArray();
            var buffer = new List<byte>(commandStringBytes);
            
            if (TryParseScanData(buffer, out var parsedScanData, out _))
            {
                scanData = parsedScanData;
                return true;
            }
            
            // If ASCII parsing fails, try to parse the raw string directly
            if (ParseLmdScandata(commandString) is { } parsedData)
            {
                scanData = parsedData;
                return true;
            }
            
            return false;
        }
        catch (Exception ex)
        {
            OnErrorOccurred(ex, "Error parsing scan data from ColaB");
            return false;
        }
    }

    /// <summary>
    /// Try to parse scan data from buffer
    /// This is a simplified parser - real implementation should handle binary protocol properly
    /// </summary>
    private bool TryParseScanData(List<byte> buffer, out LaserScan scanData, out int consumedBytes)
    {
        scanData = default(LaserScan);
        consumedBytes = 0;

        if (buffer.Count < 20)
            return false;

        // Look for scan data telegram
        // For ASCII: "sSN LMDscandata ..."
        // For binary: STX STX STX STX + length + data + CRC

        bool useBinary;
        lock (_protocolLock)
        {
            useBinary = _actualUseBinaryProtocol;
        }

        if (useBinary)
        {
            // Binary protocol parsing
            // Look for STX STX STX STX (0x02 0x02 0x02 0x02)
            var stxIndex = -1;
            for (int i = 0; i <= buffer.Count - 4; i++)
            {
                if (buffer[i] == 0x02 && buffer[i + 1] == 0x02 &&
                    buffer[i + 2] == 0x02 && buffer[i + 3] == 0x02)
                {
                    stxIndex = i;
                    break;
                }
            }

            if (stxIndex < 0)
                return false;

            // Parse binary telegram (simplified - real implementation needs full parser)
            // This is a placeholder - actual parsing should be implemented based on SICK scanner documentation
            // For now, we'll use a basic ASCII parser as fallback
        }

        // ASCII protocol parsing
        var bufferStr = Encoding.ASCII.GetString(buffer.ToArray());
        var scanDataIndex = bufferStr.IndexOf("sSN LMDscandata", StringComparison.Ordinal);
        if (scanDataIndex < 0)
        {
            // Also check for "sAN LMDscandata" (answer format)
            scanDataIndex = bufferStr.IndexOf("sAN LMDscandata", StringComparison.Ordinal);
        }

        if (scanDataIndex < 0)
        {
            // Try to find end of current message to consume bytes
            var etxIndex = buffer.IndexOf(0x03);
            if (etxIndex >= 0)
            {
                consumedBytes = etxIndex + 1;
                return false;
            }
            return false;
        }

        // Find end of message (ETX or next STX)
        var messageStart = scanDataIndex;
        var messageEnd = bufferStr.IndexOf('\x03', messageStart);
        if (messageEnd < 0)
        {
            // Message not complete yet
            return false;
        }

        consumedBytes = messageEnd + 1;

        // Parse scan data (simplified - real implementation needs full parser)
        // This is a basic implementation - should be enhanced with proper parsing
        try
        {
            var parsedData = ParseLmdScandata(bufferStr.Substring(messageStart, messageEnd - messageStart));
            if (parsedData.HasValue)
            {
                scanData = parsedData.Value;
                return true;
            }
            return false;
        }
        catch
        {
            return false;
        }
    }

    /// <summary>
    /// Parse LMDscandata telegram (simplified parser)
    /// Real implementation should handle all fields properly
    /// </summary>
    private LaserScan? ParseLmdScandata(string telegram)
    {
Console.WriteLine($"[SICK-PARSER] ParseLmdScandata (ASCII) called, telegram length: {telegram.Length}");
        
        try
        {
            // Split telegram into fields (space-separated)
            var fields = telegram.Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
            
            if (fields.Length < 20)
            {
                OnErrorOccurred(new InvalidOperationException("Telegram too short"), 
                    $"LMDscandata telegram has only {fields.Length} fields, expected at least 20");
                return null;
            }

            // Find DIST1 field and parse distances
            // Format: "sSN LMDscandata <version> <device> <serial> <status> <counter> <scan> ... DIST1 <count> <distances> ... RSSI1 <count> <intensities> ..."
            var ranges = new List<double>();
            var intensities = new List<double>();
            
            int distIndex = -1;
            int rssiIndex = -1;
            
            // Find DIST1 field
            for (int i = 0; i < fields.Length; i++)
            {
                if (fields[i].StartsWith("DIST", StringComparison.Ordinal))
                {
                    distIndex = i;
                    break;
                }
            }
            
            // Find RSSI1 field
            for (int i = 0; i < fields.Length; i++)
            {
                if (fields[i].StartsWith("RSSI", StringComparison.Ordinal))
                {
                    rssiIndex = i;
                    break;
                }
            }

            // Parse distances
            if (distIndex >= 0 && distIndex + 1 < fields.Length)
            {
                // Next field after DIST1 is the count (in hex)
                if (int.TryParse(fields[distIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int distCount))
                {
                    // Parse distance values (in hex, units are mm, convert to meters)
                    for (int i = 0; i < distCount && distIndex + 2 + i < fields.Length; i++)
                    {
                        if (int.TryParse(fields[distIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int distValue))
                        {
                            // Convert from mm to meters
                            ranges.Add(distValue / 1000.0f);
                        }
                    }
                }
            }

            // Parse intensities (RSSI)
            if (rssiIndex >= 0 && rssiIndex + 1 < fields.Length)
            {
                Console.WriteLine($"[SICK-LIDAR-RSSI-ASCII] Found RSSI at index {rssiIndex}");
                
                // Next field after RSSI1 is the count (in hex)
                if (int.TryParse(fields[rssiIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int rssiCount))
                {
                    Console.WriteLine($"[SICK-LIDAR-RSSI-ASCII] Parsing {rssiCount} RSSI values");
                    
                    // Parse RSSI values (in hex)
                    for (int i = 0; i < rssiCount && rssiIndex + 2 + i < fields.Length; i++)
                    {
                        if (int.TryParse(fields[rssiIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int rssiValue))
                        {
                            intensities.Add(rssiValue);
                        }
                    }
                    
                    Console.WriteLine($"[SICK-LIDAR-RSSI-ASCII] Parsed {intensities.Count} RSSI values successfully");
                }
            }
            else if (ranges.Count > 0 && ranges.Count % 100 == 0)
            {
                Console.WriteLine($"[SICK-LIDAR-RSSI-ASCII] RSSI field NOT FOUND. Fields: {fields.Length}, RSSI index: {rssiIndex}");
            }

            // If no ranges found, return null
            if (ranges.Count == 0)
            {
                OnErrorOccurred(new InvalidOperationException("No distance data found"), 
                    $"LMDscandata telegram does not contain distance data. Fields: {string.Join(" ", fields.Take(30))}");
                return null;
            }

            // Calculate angle parameters from ranges count
            var angleSpan = _maxAngleRad - _minAngleRad;
            var angleIncrement = ranges.Count > 1 
                ? (float)(angleSpan / (ranges.Count - 1)) 
                : (_angularResolutionRad.HasValue ? (float)_angularResolutionRad.Value : (float)(Math.PI / 180.0));

            // Create scan
            var scan = new LaserScan
            {
                Header = new Header
                {
                    Stamp = DateTime.UtcNow,
                    FrameId = _frameId
                },
                AngleMin = (float)_minAngleRad,
                AngleMax = (float)_maxAngleRad,
                AngleIncrement = angleIncrement,
                TimeIncrement = 0.0f,
                ScanTime = _scanFrequencyHz.HasValue ? (float)(1.0 / _scanFrequencyHz.Value) : 0.1f,
                RangeMin = (float)_minRangeM,
                RangeMax = (float)_maxRangeM,
                Ranges = ranges.ToArray(),
                Intensities = intensities.Count > 0 ? intensities.ToArray() : null
            };

            return scan;
        }
        catch (Exception ex)
        {
            OnErrorOccurred(ex, $"Error parsing LMDscandata telegram: {telegram.Substring(0, Math.Min(200, telegram.Length))}");
            return null;
        }
    }

    /// <summary>
    /// Update scan data and fire event
    /// </summary>
    private void UpdateScanData(LaserScan scanData)
    {
        lock (_scanDataLock)
        {
            _lastScanData = scanData;
            _lastScanDataTimestamp = DateTime.UtcNow;
        }

        // Fire ILidar event (this notifies subscribers like web UI)
        var eventArgs = new LidarScanDataEventArgs(_lastScanDataTimestamp.Value, scanData);
        ScanDataReceived?.Invoke(this, eventArgs);

        // Update properties for UI
        SetProperty("LastScanDataTimestamp", _lastScanDataTimestamp.Value.ToString("yyyy-MM-dd HH:mm:ss.fff"));
        
    }

    #endregion

    #region ILidar Implementation

    public LaserScan? CurrentMeasurementData
    {
        get
        {
            lock (_scanDataLock)
            {
                return _lastScanData;
            }
        }
    }

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

    public double MinAngleRad => _minAngleRad;

    public double MaxAngleRad => _maxAngleRad;

    public double MinRangeM => _minRangeM;

    public double MaxRangeM => _maxRangeM;

    public double? AngularResolutionRad => _angularResolutionRad;

    public double? ScanFrequencyHz => _scanFrequencyHz;

    public double FieldOfViewRad => Math.Abs(_maxAngleRad - _minAngleRad);

    public bool SupportsIntensity => _supportsIntensity;

    public double? AccuracyM => null; // Not specified

    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;

    #endregion

    protected override void Dispose(bool disposing)
    {
        if (disposing)
        {
            _receiveCts?.Cancel();
            _receiveTask?.Wait(1000);
            _receiveCts?.Dispose();
            _receiveTask?.Dispose();

            DisconnectInternal();
        }

        base.Dispose(disposing);
    }
}