BQP/srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Helpers/ReadWriteHelper.cs

namespace Sick.SafetyScanners.Helpers;

/// <summary>
/// Helper functions for reading and writing data with Big/Little Endian support
/// Matches the logic from C++ ReadWriteHelper.hpp exactly
/// Pure implementation without external dependencies
/// </summary>
public static class ReadWriteHelper
{
    /// <summary>
    /// Safely gets a byte from ReadOnlySpan, returns 0 if out of range
    /// </summary>
    private static byte GetByte(ReadOnlySpan<byte> span, int offset)
    {
        return (offset < 0 || offset >= span.Length) ? (byte)0 : span[offset];
    }
    
    /// <summary>
    /// Safely gets a byte from Span, returns 0 if out of range
    /// </summary>
    private static byte GetByte(Span<byte> span, int offset)
    {
        return (offset < 0 || offset >= span.Length) ? (byte)0 : span[offset];
    }

    #region Write Operations - No Endian (8-bit only)
    
    /// <summary>
    /// Writes an unsigned 8-bit integer to a buffer at offset (no endianness for 1 byte)
    /// Matches: writeUint8 in C++ - *(it + 0) = v;
    /// </summary>
    public static void WriteUint8(Span<byte> span, int offset, byte value)
    {
        if (offset >= 0 && offset < span.Length)
        {
            span[offset] = value;
        }
    }
    
    /// <summary>
    /// Writes a signed 8-bit integer to a buffer at offset (no endianness for 1 byte)
    /// Matches: writeInt8 in C++ - writeUint8(it, v);
    /// </summary>
    public static void WriteInt8(Span<byte> span, int offset, sbyte value)
    {
        WriteUint8(span, offset, unchecked((byte)value));
    }
    
    #endregion
    
    #region Write Operations - Big Endian
    
    /// <summary>
    /// Writes an unsigned 8-bit integer in big endian encoding
    /// Matches: writeUint8BigEndian in C++ - writeUint8(it, v);
    /// </summary>
    public static void WriteUint8BigEndian(Span<byte> span, int offset, byte value)
    {
        WriteUint8(span, offset, value);
    }
    
    /// <summary>
    /// Writes a signed 8-bit integer in big endian encoding
    /// Matches: writeInt8BigEndian in C++ - writeInt8(it, v);
    /// </summary>
    public static void WriteInt8BigEndian(Span<byte> span, int offset, sbyte value)
    {
        WriteInt8(span, offset, value);
    }
    
    /// <summary>
    /// Writes an unsigned 16-bit integer in big endian encoding
    /// Matches: writeUint16BigEndian in C++ - *(it + 0) = (v & 0xff00) >> 8; *(it + 1) = v & 0xff;
    /// </summary>
    public static void WriteUint16BigEndian(Span<byte> span, int offset, ushort value)
    {
        if (offset >= 0 && offset + 1 < span.Length)
        {
            span[offset + 0] = (byte)((value & 0xff00) >> 8);
            span[offset + 1] = (byte)(value & 0xff);
        }
    }
    
    /// <summary>
    /// Writes an unsigned 32-bit integer in big endian encoding
    /// Matches: writeUint32BigEndian in C++ - *(it + 0) = (v & 0xff000000) >> 24; ...
    /// </summary>
    public static void WriteUint32BigEndian(Span<byte> span, int offset, uint value)
    {
        if (offset >= 0 && offset + 3 < span.Length)
        {
            span[offset + 0] = (byte)((value & 0xff000000) >> 24);
            span[offset + 1] = (byte)((value & 0xff0000) >> 16);
            span[offset + 2] = (byte)((value & 0xff00) >> 8);
            span[offset + 3] = (byte)(value & 0xff);
        }
    }
    
    /// <summary>
    /// Writes a signed 16-bit integer in big endian encoding
    /// Matches: readInt16BigEndian calls readUint16BigEndian in C++
    /// </summary>
    public static void WriteInt16BigEndian(Span<byte> span, int offset, short value)
    {
        WriteUint16BigEndian(span, offset, unchecked((ushort)value));
    }
    
    /// <summary>
    /// Writes a signed 32-bit integer in big endian encoding
    /// Matches: readInt32BigEndian calls readUint32BigEndian in C++
    /// </summary>
    public static void WriteInt32BigEndian(Span<byte> span, int offset, int value)
    {
        WriteUint32BigEndian(span, offset, unchecked((uint)value));
    }
    
    #endregion
    
    #region Write Operations - Little Endian
    
    /// <summary>
    /// Writes an unsigned 8-bit integer in little endian encoding
    /// Matches: writeUint8LittleEndian in C++ - writeUint8(it, v);
    /// </summary>
    public static void WriteUint8LittleEndian(Span<byte> span, int offset, byte value)
    {
        WriteUint8(span, offset, value);
    }
    
    /// <summary>
    /// Writes a signed 8-bit integer in little endian encoding
    /// Matches: writeInt8LittleEndian in C++ - writeInt8(it, v);
    /// </summary>
    public static void WriteInt8LittleEndian(Span<byte> span, int offset, sbyte value)
    {
        WriteInt8(span, offset, value);
    }
    
    /// <summary>
    /// Writes an unsigned 16-bit integer in little endian encoding
    /// Matches: writeUint16LittleEndian in C++ - *(it + 0) = v & 0xff; *(it + 1) = (v & 0xff00) >> 8;
    /// </summary>
    public static void WriteUint16LittleEndian(Span<byte> span, int offset, ushort value)
    {
        if (offset >= 0 && offset + 1 < span.Length)
        {
            span[offset + 0] = (byte)(value & 0xff);
            span[offset + 1] = (byte)((value & 0xff00) >> 8);
        }
    }
    
    /// <summary>
    /// Writes an unsigned 32-bit integer in little endian encoding
    /// Matches: writeUint32LittleEndian in C++ - *(it + 3) = (v & 0xff000000) >> 24; ...
    /// </summary>
    public static void WriteUint32LittleEndian(Span<byte> span, int offset, uint value)
    {
        if (offset >= 0 && offset + 3 < span.Length)
        {
            span[offset + 3] = (byte)((value & 0xff000000) >> 24);
            span[offset + 2] = (byte)((value & 0xff0000) >> 16);
            span[offset + 1] = (byte)((value & 0xff00) >> 8);
            span[offset + 0] = (byte)(value & 0xff);
        }
    }
    
    /// <summary>
    /// Writes a signed 16-bit integer in little endian encoding
    /// Matches: readInt16LittleEndian calls readUint16LittleEndian in C++
    /// </summary>
    public static void WriteInt16LittleEndian(Span<byte> span, int offset, short value)
    {
        WriteUint16LittleEndian(span, offset, unchecked((ushort)value));
    }
    
    /// <summary>
    /// Writes a signed 32-bit integer in little endian encoding
    /// Matches: writeInt32LittleEndian in C++ - *(it + 3) = (v & 0xff000000) >> 24; ...
    /// </summary>
    public static void WriteInt32LittleEndian(Span<byte> span, int offset, int value)
    {
        if (offset >= 0 && offset + 3 < span.Length)
        {
            span[offset + 3] = (byte)((unchecked((uint)value) & 0xff000000) >> 24);
            span[offset + 2] = (byte)((unchecked((uint)value) & 0xff0000) >> 16);
            span[offset + 1] = (byte)((unchecked((uint)value) & 0xff00) >> 8);
            span[offset + 0] = (byte)(unchecked((uint)value) & 0xff);
        }
    }
    
    #endregion
    
    #region Read Operations - No Endian (8-bit only)
    
    /// <summary>
    /// Reads an unsigned 8-bit integer at offset (no endianness for 1 byte)
    /// Matches: readUint8 in C++ - return *(it + 0);
    /// </summary>
    public static byte ReadUint8(ReadOnlySpan<byte> span, int offset)
    {
        return GetByte(span, offset);
    }
    
    /// <summary>
    /// Reads a signed 8-bit integer at offset (no endianness for 1 byte)
    /// Matches: readInt8 in C++ - return readUint8(it);
    /// </summary>
    public static sbyte ReadInt8(ReadOnlySpan<byte> span, int offset)
    {
        return unchecked((sbyte)ReadUint8(span, offset));
    }
    
    #endregion
    
    #region Read Operations - Big Endian
    
    /// <summary>
    /// Reads an unsigned 8-bit integer in big endian encoding
    /// Matches: readUint8BigEndian in C++ - return readUint8(it);
    /// </summary>
    public static byte ReadUint8BigEndian(ReadOnlySpan<byte> span, int offset)
    {
        return ReadUint8(span, offset);
    }
    
    /// <summary>
    /// Reads a signed 8-bit integer in big endian encoding
    /// Matches: readInt8BigEndian in C++ - return readInt8(it);
    /// </summary>
    public static sbyte ReadInt8BigEndian(ReadOnlySpan<byte> span, int offset)
    {
        return ReadInt8(span, offset);
    }
    
    /// <summary>
    /// Reads an unsigned 16-bit integer in big endian encoding
    /// Matches: readUint16BigEndian in C++ - return (*(it + 0) << 8) + *(it + 1);
    /// </summary>
    public static ushort ReadUint16BigEndian(ReadOnlySpan<byte> span, int offset)
    {
        return (ushort)((GetByte(span, offset + 0) << 8) + GetByte(span, offset + 1));
    }
    
    /// <summary>
    /// Reads an unsigned 32-bit integer in big endian encoding
    /// Matches: readUint32BigEndian in C++ - return (*(it + 0) << 24) + (*(it + 1) << 16) + (*(it + 2) << 8) + *(it + 3);
    /// </summary>
    public static uint ReadUint32BigEndian(ReadOnlySpan<byte> span, int offset)
    {
        return ((uint)GetByte(span, offset + 0) << 24) + ((uint)GetByte(span, offset + 1) << 16) + ((uint)GetByte(span, offset + 2) << 8) + GetByte(span, offset + 3);
    }
    
    /// <summary>
    /// Reads a signed 16-bit integer in big endian encoding
    /// Matches: readInt16BigEndian in C++ - return readUint16BigEndian(it);
    /// </summary>
    public static short ReadInt16BigEndian(ReadOnlySpan<byte> span, int offset)
    {
        return unchecked((short)ReadUint16BigEndian(span, offset));
    }
    
    /// <summary>
    /// Reads a signed 32-bit integer in big endian encoding
    /// Matches: readInt32BigEndian in C++ - return readUint32BigEndian(it);
    /// </summary>
    public static int ReadInt32BigEndian(ReadOnlySpan<byte> span, int offset)
    {
        return unchecked((int)ReadUint32BigEndian(span, offset));
    }
    
    #endregion
    
    #region Read Operations - Little Endian
    
    /// <summary>
    /// Reads an unsigned 8-bit integer in little endian encoding
    /// Matches: readUint8LittleEndian in C++ - return readUint8(it);
    /// </summary>
    public static byte ReadUint8LittleEndian(ReadOnlySpan<byte> span, int offset)
    {
        return ReadUint8(span, offset);
    }
    
    /// <summary>
    /// Reads a signed 8-bit integer in little endian encoding
    /// Matches: readInt8LittleEndian in C++ - return readInt8(it);
    /// </summary>
    public static sbyte ReadInt8LittleEndian(ReadOnlySpan<byte> span, int offset)
    {
        return ReadInt8(span, offset);
    }
    
    /// <summary>
    /// Reads an unsigned 16-bit integer in little endian encoding
    /// Matches: readUint16LittleEndian in C++ - return (*(it + 1) << 8) + *(it + 0);
    /// </summary>
    public static ushort ReadUint16LittleEndian(ReadOnlySpan<byte> span, int offset)
    {
        return (ushort)((GetByte(span, offset + 1) << 8) + GetByte(span, offset + 0));
    }
    
    /// <summary>
    /// Reads an unsigned 32-bit integer in little endian encoding
    /// Matches: readUint32LittleEndian in C++ - return (*(it + 3) << 24) + (*(it + 2) << 16) + (*(it + 1) << 8) + *(it + 0);
    /// </summary>
    public static uint ReadUint32LittleEndian(ReadOnlySpan<byte> span, int offset)
    {
        return ((uint)GetByte(span, offset + 3) << 24) + ((uint)GetByte(span, offset + 2) << 16) + ((uint)GetByte(span, offset + 1) << 8) + GetByte(span, offset + 0);
    }
    
    /// <summary>
    /// Reads a signed 16-bit integer in little endian encoding
    /// Matches: readInt16LittleEndian in C++ - return readUint16LittleEndian(it);
    /// </summary>
    public static short ReadInt16LittleEndian(ReadOnlySpan<byte> span, int offset)
    {
        return unchecked((short)ReadUint16LittleEndian(span, offset));
    }
    
    /// <summary>
    /// Reads a signed 32-bit integer in little endian encoding
    /// Matches: readInt32LittleEndian in C++ - return readUint32LittleEndian(it);
    /// </summary>
    public static int ReadInt32LittleEndian(ReadOnlySpan<byte> span, int offset)
    {
        return unchecked((int)ReadUint32LittleEndian(span, offset));
    }
    
    #endregion
}