BQP/srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/CommandBase.cs

using Sick.SafetyScanners.Cola2.Commands;
using Sick.SafetyScanners.Helpers;

namespace Sick.SafetyScanners.Cola2.Commands;

/// <summary>
/// Base class for all COLA2 commands
/// Thread-safe implementation
/// </summary>
public abstract class CommandBase : ICola2Command
{
    private readonly byte _commandType;
    private readonly byte _commandMode;
    private uint _sessionId;
    private ushort _requestId;
    private bool _wasSuccessful;
    private readonly List<byte> _dataVector;
    private readonly object _lock = new();
    
    protected CommandBase(byte commandType, byte commandMode)
    {
        _commandType = commandType;
        _commandMode = commandMode;
        _dataVector = new List<byte>();
    }
    
    public byte CommandType => _commandType;
    public byte CommandMode => _commandMode;
    
    public uint SessionId
    {
        get
        {
            lock (_lock)
            {
                return _sessionId;
            }
        }
        set
        {
            lock (_lock)
            {
                _sessionId = value;
            }
        }
    }
    
    public ushort RequestId
    {
        get
        {
            lock (_lock)
            {
                return _requestId;
            }
        }
        set
        {
            lock (_lock)
            {
                _requestId = value;
            }
        }
    }
    
    public bool WasSuccessful
    {
        get
        {
            lock (_lock)
            {
                return _wasSuccessful;
            }
        }
        protected set
        {
            lock (_lock)
            {
                _wasSuccessful = value;
            }
        }
    }
    
    public abstract bool CanBeExecutedWithoutSessionId { get; }
    
    /// <summary>
    /// Gets the data vector for the command payload
    /// </summary>
    public ReadOnlyMemory<byte> GetDataVector()
    {
        lock (_lock)
        {
            return _dataVector.ToArray();
        }
    }
    
    /// <summary>
    /// Sets the data vector
    /// </summary>
    protected void SetDataVector(ReadOnlyMemory<byte> data)
    {
        lock (_lock)
        {
            _dataVector.Clear();
            _dataVector.AddRange(data.ToArray());
        }
    }
    
    /// <summary>
    /// Adds data to the data vector
    /// </summary>
    protected void AddData(ReadOnlyMemory<byte> data)
    {
        lock (_lock)
        {
            _dataVector.AddRange(data.ToArray());
        }
    }
    
    /// <summary>
    /// Constructs the complete telegram including header
    /// </summary>
    public byte[] ConstructTelegram()
    {
        lock (_lock)
        {
            var data = AddTelegramData();
            return AddTelegramHeader(data);
        }
    }
    
    /// <summary>
    /// Adds command-specific data to the telegram
    /// </summary>
    protected abstract ReadOnlyMemory<byte> AddTelegramData();
    
    /// <summary>
    /// Processes the reply from the sensor
    /// In C++ reference, ParseTCPPacket::parseTCPSequence calls command.setDataVector(byte_vector)
    /// to store the reply data in the command. We need to do the same here.
    /// </summary>
    public bool ProcessReply(ReadOnlyMemory<byte> replyData, byte replyCommandType, byte replyCommandMode)
    {
        lock (_lock)
        {
            // Store reply data in _dataVector (matching C++ reference behavior)
            // In C++: command.setDataVector(byte_vector) is called by ParseTCPPacket
            SetDataVector(replyData);
            
            _wasSuccessful = ProcessReplyInternal(replyData, replyCommandType, replyCommandMode);
            return _wasSuccessful;
        }
    }
    
    /// <summary>
    /// Internal method to process reply (implemented by derived classes)
    /// </summary>
    protected abstract bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
        byte replyCommandType, byte replyCommandMode);
    
    /// <summary>
    /// Adds the COLA2 header to the telegram
    /// </summary>
    private byte[] AddTelegramHeader(ReadOnlyMemory<byte> data)
    {
        const int headerSize = 18;
        var totalLength = headerSize + data.Length;
        var telegram = new byte[totalLength];
        var span = telegram.AsSpan();
        
        // STX (4 bytes): 0x02020202
        ReadWriteHelper.WriteUint32BigEndian(span, 0, 0x02020202);
        
        // Length (4 bytes): 10 + data.Length
        ReadWriteHelper.WriteUint32BigEndian(span, 4, (uint)(10 + data.Length));
        
        // HubCntr (1 byte): 0x00
        ReadWriteHelper.WriteUint8BigEndian(span, 8, 0x00);
        
        // NoC (1 byte): 0x00
        ReadWriteHelper.WriteUint8BigEndian(span, 9, 0x00);
        
        // Session ID (4 bytes)
        ReadWriteHelper.WriteUint32BigEndian(span, 10, _sessionId);
        
        // Request ID (2 bytes)
        ReadWriteHelper.WriteUint16BigEndian(span, 14, _requestId);
        
        // Command Type (1 byte)
        ReadWriteHelper.WriteUint8BigEndian(span, 16, _commandType);
        
        // Command Mode (1 byte)
        ReadWriteHelper.WriteUint8BigEndian(span, 17, _commandMode);
        
        // Copy data
        data.CopyTo(telegram.AsMemory(headerSize));
        
        return telegram;
    }
}