commit 6d9543aae46eb5a5c24abfc0eef8e0aca904c01d
Author: Jakub Hejman <hejmanj@students.zcu.cz>
Date:   Tue May 11 19:13:41 2021 +0000

    Re #8899 - Server data reception

diff --git a/deltarobot-vr/Assets/DeltaRobotVr/Client.cs b/deltarobot-vr/Assets/DeltaRobotVr/Client.cs
index 73f583e..0851fdb 100644
--- a/deltarobot-vr/Assets/DeltaRobotVr/Client.cs
+++ b/deltarobot-vr/Assets/DeltaRobotVr/Client.cs
@@ -36,6 +36,7 @@ namespace DeltaRobotVr
         private const UInt16 ProtocolMagicId = 0x3001;
         private const UInt16 ProtocolVersionId = 0x2002;
         private const UInt16 PingId = 0x3004;
+        private const UInt16 PongId = 0x3005;
         private const UInt16 EotId = 0xF006;
         private const UInt16 CurrentActuatorPositionId = 0x4003;
         private const UInt16 CurrentDirectionVectorId = 0x4007;
@@ -416,6 +417,8 @@ namespace DeltaRobotVr
         private void ProcessPing(BinaryReader reader, BinaryWriter writer)
         {
             UInt64 pingValue = reader.ReadUInt64();
+
+            writer.Write(PongId);
             writer.Write(pingValue);
         }
     }
diff --git a/deltarobot/curvedataserver.cpp b/deltarobot/curvedataserver.cpp
index ebcb255..127bc7c 100644
--- a/deltarobot/curvedataserver.cpp
+++ b/deltarobot/curvedataserver.cpp
@@ -6,6 +6,7 @@
 #include <QVector4D>
 #include <chrono>
 
+#include "../element/trajectory.hpp"
 
 // Utility functions ///////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -101,7 +102,7 @@ static QByteArray messageVarLength(uint16_t messageId, uint32_t length, const ch
     return result;
 }
 
-inline long currentTimeMillis() {
+inline uint64_t currentTimeMillis() {
     return std::chrono::duration_cast<std::chrono::milliseconds>(
                 std::chrono::steady_clock::now().time_since_epoch())
             .count();
@@ -121,6 +122,7 @@ CurveDataServer::Client::Client(QTcpSocket* socket) :
 
 CurveDataServer::CurveDataServer() :
     _server(this),
+    _pingThread([this](){threadFunction();}),
     _sockets()
 {
     _server.listen(QHostAddress::Any, 4242);
@@ -132,10 +134,11 @@ CurveDataServer::~CurveDataServer()
     std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
     for (auto& client : _sockets)
     {
-        sendEot(client.socket, "Server is stopping");
-        client.socket->close();
+        terminateConnection(client, "Server is stopping");
     }
     _sockets.clear();
+
+    _threadIsRunning = false;
 }
 
 void CurveDataServer::onNewConnection()
@@ -154,13 +157,7 @@ void CurveDataServer::onSocketStateChanged(QAbstractSocket::SocketState socketSt
     {
         QTcpSocket* sender = static_cast<QTcpSocket*>(QObject::sender());
 
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto it = _sockets.begin(); it != _sockets.end(); it++) {
-            if (it->socket == sender) {
-                _sockets.erase(it);
-                break;
-            }
-        }
+        removeSocket(sender);
     }
 }
 
@@ -206,7 +203,8 @@ void CurveDataServer::onReadyRead()
             break;
         case 0xF000:
             // TODO - implement support for variable length messages
-            return;
+            contentSize = 0;
+            break;
         default:
             // unsupported message size
             return;
@@ -228,16 +226,75 @@ void CurveDataServer::handleMessage(Client& client, uint16_t messageId, void *co
     switch (messageId) {
     case 0x3001:
         qDebug() << "Magic arrived!";
+        handleProtocolMagic(client, (char *) content);
         break;
     case 0x2002:
         qDebug() << "Version arrived!";
+        handleVersion(client, *((uint8_t*) content));
         break;
     case 0x3005:
         qDebug() << "Pong " << *((uint64_t*) content) << " arrived!";
+        handlePong(client, (uint64_t*) content);
         break;
+    case 0xF006:
+        qDebug() << "EOT!";
+        handleEOT(client, (char *) content);
+        break;
+    }
+}
+
+void CurveDataServer::handleProtocolMagic(Client& client, char *content)
+{
+    char *correct = "DeltaRVr";
+
+    if (strncmp(content, correct, 8) == 0) {
+        client.magic = true;
     }
 }
 
+void CurveDataServer::handleVersion(Client& client, uint8_t content)
+{
+    if (((uint8_t*) &content)[0] == 1)
+    {
+        client.version = true;
+    }
+}
+
+void CurveDataServer::handlePong(Client& client, uint64_t* content)
+{
+
+    uint64_t time = currentTimeMillis();
+
+    uint64_t number = *content;
+
+    uint64_t timeSent = client.ping.at(number);
+
+    client.ping.erase(number);
+
+    client.latency = time - timeSent;
+    client.lastPong = time;
+    //qDebug() << "prisel pong" << number;
+}
+
+void CurveDataServer::handleEOT(Client& client, char* content)
+{
+    qDebug() << "Client" << client.socket->peerName() << "disconnected:" << content;
+    terminateConnection(client, "Recieved EOT from client");
+}
+
+
+void CurveDataServer::terminateConnection(Client& client, std::string&& reason)
+{
+    if (client.socket->isOpen())
+    {
+        sendEot(client.socket, std::forward<std::string&&>(reason));
+
+        client.socket->close();
+    }
+
+    removeSocket(client.socket);
+}
+
 void CurveDataServer::sendActuatorPosition(float x, float y, float z)
 {
     QByteArray message = message128(0x4003,
@@ -245,12 +302,7 @@ void CurveDataServer::sendActuatorPosition(float x, float y, float z)
                                     *((uint32_t*) &y),
                                     *((uint32_t*) &z));
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
 }
 
 void CurveDataServer::sendActualDirection(float x, float y, float z)
@@ -260,12 +312,7 @@ void CurveDataServer::sendActualDirection(float x, float y, float z)
                                     *((uint32_t*) &y),
                                     *((uint32_t*) &z));
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
 }
 
 void CurveDataServer::sendTargetDirection(float x, float y, float z)
@@ -275,12 +322,7 @@ void CurveDataServer::sendTargetDirection(float x, float y, float z)
                                     *((uint32_t*) &y),
                                     *((uint32_t*) &z));
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
 }
 
 void CurveDataServer::sendNewCurve(QList<QVector3D> &points)
@@ -315,12 +357,12 @@ void CurveDataServer::sendNewCurve(QList<QVector3D> &points)
         message[2 + 4 + i*12 + 11] = z[3];
     }
 
-    {
-        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
-        for (auto& client : _sockets) {
-                client.socket->write(message);
-        }
-    }
+    sendMessageToAllConnected(message);
+
+
+    std::lock_guard<decltype (_currentCurveLock)> l(_currentCurveLock);
+    _currentCurve.clear();
+    _currentCurve.append(points);
 }
 
 void CurveDataServer::sendNewCurve(QList<QVector4D> &points)
@@ -363,7 +405,6 @@ void CurveDataServer::sendProtocolMagic(QTcpSocket *socket)
     socket->write(message);
 
     qDebug() << "Sent Protocol Magic to" << socket->peerAddress();
-    //qDebug() << message.toHex();
 }
 
 void CurveDataServer::sendVersion(QTcpSocket *socket)
@@ -373,7 +414,6 @@ void CurveDataServer::sendVersion(QTcpSocket *socket)
     socket->write(message);
 
     qDebug() << "Sent Protocol Version to" << socket->peerAddress();
-    //qDebug() << message.toHex();
 }
 
 void CurveDataServer::sendEot(QTcpSocket *socket, std::string&& reason)
@@ -381,3 +421,116 @@ void CurveDataServer::sendEot(QTcpSocket *socket, std::string&& reason)
     QByteArray message = messageVarLength(0xF006, ((uint32_t) reason.length()), reason.c_str());
     socket->write(message);
 }
+
+
+
+void CurveDataServer::sendMessageToAllConnected(QByteArray &message)
+{
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+    for (auto& client : _sockets) {
+        if (client.magic && client.version)
+        {
+            client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::threadFunction()
+{
+    while (_threadIsRunning)
+    {
+        //processPings();
+
+        // TODO tim se zpravilo varovani se socketama
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+void CurveDataServer::processPings()
+{
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+    for (auto& client : _sockets) {
+
+        if (client.magic && client.version)
+        {
+            if (client.lastPong  < currentTimeMillis() - 10000)
+            {
+                terminateConnection(client, "Timeout");
+                continue;
+            }
+
+            sendPing(client);
+
+            std::lock_guard<decltype (_currentCurveLock)> l(_currentCurveLock);
+            if (!client.initialized && !_currentCurve.isEmpty())
+            {
+                sendCurve(client);
+                client.initialized = true;
+            }
+        }
+        else if (client.lastPong  < currentTimeMillis() - 10000)
+        {
+            terminateConnection(client, "Preamble timeout");
+            continue;
+        }
+    }
+}
+
+void CurveDataServer::removeSocket(QTcpSocket *socket)
+{
+    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+    for (auto it = _sockets.begin(); it != _sockets.end(); it++) {
+        if (it->socket == socket) {
+            _sockets.erase(it);
+            break;
+        }
+    }
+}
+
+void CurveDataServer::sendPing(Client& client)
+{
+    uint64_t number = (((uint64_t) rand()) << 32) + (uint64_t) rand();
+
+    //qDebug() << "posilam ping" << ((qint64) number) << ".";
+
+    client.ping.emplace(std::make_pair(number, currentTimeMillis()));
+    //client.ping.insert({number, currentTimeMillis()});
+
+    client.socket->write(message64(0x3004, (uint64_t) number));
+}
+
+void CurveDataServer::sendCurve(Client &client)
+{
+    int size = _currentCurve.size();
+
+    QByteArray message = messageVarLength(0xF009, (uint32_t) size * 12);
+
+    for (int i = 0; i < size; i++)
+    {
+        float xf = _currentCurve[i].x();
+        float yf = _currentCurve[i].y();
+        float zf = _currentCurve[i].z();
+
+        uint8_t *x = ((uint8_t*) &xf);
+        uint8_t *y = ((uint8_t*) &yf);
+        uint8_t *z = ((uint8_t*) &zf);
+
+        message[2 + 4 + i*12 + 0] = x[0];
+        message[2 + 4 + i*12 + 1] = x[1];
+        message[2 + 4 + i*12 + 2] = x[2];
+        message[2 + 4 + i*12 + 3] = x[3];
+
+        message[2 + 4 + i*12 + 4] = y[0];
+        message[2 + 4 + i*12 + 5] = y[1];
+        message[2 + 4 + i*12 + 6] = y[2];
+        message[2 + 4 + i*12 + 7] = y[3];
+
+        message[2 + 4 + i*12 + 8] = z[0];
+        message[2 + 4 + i*12 + 9] = z[1];
+        message[2 + 4 + i*12 + 10] = z[2];
+        message[2 + 4 + i*12 + 11] = z[3];
+    }
+
+    client.socket->write(message);
+}
diff --git a/deltarobot/curvedataserver.h b/deltarobot/curvedataserver.h
index a41abf0..68c7efe 100644
--- a/deltarobot/curvedataserver.h
+++ b/deltarobot/curvedataserver.h
@@ -6,9 +6,7 @@
 #include <QTcpSocket>
 #include <map>
 #include <mutex>
-
-
-#include "../element/trajectory.hpp" //obsahuje MAX_CURVE_SIZE kterou se ridi sendNewCurve 4D
+#include <thread>
 
 class CurveDataServer : QObject
 {
@@ -16,17 +14,21 @@ class CurveDataServer : QObject
 
 public:
     struct Client {
-        static const size_t BUF_SIZE = 32;
+        static const size_t BUF_SIZE = 4096;
         static const size_t MESSAGE_ID_SIZE = 2;
 
         QTcpSocket* socket;
-        std::map<long, long> ping;  ///< key: ping ID;  value: ping timestamp (msec);
-        long latency;               ///< client's latency
-        long lastPong;              ///< timestamp of last pong
+        std::map<uint64_t, uint64_t> ping;  ///< key: ping ID;  value: ping timestamp (msec);
+        uint64_t latency;               ///< client's latency
+        uint64_t lastPong;              ///< timestamp of last pong
 
         char buf[BUF_SIZE];
         size_t bufUsed;
 
+        bool magic = false;
+        bool version = false;
+        bool initialized = false;
+
         Client(QTcpSocket* socket);
     };
 
@@ -38,6 +40,9 @@ public:
     void sendTargetDirection(float x, float y, float z);
     void sendNewCurve(QList<QVector4D> &points);
     void sendNewCurve(QList<QVector3D> &points);
+    void sendCurve(Client &client);
+
+    void processPings();            ///< send pings, check timeouts
 
 public slots:
     void onNewConnection();
@@ -48,16 +53,35 @@ private:
     QList<Client>::iterator clientOf(QTcpSocket *socket);
 
     void handleMessage(Client& client, uint16_t messageId, void* content);
+    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 validPreamble(Client& client);
+    void removeSocket(QTcpSocket *socket);
+    void terminateConnection(Client& client, std::string&& reason);
+    void sendPing(Client& client);
 
     void sendPreamble(QTcpSocket *socket);
     void sendProtocolMagic(QTcpSocket *socket);
     void sendVersion(QTcpSocket *socket);
     void sendEot(QTcpSocket *socket, std::string&& message);
 
+    void sendMessageToAllConnected(QByteArray &message);
+
+    void threadFunction();
+
     QTcpServer _server;
+    std::thread _pingThread;
+
+    volatile bool _threadIsRunning = true;
 
     std::recursive_mutex _socketsLock;
     QList<Client> _sockets;
+
+    std::recursive_mutex _currentCurveLock;
+    QList<QVector3D> _currentCurve;
 };
 
 #endif // CURVEDATASERVER_H
diff --git a/deltarobot/mainwindow.cpp b/deltarobot/mainwindow.cpp
index 964ca38..f83cf7b 100644
--- a/deltarobot/mainwindow.cpp
+++ b/deltarobot/mainwindow.cpp
@@ -1074,6 +1074,7 @@ void MainWindow::mainCycle()
     cdServer->sendActuatorPosition(spherePos.x(), spherePos.y(), spherePos.z());
     cdServer->sendActualDirection(actualArrow.x(), actualArrow.y(), actualArrow.z());
     cdServer->sendTargetDirection(dirArrow.x(), dirArrow.y(), dirArrow.z());
+    cdServer->processPings();
 }
 
 //! Příjem dat ze stroje
