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

#include "curvedataserver.h"

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


// 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 long 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)
    {
        sendEot(client.socket, "Server is stopping");
        client.socket->close();
    }
    _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)));

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

void CurveDataServer::onSocketStateChanged(QAbstractSocket::SocketState socketState)
{
    if (socketState == QAbstractSocket::UnconnectedState)
    {
        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;
            }
        }
    }
}

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
            return;
        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!";
        break;
    case 0x2002:
        qDebug() << "Version arrived!";
        break;
    case 0x3005:
        qDebug() << "Pong " << *((uint64_t*) content) << " arrived!";
        break;
    }
}

void CurveDataServer::sendActuatorPosition(float x, float y, float z)
{
    QByteArray message = message128(0x4003,
                                    *((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::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++) {
        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();
    //qDebug() << message.toHex();
}

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

    socket->write(message);

    qDebug() << "Sent Protocol Version to" << socket->peerAddress();
    //qDebug() << message.toHex();
}

void CurveDataServer::sendEot(QTcpSocket *socket, std::string&& reason)
{
    QByteArray message = messageVarLength(0xF006, ((uint32_t) reason.length()), reason.c_str());
    socket->write(message);
}