#include <QDebug>
#include <QTimer>
#include "aienginegimbalserverserialport.h"
#include "aienginegimbalservercrc16.h"
#include "aienginegimbalserverdefines.h"


AiEngineGimbalServerSerialPort::AiEngineGimbalServerSerialPort(QObject *parent)
    : QObject{parent}
{
    mSerialPort = new QSerialPort();
    mSerialPort->setPortName(AI_ENGINE_SERIAL_PORT);
    mSerialPort->setBaudRate(QSerialPort::Baud115200);
    mSerialPort->setDataBits(QSerialPort::Data8);
    mSerialPort->setStopBits(QSerialPort::OneStop);
    mSerialPort->setFlowControl(QSerialPort::NoFlowControl);

    // Open the serial port
    if (openPort() == false) {
        qCritical().noquote().nospace() << "SerialPort(): Unable to open port " << AI_ENGINE_SERIAL_PORT;
        delete mSerialPort;
        exit(EXIT_FAILURE);
    }

    qDebug().noquote().nospace() << "SerialPort(): Opened port " << AI_ENGINE_SERIAL_PORT;
}

AiEngineGimbalServerSerialPort::~AiEngineGimbalServerSerialPort()
{
    closePort();
    delete mSerialPort;
}

bool AiEngineGimbalServerSerialPort::openPort()
{
    if (mSerialPort->isOpen()) {
        qDebug().noquote().nospace() << "Port already open";
        return true;
    }

    return mSerialPort->open(QIODevice::ReadWrite);
}

void AiEngineGimbalServerSerialPort::closePort()
{
    if (mSerialPort->isOpen()) {
        mSerialPort->close();
    }
}

void AiEngineGimbalServerSerialPort::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

    QString commandStr;
    for (int i = 0; i < toSend.size(); i++) {
        if (i > 0) {
            commandStr += ",";
        }
        commandStr += QString("0x%1").arg(toSend.at(i), 2, 16, QChar('0')).toUpper();
        commandStr.replace("0X", "0x");
    }
    qDebug().noquote().nospace() << "Command: " << commandStr;

    if (!mSerialPort->isOpen()) {
        qCritical().noquote().nospace() << "Error: Port not open (sendCommand)";
        return;
    }

    mSerialPort->write(toSend);
}

QByteArray AiEngineGimbalServerSerialPort::readResponse()
{
    if (!mSerialPort->isOpen()) {
        qDebug().noquote().nospace() << "Error: Port not open (readResponse)";
        return QByteArray();
    }

    // Read data from serial port until timeout or specific criteria met
    QByteArray response;
    while (mSerialPort->waitForReadyRead(AI_ENGINE_SERIAL_RESPONSE_WAIT_TIME)) { // Adjust timeout as needed
        response.append(mSerialPort->readAll());
    }

    return response;
}