Initial commit

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataProcessing/ParseFieldGeometryData.cs

@@ -0,0 +1,51 @@
+using Sick.SafetyScanners.DataStructures;
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.DataProcessing;
+
+/// <summary>
+/// Parser for FieldGeometryData response from COLA2 variable command
+/// Thread-safe implementation
+/// </summary>
+public sealed class ParseFieldGeometryData
+{
+    private const double StartAngleDegrees = -47.5; // Defined start angle in degrees in SICK coordinates
+    
+    /// <summary>
+    /// Parses the field geometry data from a TCP sequence (COLA2 response)
+    /// Matches: ParseFieldGeometryData::parseTCPSequence in C++
+    /// </summary>
+    public FieldData ParseTcpSequence(PacketBuffer buffer)
+    {
+        var span = buffer.GetBufferSpan();
+        uint arrayLength = ReadArrayLength(span);
+        
+        var geometryDistance = new List<ushort>((int)arrayLength);
+        for (uint i = 0; i < arrayLength; i++)
+        {
+            geometryDistance.Add(ReadArrayElement(span, i));
+        }
+        
+        // Values are persistent for scanners
+        double res = (275.0 / arrayLength);
+        
+        return new FieldData
+        {
+            IsValid = true,
+            BeamDistances = geometryDistance,
+            StartAngle = (StartAngleDegrees * Math.PI / 180.0),
+            AngularBeamResolution = (res * Math.PI / 180.0)
+        };
+    }
+    
+    private uint ReadArrayLength(ReadOnlySpan<byte> span)
+    {
+        return ReadWriteHelper.ReadUint32LittleEndian(span, 4);
+    }
+    
+    private ushort ReadArrayElement(ReadOnlySpan<byte> span, uint elemNumber)
+    {
+        return ReadWriteHelper.ReadUint16LittleEndian(span, 8 + (int)(elemNumber * 2));
+    }
+}
+