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

#include "curvedataserver.h"

#include <QDebug>
#include <QHostAddress>
#include <QAbstractSocket>

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) {
    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];
    return result;
}

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

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

    _sockets.push_back(clientSocket);

    sendPreamble(clientSocket);
}

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

void CurveDataServer::onReadyRead()
{
    // tady actually nakonec switch zpracovani ruznych zprav dle identifikatoru
    QTcpSocket* sender = static_cast<QTcpSocket*>(QObject::sender());
    QByteArray data = sender->readAll();
    for (QTcpSocket* socket : _sockets) {
        if (socket != sender)
            socket->write(QByteArray::fromStdString(sender->peerAddress().toString().toStdString() + ": " + data.toStdString()));
    }
}

QByteArray *CurveDataServer::putFloatIntoMessage(QByteArray *message, int index, float f)
{
    if (!message || message->size() < index + 4)
        return message;

    unsigned char *c = reinterpret_cast<unsigned char *>(&f);

    for (int i = 0; i < 4; i++)
    {
        (*message)[index + i] = c[i];
    }

    return message;
}


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

    for (QTcpSocket* socket : _sockets) {
            socket->write(message);
    }
    //qDebug() << message.toHex();
    //qDebug() << x << " " << y << " " << z;
}


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