#include <QDebug>
#include <QTimer>
#include "aienginegimbalserverudp.h"
#include "aienginegimbalservercrc16.h"


AiEngineGimbalServerUDP::AiEngineGimbalServerUDP(QObject *parent)
    : QObject{parent}
{
    // Open UDP socket
    mUdpSocket = new QUdpSocket();
    mUdpSocket->connectToHost(QHostAddress(SERVER_IP), SERVER_PORT);

    if (mUdpSocket->isOpen() == false) {
        qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Unable to connect " << SERVER_IP << ":" << SERVER_PORT;
        delete mUdpSocket;
        exit(EXIT_FAILURE);
    }

    qDebug().noquote().nospace() << "AiEngineGimbalServerUDP(): Connected " << SERVER_IP << ":" << SERVER_PORT;
}


AiEngineGimbalServerUDP::~AiEngineGimbalServerUDP()
{
    if (mUdpSocket->isOpen() == true) {
        mUdpSocket->close();
    }

    delete mUdpSocket;
}


void AiEngineGimbalServerUDP::sendCommand(const QByteArray &command)
{
    QByteArray toSend = command;
    int8_t crcBytes[2];
    AiEngineGimbalServerCrc16::getCRCBytes(toSend, crcBytes);
    toSend.resize(toSend.size() + 2);        // Increase array size to accommodate CRC bytes
    toSend[toSend.size() - 2] = crcBytes[0]; // Set LSB
    toSend[toSend.size() - 1] = crcBytes[1]; // Set MSB

    qDebug().noquote().nospace() << "AiEngineGimbalServerUDP(): Sent HEX data: " << toSend.toHex();

    if (mUdpSocket->isOpen() == false) {
        qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Connection not open (sendCommand)";
        return;
    }

    if (mUdpSocket->write(toSend) == -1) {
        qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Failed to send data: " << mUdpSocket->errorString();
    }
}

QByteArray AiEngineGimbalServerUDP::readResponse()
{
    QByteArray response;
    QByteArray recvBuff(RECV_BUF_SIZE, 0);

    if (mUdpSocket->waitForReadyRead(3000)) {
        qint64 recvLen = mUdpSocket->read(recvBuff.data(), RECV_BUF_SIZE);
        if (recvLen > 0) {
            response = recvBuff.left(recvLen);
            qDebug().noquote().nospace() << "AiEngineGimbalServerUDP(): Received HEX data: " << response.toHex();
        } else {
            qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): Failed to receive data: " << mUdpSocket->errorString();
        }
    } else {
        qCritical().noquote().nospace() << "AiEngineGimbalServerUDP(): No data received.";
    }

    return response;
}