aswi2021virtual-surreality-gitlab/deltarobot/curvedataserver.cpp @ 9360266c

#include "curvedataserver.h"

#include <QDebug>
#include <QHostAddress>
#include <QAbstractSocket>
#include <QVector4D>
#include <chrono>

#include "../element/trajectory.hpp"

// Utility functions ///////////////////////////////////////////////////////////////////////////////////////////////////

static QByteArray message8(uint16_t messageId, uint8_t content = 0) {
    QByteArray result(2 + 1, 0);
    result[0] = ((uint8_t*) &messageId)[0];
    result[1] = ((uint8_t*) &messageId)[1];
    result[2] = content;
    return result;
}

static QByteArray message16(uint16_t messageId, uint16_t content = 0) {
    QByteArray result(2 + 2, 0);
    result[0] = ((uint8_t*) &messageId)[0];
    result[1] = ((uint8_t*) &messageId)[1];
    result[2] = ((uint8_t*) &content)[0];
    result[3] = ((uint8_t*) &content)[1];
    return result;
}

static QByteArray message32(uint16_t messageId, uint32_t content = 0) {
    QByteArray result(2 + 4, 0);
    result[0] = ((uint8_t*) &messageId)[0];
    result[1] = ((uint8_t*) &messageId)[1];
    result[2] = ((uint8_t*) &content)[0];
    result[3] = ((uint8_t*) &content)[1];
    result[4] = ((uint8_t*) &content)[2];
    result[5] = ((uint8_t*) &content)[3];
    return result;
}

static QByteArray message64(uint16_t messageId, uint64_t content = 0) {
    QByteArray result(2 + 8, 0);
    result[0] = ((uint8_t*) &messageId)[0];
    result[1] = ((uint8_t*) &messageId)[1];
    result[2] = ((uint8_t*) &content)[0];
    result[3] = ((uint8_t*) &content)[1];
    result[4] = ((uint8_t*) &content)[2];
    result[5] = ((uint8_t*) &content)[3];
    result[6] = ((uint8_t*) &content)[4];
    result[7] = ((uint8_t*) &content)[5];
    result[8] = ((uint8_t*) &content)[6];
    result[9] = ((uint8_t*) &content)[7];
    return result;
}

static QByteArray message128(uint16_t messageId,
                             uint32_t contentX = 0,
                             uint32_t contentY = 0,
                             uint32_t contentZ = 0,
                             uint32_t contentW = 0)
{
    QByteArray result(2 + 16, 0);
    result[0] = ((uint8_t*) &messageId)[0];
    result[1] = ((uint8_t*) &messageId)[1];

    result[2] = ((uint8_t*) &contentX)[0];
    result[3] = ((uint8_t*) &contentX)[1];
    result[4] = ((uint8_t*) &contentX)[2];
    result[5] = ((uint8_t*) &contentX)[3];

    result[6] = ((uint8_t*) &contentY)[0];
    result[7] = ((uint8_t*) &contentY)[1];
    result[8] = ((uint8_t*) &contentY)[2];
    result[9] = ((uint8_t*) &contentY)[3];

    result[10] = ((uint8_t*) &contentZ)[0];
    result[11] = ((uint8_t*) &contentZ)[1];
    result[12] = ((uint8_t*) &contentZ)[2];
    result[13] = ((uint8_t*) &contentZ)[3];

    result[14] = ((uint8_t*) &contentW)[0];
    result[15] = ((uint8_t*) &contentW)[1];
    result[16] = ((uint8_t*) &contentW)[2];
    result[17] = ((uint8_t*) &contentW)[3];
    return result;
}

static QByteArray messageVarLength(uint16_t messageId, uint32_t length, const char* content = nullptr) {
    QByteArray result(2 + 4 + length, 0);
    result[0] = ((uint8_t*) &messageId)[0];
    result[1] = ((uint8_t*) &messageId)[1];
    result[2] = ((uint8_t*) &length)[0];
    result[3] = ((uint8_t*) &length)[1];
    result[4] = ((uint8_t*) &length)[2];
    result[5] = ((uint8_t*) &length)[3];

    if (content) {
        for (uint32_t i = 0; i < length; i++) {
            result[6 + i] = content[i];
        }
    }
    return result;
}

inline uint64_t currentTimeMillis() {
    return std::chrono::duration_cast<std::chrono::milliseconds>(
                std::chrono::steady_clock::now().time_since_epoch())
            .count();
}


// Client //////////////////////////////////////////////////////////////////////////////////////////////////////////////

CurveDataServer::Client::Client(QTcpSocket* socket) :
    socket(socket),
    lastPong(currentTimeMillis()),
    bufUsed(0)
{}


// Server //////////////////////////////////////////////////////////////////////////////////////////////////////////////

CurveDataServer::CurveDataServer() :
    _server(this),
    _pingThread([this](){threadFunction();}),
    _sockets()
{
    _server.listen(QHostAddress::Any, 4242);
    connect(&_server, SIGNAL(newConnection()), this, SLOT(onNewConnection()));
}

CurveDataServer::~CurveDataServer()
{
    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
    for (auto& client : _sockets)
    {
        terminateConnection(client, "Server is stopping");
    }
    _sockets.clear();

    _threadIsRunning = false;
}

void CurveDataServer::onNewConnection()
{
    QTcpSocket *clientSocket = _server.nextPendingConnection();
    connect(clientSocket, SIGNAL(readyRead()), this, SLOT(onReadyRead()));
    connect(clientSocket, SIGNAL(stateChanged(QAbstractSocket::SocketState)), this, SLOT(onSocketStateChanged(QAbstractSocket::SocketState)));

    sendPreamble(clientSocket);
    _sockets.push_back(Client(clientSocket));
}

void CurveDataServer::onSocketStateChanged(QAbstractSocket::SocketState socketState)
{
    if (socketState == QAbstractSocket::UnconnectedState)
    {
        QTcpSocket* sender = static_cast<QTcpSocket*>(QObject::sender());

        removeSocket(sender);
    }
}

void CurveDataServer::onReadyRead()
{
    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);

    QTcpSocket* sender = static_cast<QTcpSocket*>(QObject::sender());
    auto it = clientOf(sender);
    if (it == _sockets.end()) {
        return;
    }
    Client& client = *it;

    while (sender->bytesAvailable()) {
        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 (client.bufUsed < 2) {
            // no ID yet, wait for more data
            return;
        }

        uint16_t messageId = *((uint16_t*) client.buf);
        qint64 contentSize;
        switch (messageId & 0xF000) {
        case 0x0000:
            contentSize = 1;
            break;
        case 0x1000:
            contentSize = 2;
            break;
        case 0x2000:
            contentSize = 4;
            break;
        case 0x3000:
            contentSize = 8;
            break;
        case 0x4000:
            contentSize = 16;
            break;
        case 0xF000:
            // TODO - implement support for variable length messages
            contentSize = 0;
            break;
        default:
            // unsupported message size
            return;
        }

        qint64 toRead = qMin(qint64(contentSize - (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 + contentSize)) {
            handleMessage(client, messageId, &client.buf[Client::MESSAGE_ID_SIZE]);
            client.bufUsed = 0;
        }
    }
}

void CurveDataServer::handleMessage(Client& client, uint16_t messageId, void *content) {
    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, content);
        break;
    case 0xF006:
        qDebug() << "EOT!";
        handleEOT(client, 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, void* content)
{
    long time = currentTimeMillis();

    long number = *((long*) content);

    long timeSent = client.ping.at(number);

    client.ping.erase(number);

    client.latency = time - timeSent;
    client.lastPong = time;
}

void CurveDataServer::handleEOT(Client& client, void* content)
{
    qDebug() << "Client" << client.socket->peerName() << "disconnected:" << content;
    terminateConnection(client, "Recieved EOT from client");
}


void CurveDataServer::terminateConnection(Client& client, std::string&& reason)
{
    removeSocket(client.socket);

    sendEot(client.socket, std::forward<std::string&&>(reason));

    client.socket->close();
}

void CurveDataServer::sendActuatorPosition(float x, float y, float z)
{
    QByteArray message = message128(0x4003,
                                    *((uint32_t*) &x),
                                    *((uint32_t*) &y),
                                    *((uint32_t*) &z));

    sendMessageToAllConnected(message);
}

void CurveDataServer::sendActualDirection(float x, float y, float z)
{
    QByteArray message = message128(0x4007,
                                    *((uint32_t*) &x),
                                    *((uint32_t*) &y),
                                    *((uint32_t*) &z));

    sendMessageToAllConnected(message);
}

void CurveDataServer::sendTargetDirection(float x, float y, float z)
{
    QByteArray message = message128(0x4008,
                                    *((uint32_t*) &x),
                                    *((uint32_t*) &y),
                                    *((uint32_t*) &z));

    sendMessageToAllConnected(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];
    }

    sendMessageToAllConnected(message);


    std::lock_guard<decltype (_currentCurveLock)> l(_currentCurveLock);
    _currentCurve.clear();
    _currentCurve.append(points);
}

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++) {
        if (it->socket == socket) {
            return it;
        }
    }
    return _sockets.end();
}

void CurveDataServer::sendPreamble(QTcpSocket *socket)
{
    sendProtocolMagic(socket);
    sendVersion(socket);
}

void CurveDataServer::sendProtocolMagic(QTcpSocket *socket)
{
    char *messageContent = "DeltaRVr";
    QByteArray message = message64(0x3001, *((uint64_t*) messageContent));

    socket->write(message);

    qDebug() << "Sent Protocol Magic to" << socket->peerAddress();
}

void CurveDataServer::sendVersion(QTcpSocket *socket)
{
    QByteArray message = message32(0x2002, 1);

    socket->write(message);

    qDebug() << "Sent Protocol Version to" << socket->peerAddress();
}

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)
    {
        // odeslat ping, popr krivku vsem, take kontrola odpojeni
        {
            std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
            for (auto& client : _sockets) {
                if (client.magic && client.version)
                {
                    if (client.lastPong  < currentTimeMillis() - 10000)
                    {
                        terminateConnection(client, "Timeout");
                    }

                    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");
                }
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

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();

    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);
}