Initial commit

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

@@ -0,0 +1,201 @@
+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;
+    }
+}
+