#include "serialResponse.h"
#include "crc16.h"
#include "defines.h"
#include <QDebug>
#include <QString>

void SerialResponse::printResponse(QByteArray response) {
    QString responseStr;
    for (int i = 0; i < response.size(); i++) {
        responseStr +=
            QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
    }
    qDebug() << "Response: " << responseStr;

    responseStr = "";
    uint8_t command = response.at(MESSAGE_IDX::CMD_ID);

    // Check response data validity
    uint8_t crcCheck[2];
    CRC16::getCRCBytes(response.mid(0, response.size() - 2), crcCheck);

    // Data not OK
    if (crcCheck[0] != response.at(response.size() - 2) ||
        crcCheck[1] != response.at(response.size() - 1)) {
        qWarning() << "Response data not valid";
        return;
    }
    qInfo() << "Response data is valid";

    if (command == 0x0E) {
        int16_t yaw = (static_cast<uint8_t>(response.at(9)) << 8) |
                      static_cast<uint8_t>(response.at(8));
        int16_t pitch = (static_cast<uint8_t>(response.at(11)) << 8) |
                        static_cast<uint8_t>(response.at(10));
        int16_t roll = (static_cast<uint8_t>(response.at(13)) << 8) |
                       static_cast<uint8_t>(response.at(12));

        qInfo().noquote() << "Yaw:   " << QString::number(yaw / 10) + "°";
        qInfo().noquote() << "Pitch: " << QString::number(pitch / 10) + "°";
        qInfo().noquote() << "Roll:  " << QString::number(roll / 10) + "°";
    } else {
        for (int i = 0; i < response.size(); i++) {
            responseStr +=
                QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
        }
        qInfo() << "Response: " << responseStr;
    }
}