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

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

namespace Sick.SafetyScanners.DataProcessing;

/// <summary>
/// Parser for general system state block from UDP packets
/// Contains device status, cut-off paths, monitoring cases, and errors
/// Thread-safe implementation
/// </summary>
public sealed class ParseGeneralSystemState
{
    /// <summary>
    /// Parses the general system state 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>
    public GeneralSystemState ParseUdpSequence(PacketBuffer buffer, DataHeader header)
    {
        if (buffer == null)
            throw new ArgumentNullException(nameof(buffer));
        
        if (header == null)
            throw new ArgumentNullException(nameof(header));
        
        // Check if general system state block is enabled
        if (!CheckIfGeneralSystemStateIsPublished(header))
        {
            return new GeneralSystemState
            {
                IsEmpty = true
            };
        }
        
        // Check if header is valid
        if (header.IsEmpty)
        {
            return new GeneralSystemState
            {
                IsEmpty = true
            };
        }
        
        var bufferData = buffer.GetBuffer();
        var offset = header.GeneralSystemStateBlockOffset;
        
        // Validate buffer size (at least 16 bytes for all fields)
        if (offset + 16 > bufferData.Length)
        {
            throw new ArgumentException(
                $"Buffer too small to contain general system state block (offset: {offset}, buffer size: {bufferData.Length})",
                nameof(buffer));
        }
        
        var span = bufferData.Span.Slice(offset);
        
        // Parse status bits (offset 0)
        var statusByte = ReadWriteHelper.ReadUint8LittleEndian(span, 0);
        var isRunModeActive = (statusByte & 0x01) != 0;
        var isStandbyModeActive = (statusByte & 0x02) != 0;
        var hasContaminationWarning = (statusByte & 0x04) != 0;
        var hasContaminationError = (statusByte & 0x08) != 0;
        var referenceContourStatus = (statusByte & 0x10) != 0;
        var manipulationStatus = (statusByte & 0x20) != 0;
        // Bits 6 and 7 are reserved
        
        // Parse safe cut-off paths (offsets 1-3)
        var safeCutOffPaths = ParseCutOffPaths(span, 1);
        
        // Parse non-safe cut-off paths (offsets 4-6)
        var nonSafeCutOffPaths = ParseCutOffPaths(span, 4);
        
        // Parse reset required cut-off paths (offsets 7-9)
        var resetRequiredCutOffPaths = ParseCutOffPaths(span, 7);
        
        // Parse monitoring cases (offsets 10-13)
        var currentMonitoringCaseNoTable1 = ReadWriteHelper.ReadUint8LittleEndian(span, 10);
        var currentMonitoringCaseNoTable2 = ReadWriteHelper.ReadUint8LittleEndian(span, 11);
        var currentMonitoringCaseNoTable3 = ReadWriteHelper.ReadUint8LittleEndian(span, 12);
        var currentMonitoringCaseNoTable4 = ReadWriteHelper.ReadUint8LittleEndian(span, 13);
        
        // Parse errors (offset 15)
        var errorByte = ReadWriteHelper.ReadUint8LittleEndian(span, 15);
        var hasApplicationError = (errorByte & 0x01) != 0;
        var hasDeviceError = (errorByte & 0x02) != 0;
        
        return new GeneralSystemState
        {
            IsRunModeActive = isRunModeActive,
            IsStandbyModeActive = isStandbyModeActive,
            HasContaminationWarning = hasContaminationWarning,
            HasContaminationError = hasContaminationError,
            ReferenceContourStatus = referenceContourStatus,
            ManipulationStatus = manipulationStatus,
            SafeCutOffPaths = safeCutOffPaths,
            NonSafeCutOffPaths = nonSafeCutOffPaths,
            ResetRequiredCutOffPaths = resetRequiredCutOffPaths,
            CurrentMonitoringCaseNoTable1 = currentMonitoringCaseNoTable1,
            CurrentMonitoringCaseNoTable2 = currentMonitoringCaseNoTable2,
            CurrentMonitoringCaseNoTable3 = currentMonitoringCaseNoTable3,
            CurrentMonitoringCaseNoTable4 = currentMonitoringCaseNoTable4,
            HasApplicationError = hasApplicationError,
            HasDeviceError = hasDeviceError,
            IsEmpty = false
        };
    }
    
    /// <summary>
    /// Parses cut-off paths from 3 bytes (24 bits, but only 20 paths are used)
    /// </summary>
    private static IReadOnlyList<bool> ParseCutOffPaths(ReadOnlySpan<byte> span, int startOffset)
    {
        var paths = new List<bool>(20);
        
        for (int i = 0; i < 3; i++)
        {
            var byteValue = ReadWriteHelper.ReadUint8LittleEndian(span, startOffset + i);
            
            for (int j = 0; j < 8; j++)
            {
                // As long as there are only 20 instead of 24 cut-off paths
                if (i == 2 && j > 3)
                {
                    break;
                }
                
                paths.Add((byteValue & (0x01 << j)) != 0);
            }
        }
        
        return paths;
    }
    
    /// <summary>
    /// Checks if general system state block is published (enabled)
    /// </summary>
    private static bool CheckIfGeneralSystemStateIsPublished(DataHeader header)
    {
        return !(header.GeneralSystemStateBlockOffset == 0 && header.GeneralSystemStateBlockSize == 0);
    }
}