BQP/srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/ColaBHelper.cs

using System.Text;

namespace Sick.ColaB;

/// <summary>
/// Helper functions for ColaB protocol encoding/decoding
/// ColaB is a binary protocol used by older SICK scanners like TiM781s
/// </summary>
public static class ColaBHelper
{
    /// <summary>
    /// ColaB frame header: 0x02 0x02 0x02 0x02
    /// </summary>
    private static readonly byte[] FrameHeader = [0x02, 0x02, 0x02, 0x02];

    /// <summary>
    /// Creates a ColaB frame from command data
    /// Format: [STX STX STX STX] [Length (4 bytes BE)] ['s'] [Command Data] [Checksum (1 byte)]
    /// </summary>
    public static byte[] CreateFrame(ReadOnlySpan<byte> commandData)
    {
        // Calculate total payload length: length field (4) + 's' (1) + command data + checksum (1)
        // But length field itself contains: 's' (1) + command data length
        var payloadLength = 1 + commandData.Length; // 's' + command data
        
        // Total frame size: header (4) + length (4) + payload + checksum (1)
        var totalFrameSize = 4 + 4 + payloadLength + 1;
        var frame = new byte[totalFrameSize];
        var pos = 0;

        // Write header (STX STX STX STX)
        FrameHeader.CopyTo(frame, pos);
        pos += 4;

        // Write length (4 bytes, Big Endian) - this is the length of payload ('s' + command data)
        WriteUint32BigEndian(frame, pos, (uint)payloadLength);
        pos += 4;

        // Write 's' character
        frame[pos++] = (byte)'s';

        // Write command data
        commandData.CopyTo(frame.AsSpan(pos));
        pos += commandData.Length;

        // Calculate checksum (XOR of all bytes from 's' character to end of command data)
        // According to C++ code: checksum starts from byte 8 (the 's' character)
        byte checksum = frame[8]; // Start with 's' character (byte 8)
        for (int i = 9; i < pos; i++)
        {
            checksum ^= frame[i];
        }

        // Write checksum
        frame[pos++] = checksum;

        return frame;
    }

    /// <summary>
    /// Parses a ColaB frame and extracts the command data
    /// </summary>
    public static bool TryParseFrame(ReadOnlySpan<byte> frame, out ReadOnlySpan<byte> commandData, out int consumedBytes)
    {
        commandData = default;
        consumedBytes = 0;

        if (frame.Length < 9) // Minimum: header (4) + length (4) + 's' (1)
            return false;

        // Check header
        if (frame[0] != 0x02 || frame[1] != 0x02 || frame[2] != 0x02 || frame[3] != 0x02)
            return false;

        // Read length (Big Endian)
        var payloadLength = ReadUint32BigEndian(frame, 4);
        var payloadLengthInt = (int)payloadLength;
        
        // Check if we have enough data
        var totalFrameSize = 4 + 4 + payloadLengthInt + 1; // header + length + payload + checksum
        if (frame.Length < totalFrameSize)
            return false;

        // Check 's' character
        if (frame[8] != (byte)'s')
            return false;

        // Verify checksum (XOR of all bytes from 's' character to end of command data)
        // According to C++ code: checksum starts from byte 8 (the 's' character)
        byte checksum = frame[8]; // Start with 's' character (byte 8)
        for (int i = 9; i < 8 + payloadLengthInt; i++)
        {
            checksum ^= frame[i];
        }

        if (checksum != frame[8 + payloadLengthInt])
            return false; // Checksum mismatch

        // Extract command data (skip 's' character)
        commandData = frame.Slice(9, payloadLengthInt - 1);
        consumedBytes = totalFrameSize;

        return true;
    }

    /// <summary>
    /// Encodes a SOPAS command string to ColaB binary format
    /// Example: "sRN DeviceIdent" -> binary command data
    /// </summary>
    public static byte[] EncodeCommand(string command)
    {
        return Encoding.ASCII.GetBytes(command);
    }

    /// <summary>
    /// Decodes ColaB binary command data to SOPAS command string
    /// </summary>
    public static string DecodeCommand(ReadOnlySpan<byte> data)
    {
        return Encoding.ASCII.GetString(data);
    }

    /// <summary>
    /// Writes a 32-bit unsigned integer in Big Endian format
    /// </summary>
    private static void WriteUint32BigEndian(byte[] buffer, int offset, uint value)
    {
        if (offset + 3 < buffer.Length)
        {
            buffer[offset + 0] = (byte)((value & 0xff000000) >> 24);
            buffer[offset + 1] = (byte)((value & 0xff0000) >> 16);
            buffer[offset + 2] = (byte)((value & 0xff00) >> 8);
            buffer[offset + 3] = (byte)(value & 0xff);
        }
    }

    /// <summary>
    /// Reads a 32-bit unsigned integer in Big Endian format
    /// </summary>
    private static uint ReadUint32BigEndian(ReadOnlySpan<byte> buffer, int offset)
    {
        if (offset + 3 < buffer.Length)
        {
            return ((uint)buffer[offset + 0] << 24) +
                   ((uint)buffer[offset + 1] << 16) +
                   ((uint)buffer[offset + 2] << 8) +
                   buffer[offset + 3];
        }
        return 0;
    }

    /// <summary>
    /// Writes an integer value to buffer in Big Endian format
    /// </summary>
    public static void WriteIntegerBigEndian(byte[] buffer, ref int pos, uint value, int byteWidth)
    {
        for (int i = 0; i < byteWidth; i++)
        {
            if (pos < buffer.Length)
            {
                buffer[pos + byteWidth - 1 - i] = (byte)((value >> (8 * i)) & 0xff);
            }
        }
        pos += byteWidth;
    }

    /// <summary>
    /// Reads an integer value from buffer in Big Endian format
    /// </summary>
    public static uint ReadIntegerBigEndian(ReadOnlySpan<byte> buffer, ref int pos, int byteWidth)
    {
        uint value = 0;
        for (int i = 0; i < byteWidth; i++)
        {
            if (pos + byteWidth - 1 - i < buffer.Length)
            {
                value += (uint)buffer[pos + byteWidth - 1 - i] << (8 * i);
            }
        }
        pos += byteWidth;
        return value;
    }

    /// <summary>
    /// Writes a string to buffer
    /// </summary>
    public static void WriteString(byte[] buffer, ref int pos, string value)
    {
        var bytes = Encoding.ASCII.GetBytes(value);
        if (pos + bytes.Length <= buffer.Length)
        {
            bytes.CopyTo(buffer, pos);
            pos += bytes.Length;
        }
    }

    /// <summary>
    /// Reads a string from buffer
    /// </summary>
    public static string ReadString(ReadOnlySpan<byte> buffer, ref int pos, int length)
    {
        if (pos + length <= buffer.Length)
        {
            var result = Encoding.ASCII.GetString(buffer.Slice(pos, length));
            pos += length;
            return result;
        }
        return string.Empty;
    }
}