commit 285c6fe50ba5320f29ee229bdc894fec175302cb
Author: Oto Šťáva <ostava@students.zcu.cz>
Date:   Sun May 23 20:31:23 2021 +0000

    Re #8899 - Server - Implement variable-length message reception

diff --git a/deltarobot-vr/Assets/DeltaRobotVr/Client.cs b/deltarobot-vr/Assets/DeltaRobotVr/Client.cs
index 7dcfdd2..76f8889 100644
--- a/deltarobot-vr/Assets/DeltaRobotVr/Client.cs
+++ b/deltarobot-vr/Assets/DeltaRobotVr/Client.cs
@@ -265,6 +265,11 @@ namespace DeltaRobotVr
                     {
                         ReadMessages(reader, writer);
                     }
+
+                    if (client.Connected)
+                    {
+                        SendEot(writer, "Client stopped by user");
+                    }
                 }
                 catch (Exception e)
                 {
@@ -276,10 +281,10 @@ namespace DeltaRobotVr
                     {
                         Debug.LogError($"Exception in communication thread:\n{e}");
                     }
-                }
-                finally
-                {
-                    IsConnected = false;
+                }
+                finally
+                {
+                    IsConnected = false;
                 }
                 
                 // wait before reconnection - short polls to prevent blocking if application is closed
@@ -473,5 +478,12 @@ namespace DeltaRobotVr
             writer.Write(PongId);
             writer.Write(pingValue);
         }
+
+        private void SendEot(BinaryWriter writer, string reason)
+        {
+            writer.Write(EotId);
+            writer.Write((UInt32) reason.Length);
+            writer.Write(Encoding.ASCII.GetBytes(reason));
+        }
     }
 }
\ No newline at end of file
diff --git a/deltarobot/curvedataserver.cpp b/deltarobot/curvedataserver.cpp
index 127bc7c..b319547 100644
--- a/deltarobot/curvedataserver.cpp
+++ b/deltarobot/curvedataserver.cpp
@@ -5,6 +5,8 @@
 #include <QAbstractSocket>
 #include <QVector4D>
 #include <chrono>
+#include <string>
+#include <cstring>
 
 #include "../element/trajectory.hpp"
 
@@ -173,9 +175,12 @@ void CurveDataServer::onReadyRead()
     Client& client = *it;
 
     while (sender->bytesAvailable()) {
+        // read the message ID
         if (client.bufUsed < Client::MESSAGE_ID_SIZE) {
             qint64 toRead = qMin(qint64(Client::MESSAGE_ID_SIZE - client.bufUsed), sender->bytesAvailable());
-            client.bufUsed += sender->read(&client.buf[client.bufUsed], toRead);
+            if (toRead > 0) {
+                client.bufUsed += sender->read(&client.buf[client.bufUsed], toRead);
+            }
         }
 
         if (client.bufUsed < 2) {
@@ -184,6 +189,7 @@ void CurveDataServer::onReadyRead()
         }
 
         uint16_t messageId = *((uint16_t*) client.buf);
+        bool variableLength = false;
         qint64 contentSize;
         switch (messageId & 0xF000) {
         case 0x0000:
@@ -202,7 +208,7 @@ void CurveDataServer::onReadyRead()
             contentSize = 16;
             break;
         case 0xF000:
-            // TODO - implement support for variable length messages
+            variableLength = true;
             contentSize = 0;
             break;
         default:
@@ -210,19 +216,56 @@ void CurveDataServer::onReadyRead()
             return;
         }
 
-        qint64 toRead = qMin(qint64(contentSize - (client.bufUsed - Client::MESSAGE_ID_SIZE)), sender->bytesAvailable());
+        size_t contentOffset = Client::MESSAGE_ID_SIZE;
+
+        if (variableLength) {
+            // read the message's content length
+            qint64 toRead = qMin(
+                        qint64(Client::MESSAGE_LENGTH_SIZE - (client.bufUsed - Client::MESSAGE_ID_SIZE)),
+                        sender->bytesAvailable());
+            if (toRead > 0) {
+                client.bufUsed += sender->read(&client.buf[client.bufUsed], toRead);
+            }
+
+            if (client.bufUsed < (Client::MESSAGE_ID_SIZE + Client::MESSAGE_LENGTH_SIZE)) {
+                // we do not yet know the content size, do not proceed any further
+                return;
+            }
+
+            contentSize = qint64(*((uint32_t*) &client.buf[Client::MESSAGE_ID_SIZE]));
+            contentOffset = Client::MESSAGE_ID_SIZE + Client::MESSAGE_LENGTH_SIZE;
+        }
+
+        // read message content
+        qint64 toRead = qMin(
+                    qint64(contentSize - (client.bufUsed - contentOffset)),
+                    sender->bytesAvailable());
         if (toRead > 0) {
-            client.bufUsed += sender->read(&client.buf[client.bufUsed], toRead);
+            qint64 bufRemaining = qint64(Client::BUF_SIZE - client.bufUsed);
+            if (toRead > bufRemaining) {
+                // message would overflow the buffer - we'll read whatever fits and skip the rest
+                if (bufRemaining > 0) {
+                    qint64 actuallyRead = sender->read(&client.buf[client.bufUsed], bufRemaining);
+                    client.bufUsed += actuallyRead;
+                    toRead -= actuallyRead;
+                }
+
+                if (toRead > 0) {
+                    sender->skip(toRead);
+                }
+            } else {
+                client.bufUsed += sender->read(&client.buf[client.bufUsed], toRead);
+            }
         }
 
-        if (client.bufUsed == (Client::MESSAGE_ID_SIZE + contentSize)) {
-            handleMessage(client, messageId, &client.buf[Client::MESSAGE_ID_SIZE]);
+        if (client.bufUsed == (contentOffset + contentSize)) {
+            handleMessage(client, messageId, &client.buf[contentOffset], contentSize);
             client.bufUsed = 0;
         }
     }
 }
 
-void CurveDataServer::handleMessage(Client& client, uint16_t messageId, void *content) {
+void CurveDataServer::handleMessage(Client& client, uint16_t messageId, void *content, size_t contentSize) {
     switch (messageId) {
     case 0x3001:
         qDebug() << "Magic arrived!";
@@ -238,7 +281,7 @@ void CurveDataServer::handleMessage(Client& client, uint16_t messageId, void *co
         break;
     case 0xF006:
         qDebug() << "EOT!";
-        handleEOT(client, (char *) content);
+        handleEOT(client, (char *) content, contentSize);
         break;
     }
 }
@@ -276,9 +319,13 @@ void CurveDataServer::handlePong(Client& client, uint64_t* content)
     //qDebug() << "prisel pong" << number;
 }
 
-void CurveDataServer::handleEOT(Client& client, char* content)
+void CurveDataServer::handleEOT(Client& client, char* content, size_t contentSize)
 {
-    qDebug() << "Client" << client.socket->peerName() << "disconnected:" << content;
+    char cstrContent[contentSize + 1];
+    std::memcpy(cstrContent, content, contentSize);
+    cstrContent[contentSize] = '\0';
+
+    qDebug() << "Client" << client.socket->peerName() << "disconnected:" << cstrContent;
     terminateConnection(client, "Recieved EOT from client");
 }
 
diff --git a/deltarobot/curvedataserver.h b/deltarobot/curvedataserver.h
index 68c7efe..e530da1 100644
--- a/deltarobot/curvedataserver.h
+++ b/deltarobot/curvedataserver.h
@@ -14,8 +14,15 @@ class CurveDataServer : QObject
 
 public:
     struct Client {
+        /**
+         * Size of the client buffer.
+         *
+         * @note Current implementation truncates any variable-length messages longer than what would fit in the buffer.
+         */
         static const size_t BUF_SIZE = 4096;
-        static const size_t MESSAGE_ID_SIZE = 2;
+
+        static const size_t MESSAGE_ID_SIZE = sizeof(uint16_t);      ///< Size of the protocol message identifier.
+        static const size_t MESSAGE_LENGTH_SIZE = sizeof(uint32_t);  ///< Size of the variable-length message's content length.
 
         QTcpSocket* socket;
         std::map<uint64_t, uint64_t> ping;  ///< key: ping ID;  value: ping timestamp (msec);
@@ -52,11 +59,11 @@ public slots:
 private:
     QList<Client>::iterator clientOf(QTcpSocket *socket);
 
-    void handleMessage(Client& client, uint16_t messageId, void* content);
+    void handleMessage(Client& client, uint16_t messageId, void* content, size_t contentSize);
     void handleProtocolMagic(Client& client, char* content);
     void handleVersion(Client& client, uint8_t content);
     void handlePong(Client& client, uint64_t* content);
-    void handleEOT(Client& client, char* content);
+    void handleEOT(Client& client, char* content, size_t contentSize);
 
     void validPreamble(Client& client);
     void removeSocket(QTcpSocket *socket);
