Initial commit

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataProcessing/ParseIntrusionData.cs

@@ -0,0 +1,161 @@
+using Sick.SafetyScanners.DataStructures;
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.DataProcessing;
+
+/// <summary>
+/// Parser for intrusion data block from UDP packets
+/// Contains intrusion data for 24 cut-off paths (field interruption)
+/// Thread-safe implementation
+/// </summary>
+public sealed class ParseIntrusionData
+{
+    private const int NumberOfIntrusionDatums = 24;
+    
+    /// <summary>
+    /// Parses the intrusion data block from a UDP sequence
+    /// </summary>
+    /// <param name="buffer">Packet buffer containing the data</param>
+    /// <param name="header">Parsed data header (must contain valid block offset/size)</param>
+    /// <param name="derivedValues">Parsed derived values (required for number of scan points)</param>
+    public IntrusionData ParseUdpSequence(PacketBuffer buffer, DataHeader header, DerivedValues derivedValues)
+    {
+        if (buffer == null)
+            throw new ArgumentNullException(nameof(buffer));
+        
+        if (header == null)
+            throw new ArgumentNullException(nameof(header));
+        
+        // Check if intrusion data block is enabled
+        if (!CheckIfIntrusionDataIsPublished(header))
+        {
+            return new IntrusionData
+            {
+                IsEmpty = true
+            };
+        }
+        
+        // Check if header is valid
+        if (header.IsEmpty)
+        {
+            return new IntrusionData
+            {
+                IsEmpty = true
+            };
+        }
+        
+        // Check if derived values are available (required for number of scan points)
+        if (derivedValues == null || derivedValues.IsEmpty)
+        {
+            return new IntrusionData
+            {
+                IsEmpty = true
+            };
+        }
+        
+        var bufferData = buffer.GetBuffer();
+        var offset = header.IntrusionDataBlockOffset;
+        var numberOfScanPoints = derivedValues.NumberOfBeams;
+        
+        // Validate buffer size (at least 4 bytes for first size field)
+        if (offset + 4 > bufferData.Length)
+        {
+            throw new ArgumentException(
+                $"Buffer too small to contain intrusion data block header (offset: {offset}, buffer size: {bufferData.Length})",
+                nameof(buffer));
+        }
+        
+        var span = bufferData.Span.Slice(offset);
+        var intrusionDatums = new List<IntrusionDatum>(NumberOfIntrusionDatums);
+        
+        // Parse 24 intrusion datums
+        // Each datum consists of:
+        // - Size (4 bytes, uint32) - number of bytes in flags vector
+        // - Flags vector (variable size, 1 bit per scan point indicating intrusion)
+        
+        int currentOffset = 0;
+        
+        for (int i = 0; i < NumberOfIntrusionDatums; i++)
+        {
+            // Validate we have enough data for size field
+            if (offset + currentOffset + 4 > bufferData.Length)
+            {
+                throw new ArgumentException(
+                    $"Buffer too small to read intrusion datum {i} size (offset: {offset + currentOffset}, buffer size: {bufferData.Length})",
+                    nameof(buffer));
+            }
+            
+            // Read size (4 bytes, little endian uint32)
+            var sizeBytes = ReadWriteHelper.ReadUint32LittleEndian(span, currentOffset);
+            currentOffset += 4;
+            
+            // Validate size is reasonable
+            if (sizeBytes > 10000) // Sanity check
+            {
+                throw new ArgumentException(
+                    $"Invalid intrusion datum {i} size: {sizeBytes}",
+                    nameof(buffer));
+            }
+            
+            // Validate we have enough data for flags vector
+            if (offset + currentOffset + sizeBytes > bufferData.Length)
+            {
+                throw new ArgumentException(
+                    $"Buffer too small to read intrusion datum {i} flags (size: {sizeBytes}, available: {bufferData.Length - offset - currentOffset})",
+                    nameof(buffer));
+            }
+            
+            // Parse flags vector
+            // Each byte contains 8 flags (bits), one per scan point
+            var flags = new List<bool>((int)numberOfScanPoints);
+            uint numReadFlags = 0;
+            
+            for (int byteIndex = 0; byteIndex < sizeBytes && numReadFlags < numberOfScanPoints; byteIndex++)
+            {
+                var byteValue = ReadWriteHelper.ReadUint8LittleEndian(span, currentOffset + byteIndex);
+                
+                // Extract 8 bits from this byte
+                for (int bitIndex = 0; bitIndex < 8 && numReadFlags < numberOfScanPoints; bitIndex++, numReadFlags++)
+                {
+                    flags.Add((byteValue & (0x01 << bitIndex)) != 0);
+                }
+            }
+            
+            // Ensure we have exactly numberOfScanPoints flags (pad with false if needed)
+            while (flags.Count < numberOfScanPoints)
+            {
+                flags.Add(false);
+            }
+            
+            // Truncate if we have more than numberOfScanPoints flags
+            if (flags.Count > numberOfScanPoints)
+            {
+                flags = flags.Take((int)numberOfScanPoints).ToList();
+            }
+            
+            intrusionDatums.Add(new IntrusionDatum
+            {
+                Size = (int)sizeBytes,
+                Flags = flags
+            });
+            
+            // Advance offset by size bytes
+            currentOffset += (int)sizeBytes;
+        }
+        
+        return new IntrusionData
+        {
+            IntrusionDatums = intrusionDatums,
+            IsEmpty = false
+        };
+    }
+    
+    /// <summary>
+    /// Checks if intrusion data block is published (enabled)
+    /// </summary>
+    private static bool CheckIfIntrusionDataIsPublished(DataHeader header)
+    {
+        return !(header.IntrusionDataBlockOffset == 0 && header.IntrusionDataBlockSize == 0);
+    }
+}
+