I150/srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataProcessing/ParseMeasurementCurrentConfigData.cs

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

namespace Sick.SafetyScanners.DataProcessing;

/// <summary>
/// Parser for MeasurementCurrentConfigData response from COLA2 variable command
/// Thread-safe implementation
/// </summary>
public sealed class ParseMeasurementCurrentConfigData
{
    private const double AngleResolution = 4194304.0; // Sensor units per radian
    
    /// <summary>
    /// Parses the current measurement config from a TCP sequence (COLA2 response)
    /// Matches: ParseMeasurementCurrentConfigData::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),
            DerivedMultiplicationFactor = ReadDerivedMultiplicationFactor(span),
            DerivedNumberOfBeams = ReadDerivedNumBeams(span),
            DerivedScanTime = ReadDerivedScanTime(span),
            DerivedStartAngle = ConvertToRadians(ReadDerivedStartAngle(span)),
            DerivedAngularBeamResolution = ConvertToRadians(ReadDerivedAngularBeamResolution(span)),
            DerivedInterbeamPeriod = ReadDerivedInterbeamPeriod(span)
        };
    }
    
    /// <summary>
    /// Matches: ParseMeasurementCurrentConfigData::readVersionIndicator in C++
    /// </summary>
    private string ReadVersionIndicator(ReadOnlySpan<byte> span)
    {
        byte ch = ReadWriteHelper.ReadUint8(span, 0);
        return ((char)ch).ToString();
    }
    
    /// <summary>
    /// Matches: ParseMeasurementCurrentConfigData::readMajorNumber in C++
    /// </summary>
    private byte ReadMajorNumber(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 1);
    }
    
    /// <summary>
    /// Matches: ParseMeasurementCurrentConfigData::readMinorNumber in C++
    /// </summary>
    private byte ReadMinorNumber(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 2);
    }
    
    /// <summary>
    /// Matches: ParseMeasurementCurrentConfigData::readReleaseNumber in C++
    /// </summary>
    private byte ReadReleaseNumber(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 3);
    }
    
    /// <summary>
    /// Matches: ParseMeasurementCurrentConfigData::readEnabled in C++
    /// </summary>
    private bool ReadEnabled(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 4) != 0;
    }
    
    /// <summary>
    /// Matches: ParseMeasurementCurrentConfigData::readInterfaceType in C++
    /// </summary>
    private byte ReadInterfaceType(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint8(span, 5);
    }
    
    private string ReadHostIp(ReadOnlySpan<byte> span)
    {
        // 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
        var address = new IPAddress([span[11], span[10], span[9], span[8]]);
        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 ushort ReadDerivedMultiplicationFactor(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint16LittleEndian(span, 28);
    }
    
    private ushort ReadDerivedNumBeams(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint16LittleEndian(span, 30);
    }
    
    private ushort ReadDerivedScanTime(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint16LittleEndian(span, 32);
    }
    
    private uint ReadDerivedStartAngle(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint32LittleEndian(span, 36);
    }
    
    private uint ReadDerivedAngularBeamResolution(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint32LittleEndian(span, 40);
    }
    
    private uint ReadDerivedInterbeamPeriod(ReadOnlySpan<byte> span)
    {
        return ReadWriteHelper.ReadUint32LittleEndian(span, 44);
    }
    
    /// <summary>
    /// Converts sensor units to radians
    /// According to COLA2 documentation: "This value, divided by 4194304, equals the actual start angle"
    /// According to C++ reference: value / 4194304.0 gives degrees (same as setDerivedAngularBeamResolutionDegrees)
    /// So we convert: sensorUnits / 4194304.0 = degrees, then degrees * π/180 = radians
    /// </summary>
    private double ConvertToRadians(uint sensorUnits)
    {
        var degrees = (sensorUnits / AngleResolution);
        return (degrees * Math.PI / 180.0);
    }
}