commit f06d5ae1fee80e8cb5aba06e7cd7342d240e86fd
Author: Jakub Hejman <hejmanj@students.zcu.cz>
Date:   Mon May 10 18:35:16 2021 +0200

    Re #8736 - Implement Additional Messages

diff --git a/deltarobot/curvedataserver.cpp b/deltarobot/curvedataserver.cpp
index 405af48..3c7f873 100644
--- a/deltarobot/curvedataserver.cpp
+++ b/deltarobot/curvedataserver.cpp
@@ -3,6 +3,7 @@
 #include <QDebug>
 #include <QHostAddress>
 #include <QAbstractSocket>
+#include <QVector4D>
 #include <chrono>
 
 
@@ -252,6 +253,92 @@ void CurveDataServer::sendActuatorPosition(float x, float y, float z)
     }
 }
 
+void CurveDataServer::sendActualDirection(float x, float y, float z)
+{
+    QByteArray message = message128(0x4007,
+                                    *((uint32_t*) &x),
+                                    *((uint32_t*) &y),
+                                    *((uint32_t*) &z));
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendTargetDirection(float x, float y, float z)
+{
+    QByteArray message = message128(0x4008,
+                                    *((uint32_t*) &x),
+                                    *((uint32_t*) &y),
+                                    *((uint32_t*) &z));
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendNewCurve(QList<QVector3D> points)
+{
+    int size = points.size();
+
+    QByteArray message = messageVarLength(0xF009, (uint32_t) size * 12);
+
+    for (int i = 0; i < size; i++)
+    {
+        float xf = points[i].x();
+        float yf = points[i].y();
+        float zf = points[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];
+    }
+
+    {
+        std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
+        for (auto& client : _sockets) {
+                client.socket->write(message);
+        }
+    }
+}
+
+void CurveDataServer::sendNewCurve(QList<QVector4D> points)
+{
+    QList<QVector3D> curve3d;
+    int itemCount = points.size();
+
+    curve3d.clear();
+    if (itemCount > MAX_CURVE_SIZE)
+        itemCount = MAX_CURVE_SIZE;
+    for (int i = 0; i < itemCount; i++)
+    {
+        curve3d.append(points[i].toVector3D());
+    }
+
+    sendNewCurve(curve3d);
+}
+
 QList<CurveDataServer::Client>::iterator CurveDataServer::clientOf(QTcpSocket *socket) {
     std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
     for (auto it = _sockets.begin(); it != _sockets.end(); it++) {
diff --git a/deltarobot/curvedataserver.h b/deltarobot/curvedataserver.h
index e2373c4..076cc2a 100644
--- a/deltarobot/curvedataserver.h
+++ b/deltarobot/curvedataserver.h
@@ -7,6 +7,9 @@
 #include <map>
 #include <mutex>
 
+
+#include "../element/trajectory.hpp" //obsahuje MAX_CURVE_SIZE kterou se ridi sendNewCurve 4D
+
 class CurveDataServer : QObject
 {
     Q_OBJECT
@@ -31,6 +34,10 @@ public:
     ~CurveDataServer();
 
     void sendActuatorPosition(float x, float y, float z);
+    void sendActualDirection(float x, float y, float z);
+    void sendTargetDirection(float x, float y, float z);
+    void sendNewCurve(QList<QVector4D> points);
+    void sendNewCurve(QList<QVector3D> points);
 
 public slots:
     void onNewConnection();
diff --git a/deltarobot/gl/scene.cpp b/deltarobot/gl/scene.cpp
index 4879b2e..7faa50b 100644
--- a/deltarobot/gl/scene.cpp
+++ b/deltarobot/gl/scene.cpp
@@ -1,5 +1,7 @@
 #include "scene.hpp"
 
+#include "../curvedataserver.h"
+
 const int Scene::TRAJECTORY_I = 0;
 const int Scene::SPHERE_I = 1;
 const int Scene::SPHERE_II = 2;
@@ -58,7 +60,7 @@ Scene::~Scene()
     }
 }
 
-void Scene::setCurve(QList<QVector4D> curve)
+void Scene::setCurve(QList<QVector4D> curve, CurveDataServer * cdServer)
 {
     // Vyhledani minimalnich a maximalnich souradnic.
     QList<QVector3D> curve3d;
@@ -149,6 +151,10 @@ void Scene::setCurve(QList<QVector4D> curve)
     
     // Vykreslit.
     update();
+
+    if (cdServer) {
+        cdServer->sendNewCurve(curve3d);
+    }
 }
 
 void Scene::setSphereData(const QVector3D &position, const QVector3D &fArrow, const QVector3D &sArrow)
diff --git a/deltarobot/gl/scene.hpp b/deltarobot/gl/scene.hpp
index 15a4878..b1547bb 100644
--- a/deltarobot/gl/scene.hpp
+++ b/deltarobot/gl/scene.hpp
@@ -18,6 +18,10 @@
 #include "../element/arrow.hpp"
 #include "../element/wall.hpp"
 
+
+#include "../curvedataserver.h"
+
+
 /// Instance třídy představuje vykreslovanou 3D scénu.
 class Scene: public QOpenGLWidget
 {
@@ -39,7 +43,7 @@ class Scene: public QOpenGLWidget
         /// hodnot pro dráhu, kouli a zdi. Změna kamery, pozice světla. Vynulování
         /// rotace.
         /// @param QList<QVector4D> curve - seznam bodů definující křivku.
-        void setCurve(QList<QVector4D> curve);
+        void setCurve(QList<QVector4D> curve, CurveDataServer * cdServer);
         
         /// Nastaveni pozice koule a vektoru pro obě šipky.
         /// @param const QVector3D &position - pozice koule.
diff --git a/deltarobot/mainwindow.cpp b/deltarobot/mainwindow.cpp
index d0af9b4..964ca38 100644
--- a/deltarobot/mainwindow.cpp
+++ b/deltarobot/mainwindow.cpp
@@ -485,7 +485,7 @@ void MainWindow::setCurveData()
 #endif
         ui->glwidget->setTubusColor(QVector4D(1, 0.737f, 0.231f, 0.7f));
         if (actCurveType != CURVE_NONE)
-            ui->glwidget->setCurve(calculatedCurve);
+            ui->glwidget->setCurve(calculatedCurve, cdServer);
         currState = SEND_CURVE;
     }
 }
@@ -609,7 +609,7 @@ int MainWindow::getCurveDataFromFile(QString fileName)
         file.close();
         if (calculatedCurve.size() > 0) {
             ui->glwidget->setTubusColor(QVector4D(1, 0.737f, 0.231f, 0.7f));
-            ui->glwidget->setCurve(calculatedCurve);
+            ui->glwidget->setCurve(calculatedCurve, cdServer);
         }
         actCurveFileName = fileName;
         actCurveType = CURVE_EXTERN;
@@ -1072,6 +1072,8 @@ void MainWindow::mainCycle()
     }
 #endif
     cdServer->sendActuatorPosition(spherePos.x(), spherePos.y(), spherePos.z());
+    cdServer->sendActualDirection(actualArrow.x(), actualArrow.y(), actualArrow.z());
+    cdServer->sendTargetDirection(dirArrow.x(), dirArrow.y(), dirArrow.z());
 }
 
 //! Příjem dat ze stroje
@@ -1314,7 +1316,7 @@ void MainWindow::readyRead()
                             dlg.exec();
                         }
                         if (calculatedCurve.size() > 0) {
-                            ui->glwidget->setCurve(calculatedCurve);
+                            ui->glwidget->setCurve(calculatedCurve, cdServer);
                         } else {
                             infoDialog dlg;
                             dlg.setInfoText(tr("Velikost křivky"), tr("Naučená křivka nelze použít. Neplatná data."));
@@ -1477,7 +1479,7 @@ void MainWindow::display3DScene()
     calculatedCurve.append(QVector4D(0,0,-4900,0.0));
     calculatedCurve.append(QVector4D(0,0,-4900,0.0));
     calculatedCurve.append(QVector4D(0,0,-4000,0.0));
-    ui->glwidget->setCurve(calculatedCurve);
+    ui->glwidget->setCurve(calculatedCurve, cdServer);
 }
 
 //! Aktualizace času při asistovaném cvičení pacienta
