#include <iostream>
#include <QCoreApplication>
#include <QDebug>
#include <QObject>
#include <QThread>
#include "localControl.hpp"
#include "config.hpp"
#include "serialResponse.hpp"
#include "utilsTargetLocation.hpp"

LocalControl::LocalControl(QObject *parent)
    : QObject{parent}
{
    run();
}

void LocalControl::run()
{
    while (true) {
        Config::getCommand()->printCommands();

        // Get user input
        qInfo().noquote().nospace() << "Enter a command (0 to exit): ";
        int16_t number;
        std::cin >> number;

        // Check if the input is within the valid range for uint8_t
        if (number < 0 || number > Config::getCommand()->getCommandCount()) {
            qWarning().noquote().nospace() << "Number (" << qPrintable(QString::number(number)) << ") out of range 0 -" << qPrintable(QString::number(Config::getCommand()->getCommandCount() - 1));
            continue;
        }

        // Exit loop if user enters 0
        if (number == 0) {
            exit(EXIT_SUCCESS);
        } else {
            Config::updateState();
            COMMAND_ID commandId = (COMMAND_ID) number;

            if (commandId == COMMAND_ID::RUN_TARGET_LOCATION_TEST) {
                qInfo().noquote().nospace() << "Running target location test";
                //GPSData gpsData = UtilsTargetLocation::getLocation(200.0f, 63.16122286887124f, 23.822053704379698f, 180.0f, 0.0f, 0.0f, 5.0f, 20);
                GPSData gpsData = UtilsTargetLocation::getLocation(200.0f, 63.16122286887124f, 23.822053704379698f, 180.0f, 0.0f, 0.0f);
                qInfo().noquote().nospace() << "Altitude:  " << gpsData.altitude;
                qInfo().noquote().nospace() << "Latitude:  " << gpsData.latitude;
                qInfo().noquote().nospace() << "Longitude: " << gpsData.longitude;

                QCoreApplication::processEvents();
                continue;
            }

            // Example command to send (replace with actual Siyi A8 mini camera
            // commands)
            QByteArray command = Config::getCommand()->getCommandForUI(commandId);
            Config::getSerial()->sendCommand(command);

            // Read response from the camera
            QByteArray response = Config::getSerial()->readResponse();
            SerialResponse::printResponse(response);
        }

        QCoreApplication::processEvents();
    }
}