aswi2021virtual-surreality-gitlab/deltarobot/curvedataserver.cpp @ 04839796

#include "curvedataserver.h"

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

#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),
    _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();
}

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

    qDebug() << "Client" << clientSocket->peerAddress() << "is connecting...";
    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()) {
        // read the message ID
        if (client.bufUsed < Client::MESSAGE_ID_SIZE) {
            qint64 toRead = qMin(qint64(Client::MESSAGE_ID_SIZE - client.bufUsed), sender->bytesAvailable());
            if (toRead > 0) {
                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);
        bool variableLength = false;
        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:
            variableLength = true;
            contentSize = 0;
            break;
        default:
            // unsupported message size
            return;
        }

        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) {
            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 == (contentOffset + contentSize)) {
            handleMessage(client, messageId, &client.buf[contentOffset], contentSize);
            client.bufUsed = 0;
        }
    }
}

void CurveDataServer::handleMessage(Client& client, uint16_t messageId, void *content, size_t contentSize) {
    switch (messageId) {
    case 0x3001:
        handleProtocolMagic(client, (char *) content);
        break;
    case 0x2002:
        handleVersion(client, *((uint8_t*) content));
        break;
    case 0x3005:
        handlePong(client, (uint64_t*) content);
        break;
    case 0xF006:
        qDebug() << "EOT!";
        handleEOT(client, (char *) content, contentSize);
        break;
    }
}

void CurveDataServer::handleProtocolMagic(Client& client, char *content)
{
    char *correct = "DeltaRVr";

    if (strncmp(content, correct, 8) == 0) {
        client.magic = true;
    }
    validateProtocol(client);
}

void CurveDataServer::handleVersion(Client& client, uint8_t content)
{
    if (((uint8_t*) &content)[0] == 1)
    {
        client.version = true;
    }
    validateProtocol(client);
}

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

void CurveDataServer::handleEOT(Client& client, char* content, size_t contentSize)
{
    char cstrContent[contentSize + 1];
    std::memcpy(cstrContent, content, contentSize);
    cstrContent[contentSize] = '\0';

    qDebug() << "Client" << client.socket->peerAddress() << "disconnected:" << cstrContent;
    terminateConnection(client, "Recieved EOT from client");
}


void CurveDataServer::validateProtocol(Client &client) {
    if (!client.protocolValid && client.magic && client.version) {
        client.protocolValid = true;
        qDebug() << "Client" << client.socket->peerAddress() << "has successfully connected.";
    }
}


void CurveDataServer::terminateConnection(Client& client, std::string&& reason)
{
    if (client.socket->isOpen())
    {
        sendEot(client.socket, std::forward<std::string&&>(reason));

        client.socket->close();
    }

    qDebug() << "Terminating connection to client" << client.socket->peerAddress() << "reason:" << reason.c_str();
    removeSocket(client.socket);
}

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

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

    socket->write(message);
}

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.protocolValid)
        {
            client.socket->write(message);
        }
    }
}

void CurveDataServer::processPings()
{
    std::lock_guard<decltype (_socketsLock)> l(_socketsLock);
    for (auto& client : _sockets) {

        if (client.protocolValid)
        {
            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();

    client.ping.emplace(std::make_pair(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);
}