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

#include "curvedataserver.h"

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

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

CurveDataServer::~CurveDataServer()
{
    // asi vlastne neni co
}

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::createMessageId(int sizeId, int typeId)
{
    QByteArray *id = new QByteArray();

    id->append((sizeId & 0x0F) << 4 + ((typeId & 0x0F00) >> 12));
    id->append(typeId & 0xFF);

    return id;
}
/*
QByteArray *CurveDataServer::appendFloatToMessage(QByteArray *message, float f)
{
    if (!message)
        return message;

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

    for (int i = 0; i < 4; i++)
    {
        message->append(c[3 - i]);
    }

    return message;
}
*/
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::sendApplicatorPoint(float x, float y, float z)
{
    QByteArray message(18, 0);
    message[0] = 0x03;
    message[1] = 0x40;

    putFloatIntoMessage(&message, 2, x);
    putFloatIntoMessage(&message, 6, y);
    putFloatIntoMessage(&message, 10, z);
    putFloatIntoMessage(&message, 14, 0.0);

    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(10, 0);
    message[0] = 0x01;
    message[1] = 0x30;

    for (int i = 0; i < 8; i++)
    {
        message[i + 2] = messageContent[i];
    }

    socket->write(message);

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

void CurveDataServer::sendVersion(QTcpSocket *socket)
{
    QByteArray message(6, 0);
    message[0] = 0x02;
    message[1] = 0x20;

    message[2] = 0x01;

    socket->write(message);

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