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

using Sick.SafetyScanners.DataStructures;
using Sick.SafetyScanners.Helpers;
using System.Net;

namespace Sick.SafetyScanners.DataProcessing;

/// <summary>
/// Parser for MeasurementPersistentConfigData response from COLA2 variable command
/// Thread-safe implementation
/// </summary>
public sealed class ParseMeasurementPersistentConfigData
{
    private const double AngleResolution = 4194304.0; // Sensor units per radian
    
    /// <summary>
    /// Parses the persistent measurement config from a TCP sequence (COLA2 response)
    /// Matches: ParseMeasurementPersistentConfigData::parseTCPSequence in C++
    /// </summary>
    public ConfigData ParseTcpSequence(PacketBuffer buffer)
    {
        var span = buffer.GetBufferSpan();
        var features = ReadFeatures(span);
        
        return new ConfigData
        {
            VersionCVersion = ReadVersionIndicator(span),
            VersionMajorVersionNumber = ReadMajorNumber(span),
            VersionMinorVersionNumber = ReadMinorNumber(span),
            VersionReleaseNumber = ReadReleaseNumber(span),
            Enabled = ReadEnabled(span),
            EInterfaceType = (InterfaceType)ReadInterfaceType(span),
            HostIp = ReadHostIp(span),
            HostUdpPort = ReadHostPort(span),
            PublishingFrequency = ReadPublishingFreq(span),
            StartAngle = ConvertToRadians(ReadStartAngle(span)),
            EndAngle = ConvertToRadians(ReadEndAngle(span)),
            Features = features,
            GeneralSystemStateEnabled = SensorDataFeatures.IsFlagSet(features, SensorDataFeatures.GeneralSystemState),
            DerivedSettingsEnabled = SensorDataFeatures.IsFlagSet(features, SensorDataFeatures.DerivedSettings),
            MeasurementDataEnabled = SensorDataFeatures.IsFlagSet(features, SensorDataFeatures.MeasurementData),
            IntrusionDataEnabled = SensorDataFeatures.IsFlagSet(features, SensorDataFeatures.IntrusionData),
            ApplicationDataEnabled = SensorDataFeatures.IsFlagSet(features, SensorDataFeatures.ApplicationData)
        };
    }
    
    /// <summary>
    /// Matches: ParseMeasurementPersistentConfigData::readVersionIndicator in C++
    /// </summary>
    private string ReadVersionIndicator(ReadOnlySpan<byte> span)
    {
        byte ch = ReadWriteHelper.ReadUint8(span, 0);
        return ((char)ch).ToString();
    }
    
    /// <summary>
    /// Matches: ParseMeasurementPersistentConfigData::readMajorNumber in C++
    /// </summary>
    private byte ReadMajorNumber(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 1);
    }
    
    /// <summary>
    /// Matches: ParseMeasurementPersistentConfigData::readMinorNumber in C++
    /// </summary>
    private byte ReadMinorNumber(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 2);
    }
    
    /// <summary>
    /// Matches: ParseMeasurementPersistentConfigData::readReleaseNumber in C++
    /// </summary>
    private byte ReadReleaseNumber(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 3);
    }
    
    /// <summary>
    /// Matches: ParseMeasurementPersistentConfigData::readEnabled in C++
    /// </summary>
    private bool ReadEnabled(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 4) != 0;
    }
    
    /// <summary>
    /// Matches: ParseMeasurementPersistentConfigData::readInterfaceType in C++
    /// </summary>
    private byte ReadInterfaceType(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 5);
    }
    
    private string ReadHostIp(ReadOnlySpan<byte> span)
    {
        uint word = ReadWriteHelper.ReadUint32LittleEndian(span, 8);
        // Convert uint32 (little endian from packet) to IP address bytes
        // IPAddress constructor expects bytes in network byte order (big endian)
        // word is little endian: [b0, b1, b2, b3] represents IP b0.b1.b2.b3
        // We need to extract bytes in order: b0, b1, b2, b3
        byte[] ipBytes = new byte[4];
        ipBytes[0] = (byte)(word & 0xFF);
        ipBytes[1] = (byte)((word >> 8) & 0xFF);
        ipBytes[2] = (byte)((word >> 16) & 0xFF);
        ipBytes[3] = (byte)((word >> 24) & 0xFF);
        var address = new IPAddress(ipBytes);
        return address.ToString();
    }
    
    private ushort ReadHostPort(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint16LittleEndian(span, 12);
    }
    
    private ushort ReadPublishingFreq(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint16LittleEndian(span, 14);
    }
    
    private uint ReadStartAngle(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint32LittleEndian(span, 16);
    }
    
    private uint ReadEndAngle(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint32LittleEndian(span, 20);
    }
    
    private ushort ReadFeatures(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint16LittleEndian(span, 24);
    }
    
    private double ConvertToRadians(uint sensorUnits)
    {
        return (sensorUnits / AngleResolution);
    }
}