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));
    }
}