autopilot/misc/camera/a8/remoteControl.cpp

#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>
#include <QCoreApplication>
#include <QDebug>
#include <QJsonDocument>
#include <QJsonObject>
#include <QObject>
#include <QTimer>
#include <QUdpSocket>
#include "config.hpp"
#include "defines.hpp"
#include "utilsTargetLocation.hpp"
#include "remoteControl.hpp"

RemoteControl::RemoteControl(QObject *parent)
    : QObject{parent}, mIsBusy(false)
{
    mUdpSocket = new QUdpSocket(this);
    connect(mUdpSocket, &QUdpSocket::readyRead, this, &RemoteControl::readPendingDatagrams);
    mUdpSocket->bind(QHostAddress::Any, UDP_PORT);
}

void RemoteControl::readPendingDatagrams()
{
    qDebug() << "readPendingDatagrams() Got UDP message!";

    while (mUdpSocket->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(mUdpSocket->pendingDatagramSize());
        mUdpSocket->readDatagram(datagram.data(), datagram.size());

        QJsonParseError jsonError;
        QJsonDocument jsonDoc = QJsonDocument::fromJson(datagram, &jsonError);
        if (jsonError.error != QJsonParseError::NoError) {
            qWarning() << "Error parsing JSON:" << jsonError.errorString();
            continue;
        }

        if (jsonDoc.isObject()) {
            if (mIsBusy == false) {
                qDebug() << "readPendingDatagrams() mIsBusy == false! Processing JSON";
                processJSON(jsonDoc);
            }
        }
        else {
            qWarning().noquote().nospace() << "Received data was not JSON object.";
        }
    }
}

void RemoteControl::processJSON(QJsonDocument commandDoc)
{
    mIsBusy = true;
    qDebug().noquote().nospace() << "Received: " << QString(commandDoc.toJson());

    QJsonObject commandObject = commandDoc.object();

    // Exit with exit message
    if (commandObject.contains("extra") == true && commandObject["extra"] == "EXIT") {
        qDebug().noquote().nospace() << "Exit message received: Exiting...";
        exit(EXIT_SUCCESS);
    }

    // TODO:
    // Wait that these are actually available
    // Without these it is impossible to calculate target location
    commandObject["altitude"] = 10.5f;
    commandObject["latitude"] = 55.75000000f;
    commandObject["longitude"] = 37.61666670f;
    commandObject["pitch"] = 0.0f;
    commandObject["yaw"] = 152.5f;
    commandObject["target_real_height"] = 2.5f;
    commandObject["target_real_width"] = 5.0f;

    // Rectangle calculation for having proper zoom on group / target
    RectangleProperties rectangle = calculateRectangleProperties(commandObject["top"].toInt(), commandObject["left"].toInt(), commandObject["bottom"].toInt(), commandObject["right"].toInt());
    commandObject["target_x"] = rectangle.middleX;
    commandObject["target_y"] = rectangle.middleY;
    commandObject["target_pixel_height"] = rectangle.height;
    commandObject["target_pixel_width"] = rectangle.width;
    qInfo() << "target_x: " << commandObject["target_x"];
    qInfo() << "target_y: " << commandObject["target_y"];
    qInfo() << "target_pixel_height: " << commandObject["target_pixel_height"];
    qInfo() << "target_pixel_width: " << commandObject["target_pixel_width"];

    // Prepare responce object
    mResponseObject = QJsonObject();
    mResponseObject["sender"] = UDP_WHO_AM_I;
    mResponseObject["status"] = "OK";

    // Get current orientation and zoom
    Config::updateState();

    QTimer::singleShot(0, this, [this, commandObject]() mutable { turnToTarget(commandObject); });
}

void RemoteControl::sendResponse(void)
{
    // TODO: Implement
    mIsBusy = false;
}

void RemoteControl::restoreOrientation(void)
{
    qDebug().noquote().nospace() << "Restoring orientation";

    QByteArray serialCommandAngle = Config::getCommand()->getCommandForInternal(COMMAND_ID::TURN_TO_DEGREES);

    int16_t degreesVal = Config::getCurrentYaw() * 10;
    serialCommandAngle[8] = degreesVal & 0xFF;
    serialCommandAngle[9] = degreesVal >> 8;
    degreesVal = Config::getCurrentPitch() * 10;
    serialCommandAngle[10] = degreesVal & 0xFF;
    serialCommandAngle[11] = degreesVal >> 8;

    Config::getSerial()->sendCommand(serialCommandAngle);

    QTimer::singleShot(0, this, [this]() mutable { sendResponse(); });
}

void RemoteControl::restoreZoom(void)
{
    qDebug().noquote().nospace() << "Restoring zoom";

    QByteArray serialCommandZoom = Config::getCommand()->getCommandForInternal(COMMAND_ID::ZOOM_TO_X);

    uint8_t integerPart = static_cast<uint8_t>(Config::getCurrentZoom());
    float fractionalPart = Config::getCurrentZoom() - integerPart;
    uint8_t scaledFractional = uint8_t(fractionalPart * 10);

    serialCommandZoom[8] = integerPart;
    serialCommandZoom[9] = scaledFractional;

    Config::getSerial()->sendCommand(serialCommandZoom);

    QTimer::singleShot(100, this, [this]() mutable { restoreOrientation(); });
}

void RemoteControl::calculateTargetPosition(QJsonObject &commandObject)
{
    qDebug().noquote().nospace() << "Getting target location";

    float altitude = commandObject["altitude"].toDouble();
    float latitude = commandObject["latitude"].toDouble();
    float longitude = commandObject["longitude"].toDouble();
    float yaw = commandObject["yaw"].toDouble();
    float pitch = commandObject["pitch"].toDouble();
    //float targetPixelWidth = commandObject["target_pixel_width"].toInt();
    //float targetPixelHeight = commandObject["target_pixel_height"].toInt();
    //float targetRealWidth = commandObject["target_real_width"].toDouble();
    //float targetRealHeight = commandObject["target_real_height"].toDouble();

    //GPSData gpsData = UtilsTargetLocation::getLocation(altitude, latitude, longitude, yaw, pitch, 0.0f, targetRealWidth, targetPixelWidth);
    GPSData gpsData = UtilsTargetLocation::getLocation(altitude, latitude, longitude, yaw, pitch, 0.0f);
    mResponseObject["altitude"] = gpsData.altitude;
    mResponseObject["latitude"] = gpsData.latitude;
    mResponseObject["longitude"] = gpsData.longitude;

    QTimer::singleShot(100, this, [this, commandObject]() mutable { zoomToTarget(commandObject); });
}

void RemoteControl::turnToTarget(QJsonObject &commandObject)
{
    qDebug().noquote().nospace() << "Turning to target";

    uint16_t targetX = commandObject["target_x"].toInt();
    uint16_t targetY = commandObject["target_y"].toInt();

    float resultYaw = 0.0f;
    float resultPitch = 0.0f;
    UtilsTargetLocation::getAnglesToOnScreenTarget(targetX, targetY, resultYaw, resultPitch);

    QByteArray serialCommandTurn = Config::getCommand()->getCommandForInternal(COMMAND_ID::TURN_TO_PIXEL);

    int16_t degreesVal = resultYaw * 10;
    serialCommandTurn[8] = degreesVal & 0xFF;
    serialCommandTurn[9] = degreesVal >> 8;

    degreesVal = resultPitch * 10;
    serialCommandTurn[10] = degreesVal & 0xFF;
    serialCommandTurn[11] = degreesVal >> 8;

    Config::getSerial()->sendCommand(serialCommandTurn);

    QTimer::singleShot(1000, this, [this, commandObject]() mutable { calculateTargetPosition(commandObject); });
}

void RemoteControl::zoomToTarget(QJsonObject &commandObject)
{
    qDebug().noquote().nospace() << "Zooming to target";

    float fillRatio = 0.5;
    float targetPixelWidth = commandObject["target_pixel_width"].toInt();
    float targetPixelHeight = commandObject["target_pixel_height"].toInt();
    float adjustedObjectWidth = targetPixelWidth / fillRatio;
    float adjustedObjectHeight = targetPixelHeight / fillRatio;
    float zoomWidth = static_cast<double>(Config::getResolutionWidth()) / adjustedObjectWidth;
    float zoomHeight = static_cast<double>(Config::getResolutionHeight()) / adjustedObjectHeight;
    float zoom = std::min(zoomWidth, zoomHeight);
    if (zoom < 1.0f) {
        zoom = 1.0f;
    }

    QByteArray serialCommandNewZoom = Config::getCommand()->getCommandForInternal(COMMAND_ID::ZOOM_TO_X);

    uint8_t integerPart = static_cast<uint8_t>(zoom);
    float fractionalPart = zoom - integerPart;
    uint8_t scaledFractional = uint8_t(fractionalPart * 10);

    serialCommandNewZoom[8] = integerPart;
    serialCommandNewZoom[9] = scaledFractional;

    Config::getSerial()->sendCommand(serialCommandNewZoom);

    QTimer::singleShot(5000, this, [this, commandObject]() mutable { restoreZoom(); });
}

RectangleProperties RemoteControl::calculateRectangleProperties(uint16_t top, uint16_t left, uint16_t bottom, uint16_t right)
{
    // Sanity check
    // top cannot be greater than bottom
    // left cannot be greater than right
    if (top > bottom) {
        uint16_t temp = top;
        top = bottom;
        bottom = temp;
        qWarning().noquote().nospace() << "calculateRectangleProperties(): top and bottom mixed?";
    }
    if (left > right) {
        uint16_t temp = left;
        left = right;
        right = temp;
        qWarning().noquote().nospace() << "calculateRectangleProperties(): left and right mixed?";
    }

    RectangleProperties properties;
    properties.width = right - left;
    properties.height = bottom - top;
    properties.middleX = static_cast<uint16_t>(left + properties.width / 2);
    properties.middleY = static_cast<uint16_t>(top + properties.height / 2);

    // Sanity check, none cannot be 0
    // If that is the case, we will not turn or zoom
    if (properties.height == 0 || properties.width == 0 || properties.middleX == 0 || properties.middleY == 0) {
        properties.height = CAMERA_RESOLUTION_HEIGHT;
        properties.width = CAMERA_RESOLUTION_WIDTH;
        properties.middleX = CAMERA_RESOLUTION_WIDTH / 2;
        properties.middleY = CAMERA_RESOLUTION_HEIGHT / 2;
        qWarning().noquote().nospace() << "calculateRectangleProperties(): Something was zero -> No zoom, no turn!";
    }

    return properties;
}