#include <QCoreApplication>
#include <QThread>
#include "serialCommand.h"
#include "serialPort.h"
#include "serialResponse.h"
#include "utilsTargetLocation.h"
#include <iostream>

int main(int argc, char *argv[])
{
    QCoreApplication app(argc, argv);

    // Replace with your actual port name
    const QString portName = "/dev/ttyUSB0";

    // Create SerialPort object
    SerialPort serial(portName);

    // Open the serial port
    if (!serial.openPort()) {
        qDebug() << "Failed to open serial port";
        return 1;
    }

    SerialCommand commands;

    while (true) {
        commands.printCommands();

        // Get user input
        qInfo() << "Enter a command (0 to exit, 1 to run test): ";
        int16_t number;
        std::cin >> number;

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

        // Exit loop if user enters 0
        if (number == 0) {
            break;
        } else if (number == 1) {
            qInfo() << "Running target location test";

            UtilsTargetLocation locateTarget(&serial, &commands);
            GPSData gpsData = locateTarget.getLocation(200.0, 63.16122286887124, 23.822053704379698, 180.0, 0.0, 0.0, 5.0, 20);
            qInfo() << "Altitude:  " << gpsData.altitude;
            qInfo() << "Latitude:  " << gpsData.latitude;
            qInfo() << "Longitude: " << gpsData.longitude;
        } else {
            COMMAND_ID commandId = (COMMAND_ID) number;

            // Example command to send (replace with actual Siyi A8 mini camera commands)
            QByteArray command = commands.getCommandByID(commandId);
            serial.sendCommand(command);

            // Read response from the camera
            QByteArray response = serial.readResponse();
            SerialResponse::printResponse(response);
        }
    }

    // Close the serial port
    serial.closePort();

    return 0;
}