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

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);
    }
}