using Sick.SafetyScanners.Cola2;
using Sick.SafetyScanners.Cola2.Commands;
using Sick.SafetyScanners.Communication;
using Sick.SafetyScanners.DataProcessing;
using Sick.SafetyScanners.DataStructures;
using Sick.SafetyScanners.Interfaces;
using Sick.SafetyScanners.Types;

namespace Sick.SafetyScanners;

/// <summary>
/// Main class for SICK Safety Scanner communication
/// Handles both TCP (COLA2) and UDP (scan data streaming) communication
/// Thread-safe implementation
/// </summary>
public sealed class SafetyScanner : IDisposable
{
    private readonly ITcpClient _tcpClient;
    private readonly ICola2Session _cola2Session;

    // UDP components for scan data streaming
    private readonly IUdpClient? _udpClient;
    private readonly UdpPacketMerger? _udpPacketMerger;
    private readonly ParseData? _parseData;
    private readonly Lock _udpLock = new();
    private bool _isUdpStreaming;
    private bool _disposed;

    /// <summary>
    /// Creates a new Safety Scanner instance
    /// </summary>
    public SafetyScanner(string sensorIp, ushort sensorPort)
        : this(new TcpClient(sensorIp, sensorPort))
    {
    }

    /// <summary>
    /// Creates a new Safety Scanner instance with custom TCP client
    /// </summary>
    public SafetyScanner(ITcpClient tcpClient)
    {
        _tcpClient = tcpClient ?? throw new ArgumentNullException(nameof(tcpClient));
        _cola2Session = new Cola2Session(_tcpClient);
    }

    /// <summary>
    /// Creates a new Safety Scanner instance with UDP streaming support
    /// </summary>
    /// <param name="sensorIp">Sensor IP address</param>
    /// <param name="sensorPort">Sensor TCP port (COLA2)</param>
    /// <param name="udpLocalPort">Local UDP port for receiving scan data (0 = auto-assign)</param>
    public SafetyScanner(string sensorIp, ushort sensorPort, ushort udpLocalPort)
        : this(new TcpClient(sensorIp, sensorPort), udpLocalPort)
    {
    }

    /// <summary>
    /// Creates a new Safety Scanner instance with UDP streaming support and specific local IP
    /// </summary>
    /// <param name="sensorIp">Sensor IP address</param>
    /// <param name="sensorPort">Sensor TCP port (COLA2)</param>
    /// <param name="udpLocalPort">Local UDP port for receiving scan data (0 = auto-assign)</param>
    /// <param name="udpLocalIp">Local IP address to bind UDP server to (null = bind to 0.0.0.0, all interfaces)</param>
    public SafetyScanner(string sensorIp, ushort sensorPort, ushort udpLocalPort, System.Net.IPAddress? udpLocalIp)
        : this(new TcpClient(sensorIp, sensorPort), udpLocalPort, udpLocalIp)
    {
    }

    /// <summary>
    /// Creates a new Safety Scanner instance with custom TCP client and UDP streaming support
    /// </summary>
    /// <param name="tcpClient">TCP client for COLA2 communication</param>
    /// <param name="udpLocalPort">Local UDP port for receiving scan data (0 = auto-assign)</param>
    public SafetyScanner(ITcpClient tcpClient, ushort udpLocalPort)
        : this(tcpClient, udpLocalPort, null)
    {
    }

    /// <summary>
    /// Creates a new Safety Scanner instance with custom TCP client and UDP streaming support with specific local IP
    /// </summary>
    /// <param name="tcpClient">TCP client for COLA2 communication</param>
    /// <param name="udpLocalPort">Local UDP port for receiving scan data (0 = auto-assign)</param>
    /// <param name="udpLocalIp">Local IP address to bind UDP server to (null = bind to 0.0.0.0, all interfaces)</param>
    public SafetyScanner(ITcpClient tcpClient, ushort udpLocalPort, System.Net.IPAddress? udpLocalIp)
    {
        _tcpClient = tcpClient ?? throw new ArgumentNullException(nameof(tcpClient));
        _cola2Session = new Cola2Session(_tcpClient);

        // Initialize UDP components for scan data streaming
        _udpClient = new Communication.UdpClient(udpLocalPort, udpLocalIp);
        _udpPacketMerger = new UdpPacketMerger();
        _parseData = new ParseData();
    }

    /// <summary>
    /// Gets the COLA2 session
    /// </summary>
    public ICola2Session Session => _cola2Session;

    /// <summary>
    /// Indicates whether the scanner is connected
    /// - For TCP mode: checks TCP connection and COLA2 session
    /// - For UDP streaming mode: checks UDP streaming status (UDP receiver is bound and listening)
    /// Note: UDP is connectionless - this checks if UDP receiver is ready to receive packets
    /// </summary>
    public bool IsConnected
    {
        get
        {
            // If UDP streaming is enabled, check UDP streaming status instead of TCP
            if (_udpClient != null)
            {
                lock (_udpLock)
                {
                    // In UDP streaming mode, connection means UDP receiver is bound and listening
                    return _isUdpStreaming && _udpClient.IsConnected;
                }
            }

            // For TCP-only mode, check TCP connection and COLA2 session
            return _tcpClient.IsConnected && _cola2Session.IsOpen;
        }
    }

    /// <summary>
    /// Indicates whether UDP streaming is active
    /// </summary>
    public bool IsUdpStreaming
    {
        get
        {
            lock (_udpLock)
            {
                return _isUdpStreaming && _udpClient != null && _udpClient.IsConnected;
            }
        }
    }

    /// <summary>
    /// Gets the local UDP port (0 if UDP is not enabled or not bound)
    /// </summary>
    public ushort LocalUdpPort
    {
        get
        {
            lock (_udpLock)
            {
                return _udpClient?.LocalPort?.Value ?? (ushort)0;
            }
        }
    }

    /// <summary>
    /// Event fired when scan data is received via UDP
    /// </summary>
    public event EventHandler<UdpScanDataEventArgs>? ScanDataReceived;

    /// <summary>
    /// Connects to the scanner and opens a COLA2 session
    /// Note: For UDP streaming mode, COLA2 session is optional if scanner is already configured.
    /// COLA2 session is only needed to configure scanner settings (e.g., ChangeCommSettings).
    /// </summary>
    /// <param name="openCola2Session">Whether to open COLA2 session. Default: true.
    /// Set to false for UDP-only mode if scanner is already configured.</param>
    public void Connect(bool openCola2Session = true)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        if (openCola2Session)
        {
            _cola2Session.Open();
        }
    }

    /// <summary>
    /// Disconnects from the scanner and closes the COLA2 session
    /// </summary>
    public void Disconnect()
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));

        _cola2Session.Close();
    }

    /// <summary>
    /// Sends a COLA2 command to the scanner
    /// </summary>
    public void SendCommand(ICola2Command command, TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        _cola2Session.SendCommand(command, timeout);
    }

    /// <summary>
    /// Reads a variable from the scanner by index
    /// </summary>
    public ReadOnlyMemory<byte> ReadVariable(ushort variableIndex,
        TimeDuration? timeout = null)
    {
        var command = new VariableCommand(variableIndex);
        SendCommand(command, timeout);

        if (!command.WasSuccessful)
        {
            throw new Exceptions.CommandException(
                command.CommandType,
                command.CommandMode,
                $"Failed to read variable at index {variableIndex}"
            );
        }

        return command.GetDataVector();
    }

    /// <summary>
    /// Requests the latest telegram (measurement data) from the scanner via TCP
    /// Note: Unlike UDP streaming, TCP requires sending a request command to receive data
    /// </summary>
    /// <param name="channelIndex">Channel index (0-3), defaults to 0</param>
    /// <param name="timeout">Timeout for the request</param>
    /// <returns>The parsed scan data</returns>
    public DataStructures.UdpScanData RequestLatestTelegram(
        sbyte channelIndex = 0,
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var command = new LatestTelegramVariableCommand(channelIndex);
        SendCommand(command, timeout);

        if (!command.WasSuccessful)
        {
            throw new Exceptions.CommandException(
                command.CommandType,
                command.CommandMode,
                $"Failed to request latest telegram for channel {channelIndex}"
            );
        }

        if (command.ScanData == null)
        {
            throw new Exceptions.CommandException(
                command.CommandType,
                command.CommandMode,
                $"Failed to parse scan data from latest telegram response"
            );
        }

        return command.ScanData;
    }

    #region Request Methods - COLA2 Variable Commands

    /// <summary>
    /// Requests the type code from the sensor
    /// </summary>
    public DataStructures.TypeCode RequestTypeCode(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(0x000d, timeout);
        var parser = new DataProcessing.ParseTypeCode();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the application name from the sensor
    /// </summary>
    public DataStructures.ApplicationName RequestApplicationName(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));

        var data = ReadVariable(33, timeout);
        var parser = new DataProcessing.ParseApplicationName();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the serial number from the sensor
    /// </summary>
    public DataStructures.SerialNumber RequestSerialNumber(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(3, timeout);
        var parser = new DataProcessing.ParseSerialNumber();
        var buffer = new PacketBuffer(data);
        return ParseSerialNumber.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the firmware version from the sensor
    /// </summary>
    public DataStructures.FirmwareVersion RequestFirmwareVersion(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(4, timeout);
        var parser = new DataProcessing.ParseFirmwareVersion();
        var buffer = new PacketBuffer(data);
        return ParseFirmwareVersion.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the order number from the sensor
    /// </summary>
    public DataStructures.OrderNumber RequestOrderNumber(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(14, timeout);
        var parser = new DataProcessing.ParseOrderNumber();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the project name from the sensor
    /// </summary>
    public DataStructures.ProjectName RequestProjectName(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(18, timeout);
        var parser = new DataProcessing.ParseProjectName();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the user name from the sensor
    /// </summary>
    public DataStructures.UserName RequestUserName(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(35, timeout);
        var parser = new DataProcessing.ParseUserName();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the device name from the sensor
    /// </summary>
    public DataStructures.DeviceName RequestDeviceName(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(17, timeout);
        var parser = new DataProcessing.ParseDeviceName();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the device status from the sensor
    /// </summary>
    public DataStructures.DeviceStatus RequestDeviceStatus(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(15, timeout);
        var parser = new DataProcessing.ParseDeviceStatus();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the config metadata from the sensor
    /// </summary>
    public DataStructures.ConfigMetadata RequestConfigMetadata(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(28, timeout);
        var parser = new DataProcessing.ParseConfigMetadata();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the status overview from the sensor
    /// </summary>
    public DataStructures.StatusOverview RequestStatusOverview(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(23, timeout);
        var parser = new DataProcessing.ParseStatusOverview();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the required user action from the sensor
    /// </summary>
    public DataStructures.RequiredUserAction RequestRequiredUserAction(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(16, timeout);
        var parser = new DataProcessing.ParseRequiredUserAction();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the persistent configuration from the sensor
    /// </summary>
    public DataStructures.ConfigData RequestPersistentConfig(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(177, timeout);
        var parser = new DataProcessing.ParseMeasurementPersistentConfigData();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests the current measurement configuration from the sensor
    /// </summary>
    public DataStructures.ConfigData RequestCurrentConfig(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(178, timeout);
        var parser = new DataProcessing.ParseMeasurementCurrentConfigData();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests field sets data from the sensor
    /// </summary>
    public DataStructures.FieldSets RequestFieldSets(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var data = ReadVariable(1003, timeout);
        var parser = new DataProcessing.ParseFieldSetsData();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests field data (header) from the sensor for a specific field index
    /// </summary>
    /// <param name="fieldIndex">Field index (0-127)</param>
    public DataStructures.FieldData RequestFieldHeader(
        ushort fieldIndex,
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        ushort variableIndex = (ushort)(0x2710 + fieldIndex); // 10000 + fieldIndex
        var data = ReadVariable(variableIndex, timeout);
        var parser = new DataProcessing.ParseFieldHeaderData();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests field geometry data from the sensor for a specific field index
    /// </summary>
    /// <param name="fieldIndex">Field index (0-127)</param>
    public DataStructures.FieldData RequestFieldGeometry(
        ushort fieldIndex,
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        ushort variableIndex = (ushort)(0x2810 + fieldIndex); // 10256 + fieldIndex
        var data = ReadVariable(variableIndex, timeout);
        var parser = new DataProcessing.ParseFieldGeometryData();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests complete field data (header + geometry) from the sensor for a specific field index
    /// </summary>
    /// <param name="fieldIndex">Field index (0-127)</param>
    public DataStructures.FieldData RequestFieldData(
        ushort fieldIndex,
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        // Request header first
        var fieldData = RequestFieldHeader(fieldIndex, timeout);

        // If valid, request geometry
        if (fieldData.IsValid)
        {
            var geometry = RequestFieldGeometry(fieldIndex, timeout);

            // Merge geometry data into fieldData
            return new DataStructures.FieldData
            {
                IsValid = fieldData.IsValid,
                VersionCVersion = fieldData.VersionCVersion,
                VersionMajorVersionNumber = fieldData.VersionMajorVersionNumber,
                VersionMinorVersionNumber = fieldData.VersionMinorVersionNumber,
                VersionReleaseNumber = fieldData.VersionReleaseNumber,
                IsDefined = fieldData.IsDefined,
                EvalMethod = fieldData.EvalMethod,
                MultiSampling = fieldData.MultiSampling,
                ObjectResolution = fieldData.ObjectResolution,
                FieldSetIndex = fieldData.FieldSetIndex,
                NameLength = fieldData.NameLength,
                FieldName = fieldData.FieldName,
                IsWarningField = fieldData.IsWarningField,
                IsProtectiveField = fieldData.IsProtectiveField,
                BeamDistances = geometry.BeamDistances,
                StartAngle = geometry.StartAngle,
                EndAngle = geometry.EndAngle,
                AngularBeamResolution = geometry.AngularBeamResolution
            };
        }

        return fieldData;
    }

    /// <summary>
    /// Requests all valid field data from the sensor
    /// </summary>
    public List<DataStructures.FieldData> RequestAllFieldData(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var fields = new List<DataStructures.FieldData>();

        // Request up to 128 fields, stop at first invalid (after index 0)
        for (ushort i = 0; i < 128; i++)
        {
            try
            {
                var fieldData = RequestFieldData(i, timeout);

                if (fieldData.IsValid)
                {
                    fields.Add(fieldData);
                }
                else if (i > 0) // Index 0 is reserved for contour data
                {
                    break; // Stop at first invalid field (after index 0)
                }
            }
            catch
            {
                // If request fails, stop iterating
                break;
            }
        }

        return fields;
    }

    /// <summary>
    /// Requests monitoring case data from the sensor for a specific case index
    /// </summary>
    /// <param name="caseIndex">Monitoring case index (0-253)</param>
    public DataStructures.MonitoringCaseData RequestMonitoringCase(
        ushort caseIndex,
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        ushort variableIndex = (ushort)(2101 + caseIndex);
        var data = ReadVariable(variableIndex, timeout);
        var parser = new DataProcessing.ParseMonitoringCaseData();
        var buffer = new PacketBuffer(data);
        return parser.ParseTcpSequence(buffer);
    }

    /// <summary>
    /// Requests all valid monitoring cases from the sensor
    /// </summary>
    public List<DataStructures.MonitoringCaseData> RequestMonitoringCases(
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var monitoringCases = new List<DataStructures.MonitoringCaseData>();

        // Request up to 254 monitoring cases, stop at first invalid
        for (ushort i = 0; i < 254; i++)
        {
            try
            {
                var monitoringCase = RequestMonitoringCase(i, timeout);

                if (monitoringCase.IsValid)
                {
                    monitoringCases.Add(monitoringCase);
                }
                else
                {
                    break; // Stop at first invalid case
                }
            }
            catch
            {
                // If request fails, stop iterating
                break;
            }
        }

        return monitoringCases;
    }

    #endregion

    /// <summary>
    /// Changes the communication settings on the sensor (CRITICAL method)
    /// Note: This method should be called before starting UDP streaming
    /// </summary>
    /// <param name="settings">The communication settings to apply</param>
    /// <param name="timeout">Timeout for the command</param>
    public void ChangeCommSettings(
        DataStructures.CommSettings settings,
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));
        ArgumentNullException.ThrowIfNull(settings);

        // Update settings with actual UDP port if UDP client is available
        // Create new settings with updated UDP port
        var finalSettings = settings;
        if (_udpClient != null && _udpClient.LocalPort?.Value != null)
        {
            var actualPort = _udpClient.LocalPort.Value;
            if (actualPort != settings.HostUdpPort)
            {
                finalSettings = DataStructures.CommSettings.Create(
                    channel: settings.Channel,
                    hostIp: settings.HostIp,
                    hostUdpPort: actualPort,
                    generalSystemState: settings.GeneralSystemStateEnabled,
                    derivedSettings: settings.DerivedSettingsEnabled,
                    measurementData: settings.MeasurementDataEnabled,
                    intrusionData: settings.IntrusionDataEnabled,
                    applicationData: settings.ApplicationDataEnabled,
                    publishingFrequency: settings.PublishingFrequency,
                    startAngle: settings.StartAngle,
                    endAngle: settings.EndAngle,
                    interfaceType: settings.EInterfaceType,
                    enabled: settings.Enabled
                );
            }
        }

        var command = new Cola2.Commands.ChangeCommSettingsCommand(finalSettings);
        SendCommand(command, timeout);

        if (!command.WasSuccessful)
        {
            throw new Exceptions.CommandException(
                command.CommandType,
                command.CommandMode,
                "Failed to change communication settings"
            );
        }
    }

    /// <summary>
    /// Makes the scanner flash/blink its display to help locate it
    /// </summary>
    /// <param name="blinkTime">Time to flash for in seconds</param>
    /// <param name="timeout">Timeout for the command</param>
    public void FindSensor(
        ushort blinkTime,
        TimeDuration? timeout = null)
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        var command = new Cola2.Commands.FindMeCommand(blinkTime);
        SendCommand(command, timeout);

        if (!command.WasSuccessful)
        {
            throw new Exceptions.CommandException(
                command.CommandType,
                command.CommandMode,
                $"Failed to send find sensor command (blink time: {blinkTime}s)"
            );
        }
    }

    /// <summary>
    /// Starts UDP streaming to receive scan data automatically
    /// </summary>
    /// <exception cref="InvalidOperationException">If UDP client is not initialized or already streaming</exception>
    public void StartUdpStreaming()
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));


        if (_udpClient == null || _udpPacketMerger == null || _parseData == null)
            throw new InvalidOperationException("UDP streaming is not enabled. Use constructor with udpLocalPort parameter.");

        lock (_udpLock)
        {
            if (_isUdpStreaming)
                return; // Already streaming

            _isUdpStreaming = true;
        }

        // Start receiving loop on dedicated high-priority thread
        // Note: StartReceiving will create and bind the socket, then start receiving in background
        _udpClient.StartReceiving((packet) =>
        {
            ProcessUdpPacket(packet, _udpPacketMerger, _parseData);
        });
    }

    /// <summary>
    /// Stops UDP streaming
    /// </summary>
    public void StopUdpStreaming()
    {
        lock (_udpLock)
        {
            if (!_isUdpStreaming)
                return;

            _isUdpStreaming = false;

            _udpClient?.Stop();

            _udpPacketMerger?.Reset();
        }
    }

    /// <summary>
    /// Processes incoming UDP packet (merges fragments and parses data)
    /// </summary>
    private void ProcessUdpPacket(PacketBuffer packet, UdpPacketMerger packetMerger, ParseData parseData)
    {
        try
        {
            // Add packet to merger
            var isComplete = packetMerger.AddUdpPacket(packet);

            if (isComplete)
            {
                // Get merged packet
                var mergedPacket = packetMerger.GetDeployedPacketBuffer();

                // Parse scan data
                var scanData = parseData.ParseUdpSequence(mergedPacket);

                // Fire event
                var timestamp = scanData.Timestamp ?? DateTime.UtcNow;
                var eventArgs = new UdpScanDataEventArgs(timestamp, scanData);
                ScanDataReceived?.Invoke(this, eventArgs);
            }
        }
        catch (Exception ex)
        {
            // Log error but continue receiving
            // In production, you might want to fire an error event
            System.Diagnostics.Debug.WriteLine($"Error processing UDP packet: {ex.Message}");
        }
    }

    public void Dispose()
    {
        ObjectDisposedException.ThrowIf(_disposed, nameof(SafetyScanner));
        
        // Stop UDP streaming first
        StopUdpStreaming();

        // Disconnect TCP
        Disconnect();

        // Dispose TCP components
        _cola2Session.Dispose();
        _tcpClient.Dispose();

        // Dispose UDP components
        lock (_udpLock)
        {
            _udpClient?.Dispose();
            _udpPacketMerger?.Dispose();
        }

        _disposed = true;
    }
}