/**
 * This is a serial response class for Siyi Gimbal Cameras.
 * Other cameras might need their own serial response class.
 */

#include <QDebug>
#include <QHash>
#include <QString>
#include <QVariant>
#include "aienginegimbalservercrc16.h"
#include "aienginegimbalserverudpresponse.h"


AiEngineGimbalServerUDPResponse::AiEngineGimbalServerUDPResponse(QObject *parent)
    : QObject{parent}
{

}

QHash<QString, QVariant> AiEngineGimbalServerUDPResponse::getResponceValues(QByteArray response)
{
    QHash<QString, QVariant> results;

    if (response.size() == 0) {
        qWarning().noquote().nospace() << "Response is empty, exiting...";
        return results;
    }

    // Check response data validity
    int8_t crcCheck[2];
    uint8_t desiredLength = response.size() - 2;
    QByteArray subData(response.data(), desiredLength);
    AiEngineGimbalServerCrc16::getCRCBytes(subData, crcCheck);

    int8_t crcOriginal[2];
    crcOriginal[0] = response.at(response.size() - 2);
    crcOriginal[1] = response.at(response.size() - 1);

    // Data not OK
    if (crcCheck[0] != crcOriginal[0] || crcCheck[1] != crcOriginal[1]) {
        qWarning().noquote().nospace() << "Response data INVALID";
        QString responseCRC = QString("0x%1,0x%2").arg(crcOriginal[0], 2, 16, QLatin1Char('0')).arg(crcOriginal[1], 2, 16, QLatin1Char('0')).toUpper();
        QString recalcCRC = QString("0x%1,0x%2").arg(crcCheck[0], 2, 16, QLatin1Char('0')).arg(crcCheck[1], 2, 16, QLatin1Char('0')).toUpper();
        qWarning().noquote().nospace() << responseCRC << "!=" << recalcCRC;
    }

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

    if (command == 0x0E) {
        int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8);
        int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
        int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
        results.insert("yaw", (float) (yaw / 10));
        results.insert("pitch", (float) (pitch / 10));
        results.insert("roll", (float) (roll / 10));
    } else if (command == 0x0D) {
        int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8);
        int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
        int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
        int16_t yawSpeed = ((uint8_t) response.at(15) << 8) | (uint8_t) response.at(14);
        int16_t pitchSpeed = ((uint8_t) response.at(17) << 8) | (uint8_t) response.at(16);
        int16_t rollSpeed = ((uint8_t) response.at(19) << 8) | (uint8_t) response.at(18);
        results.insert("yaw", (float) (yaw / 10));
        results.insert("pitch", (float) (pitch / 10));
        results.insert("roll", (float) (roll / 10));
        results.insert("yaw_speed", (float) (yawSpeed / 10));
        results.insert("pitch_speed", (float) (pitchSpeed / 10));
        results.insert("roll_speed", (float) (rollSpeed / 10));
    } else if (command == 0x0F) {
        int8_t zoom = (int8_t) response.at(8);
        results.insert("zoom", zoom);
    } else if (command == 0x16 || command == 0x18) {
        float zoomInt = (float) response.at(8);
        float zoomFloat = (float) ((float) response.at(9) / 10);
        results.insert("zoom", zoomInt + zoomFloat);
    } else if (command == 0x20) {
        uint16_t width = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
        uint16_t height = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
        results.insert("width", width);
        results.insert("height", height);
    } else if (command == 0x02) {
        QByteArray hidArray;
        hidArray.append((char)response.at(8));
        hidArray.append((char)response.at(9));
        uint8_t hid = static_cast<uint8_t>(hidArray.toUShort(nullptr, 16));
        results.insert("hardware_id", hid);
    } else {
        qWarning().noquote().nospace() << "Getting responce values not implemented yet for command " << QString("0x%1").arg(command, 2, 16, QLatin1Char('0'));
        QString responseStr;
        for (int i = 0; i < response.size(); i++) {
            if (i > 0) {
                responseStr += ",";
            }
            responseStr += QString("0x%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
            responseStr.replace("0X", "0x");
        }
        qWarning().noquote().nospace() << "Responce byte array: " << responseStr;
    }

    return results;
}