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

using Sick.SafetyScanners.DataStructures;
using Sick.SafetyScanners.Helpers;

namespace Sick.SafetyScanners.DataProcessing;

/// <summary>
/// Parser for measurement data block from UDP packets
/// Contains scan points with distance, reflectivity, and status flags
/// Thread-safe implementation
/// </summary>
public sealed class ParseMeasurementData
{
    private const uint MaxExpectedBeams = 2751;
    
    /// <summary>
    /// Parses the measurement 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 angle calculation)</param>
    public MeasurementData 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 measurement data block is enabled
        if (!CheckIfMeasurementDataIsPublished(header))
        {
            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
        }
        
        // Check if header is valid
        if (header.IsEmpty)
        {
            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
        }
        
        // Check if derived values are available (required for angle calculation)
        if (derivedValues == null || derivedValues.IsEmpty)
        {
            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
        }
        
        var bufferData = buffer.GetBuffer();
        var offset = header.MeasurementDataBlockOffset;
        
        // Validate buffer size (at least 4 bytes for number of beams)
        if (offset + 4 > bufferData.Length)
        {
            throw new ArgumentException(
                $"Buffer too small to contain measurement data block header (offset: {offset}, buffer size: {bufferData.Length})",
                nameof(buffer));
        }
        
        var span = bufferData.Span.Slice(offset);
        
        // Parse number of beams (first 4 bytes, little endian uint32)
        var numberOfBeams = ReadWriteHelper.ReadUint32LittleEndian(span, 0);
        
        // Validate number of beams (safety check)
        if (numberOfBeams > MaxExpectedBeams)
        {
            // Log warning would go here in production
            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
        }
        
        // Calculate required buffer size: 4 bytes (number of beams) + numberOfBeams * 4 bytes (per scan point)
        var requiredSize = 4 + numberOfBeams * 4;
        if (offset + requiredSize > bufferData.Length)
        {
            throw new ArgumentException(
                $"Buffer too small to contain all measurement data (required: {requiredSize}, available: {bufferData.Length - offset})",
                nameof(buffer));
        }
        
        // Parse scan points
        var scanPoints = new List<ScanPoint>((int)numberOfBeams);
        var currentAngle = derivedValues.StartAngle;
        var angleDelta = derivedValues.AngularBeamResolution;
        
        for (uint i = 0; i < numberOfBeams; i++)
        {
            // Each scan point is 4 bytes:
            // Offset 4 + i*4: Distance (uint16, mm)
            // Offset 6 + i*4: Reflectivity (uint8, 0-255)
            // Offset 7 + i*4: Status flags (uint8)
            //   Bit 0: Valid
            //   Bit 1: Infinite
            //   Bit 2: Glare
            //   Bit 3: Reflector
            //   Bit 4: Contamination
            //   Bit 5: Contamination warning
            
            var pointOffset = 4 + (int)(i * 4);
            var distance = ReadWriteHelper.ReadUint16LittleEndian(span, pointOffset);
            var reflectivity = ReadWriteHelper.ReadUint8LittleEndian(span, pointOffset + 2);
            var status = ReadWriteHelper.ReadUint8LittleEndian(span, pointOffset + 3);
            
            var isValid = (status & 0x01) != 0;
            var isInfinite = (status & 0x02) != 0;
            var hasGlare = (status & 0x04) != 0;
            var isReflector = (status & 0x08) != 0;
            var isContaminated = (status & 0x10) != 0;
            var hasContaminationWarning = (status & 0x20) != 0;
            
            scanPoints.Add(new ScanPoint(
                angle: currentAngle,
                distance: distance,
                reflectivity: reflectivity,
                isValid: isValid,
                isInfinite: isInfinite,
                hasGlare: hasGlare,
                isReflector: isReflector,
                isContaminated: isContaminated,
                hasContaminationWarning: hasContaminationWarning
            ));
            
            // Advance angle for next point
            currentAngle += angleDelta;
        }
        
        return new MeasurementData(numberOfBeams, scanPoints, isEmpty: false);
    }
    
    /// <summary>
    /// Checks if measurement data block is published (enabled)
    /// </summary>
    private static bool CheckIfMeasurementDataIsPublished(DataHeader header)
    {
        return !(header.MeasurementDataBlockOffset == 0 && header.MeasurementDataBlockSize == 0);
    }
}