autopilot/ai_controller/aienginegimbalserver.cpp

#include <QDebug>
#include <QTimer>
#include "aienginegimbalserver.h"
#include "aienginegimbalserveractions.h"


AiEngineGimbalServer::AiEngineGimbalServer(QObject *parent)
    : QObject{parent}
{
    // TODO!! Setup and use serial port....
    mIsAvailable = false;
    qDebug() << "Initial is available: " << mIsAvailable;

    // Commented out, might be required later
    // Making camera available is currently do in processUdpCommands() with command bringCameraDown
    //QTimer::singleShot(5000, this, [this]() {
    //    mIsAvailable = true;
    //    qDebug() << "Initial is available: " << mIsAvailable;
    //});

    mActions.setup(&mUdpSocket, &mUdpCommand, &mUdpResponse, &mGimbalStatus);

    // TODO: Remove after testing
    mDronePosition.position.alt = 1000;
    mDronePosition.position.lat = 49.8397;
    mDronePosition.position.lon = 24.0319;
    mDronePosition.pitch = 0.0;
    mDronePosition.yaw = 90.0;

    connect(&mActions, &AiEngineGimbalServerActions::aiTargetZoomed, this, &AiEngineGimbalServer::aiTargetZoomed);

    // Create and bind the new UDP socket for receiving commands
    mReceiveUdpSocket = new QUdpSocket(this);
    mReceiveUdpSocket->bind(QHostAddress::LocalHost, 45454);
    
    // Connect the socket to handle incoming messages
    connect(mReceiveUdpSocket, &QUdpSocket::readyRead, this, &AiEngineGimbalServer::processUdpCommands);
}


// TODO!! Client doesn't really send any signal yet to this slot.
void AiEngineGimbalServer::dronePositionSlot(AiEngineDronePosition dronePosition)
{
    qDebug() << "AiEngineGimbalServer::dronePositionSlot() Server got new drone position:"
             << dronePosition.position.lat
             << dronePosition.position.lon
             << dronePosition.position.alt
             << dronePosition.pitch
             << dronePosition.yaw;

    mDronePosition = dronePosition;
}


// This is actually called from the client.
void AiEngineGimbalServer::zoomToAiTargetSlot(AiEngineCameraTarget target)
{
    qDebug() << "zoomToAiTargetSlot called";
    if (mIsAvailable == true) {
        mIsAvailable = false;
        qDebug() << "Is available: " << mIsAvailable;

        qDebug() << "AiEngineGimbalServer::zoomToAiTargetSlot() Move camera to the new target:"
                 << "index:" << target.index
                 << "pos:" << target.rectangle.top << target.rectangle.left << target.rectangle.bottom << target.rectangle.right;

        // Rectangle calculation for having proper zoom on group / target
        AiEngineRectangleProperties rectangle = mActions.calculateRectangleProperties(target.rectangle.top, target.rectangle.left, target.rectangle.bottom, target.rectangle.right);
        qDebug() << "rectangle.middleX: " << rectangle.middleX;
        qDebug() << "rectangle.middleY: " << rectangle.middleY;
        qDebug() << "rectangle.width:   " << rectangle.width;
        qDebug() << "rectangle.height:  " << rectangle.height;

        // Turn
        mActions.turnToTarget(rectangle);

        // Calculate location
        int delay1 = 3000; // Adjust this value as needed
        AiEngineDronePosition dronePosition = mDronePosition;
        int targetIndex = target.index;
        QTimer::singleShot(delay1, this, [this, dronePosition, targetIndex]() { mActions.getLocation(dronePosition, targetIndex); });

        // Zoom
        int delay2 = delay1 + 100; // Adjust this value as needed
        QTimer::singleShot(delay2, this, [this, rectangle]() { mActions.zoomToTarget(rectangle); });

        // Return to previous position
        int delay3 = delay2 + 10000; // Adjust this value as needed
        AiEngineGimbalStatus gimbalStatus = mGimbalStatus;
        QTimer::singleShot(delay3, this, [this, gimbalStatus]() { mActions.restoreOrientationAndZoom(gimbalStatus); });

        // Allow calls
        int delay4 = delay3 + 3000; // Adjust this value as needed
        QTimer::singleShot(delay4, this, [this]() {
            mIsAvailable = true;
            qDebug() << "Is available: " << mIsAvailable;
        });
    }
}


bool AiEngineGimbalServer::isAvailable(void)
{
    return mIsAvailable;
}


// TODO!! Not sent from the client yet.
void AiEngineGimbalServer::cameraPositionSlot(AiEngineCameraPosition position)
{
    qDebug() << "AiEngineGimbalServer::cameraPositionSlot() Move camera to:" << position.pitch <<  position.yaw << "zoom:" << position.zoom;
}


void AiEngineGimbalServer::processUdpCommands(void) {
    while (mReceiveUdpSocket->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(mReceiveUdpSocket->pendingDatagramSize());
        mReceiveUdpSocket->readDatagram(datagram.data(), datagram.size());

        QString command = QString::fromUtf8(datagram);
        qDebug() << "Received command:" << command;

        if (command == "bringCameraDown") {
            mActions.setAllowCameraCommands(true); // This will allow other camera commands
            mActions.goToInitialOrientation();

            // 10 second delay to let camera get ready for AI
            QTimer::singleShot(10000, this, [this]() {
                mIsAvailable = true;
                qDebug() << "Camera set available and to initial position";
            });
        }
        else if (command == "bringCameraUp") {
            // No delay, set camera unavailable
            mActions.setAllowCameraCommands(false); // This will prevent any ongoing commands
            mIsAvailable = false;
            mActions.goToInitialOrientation();
            qDebug() << "Camera set unavailable and to initial position";
        }
    }

    /*
    // How to call this:

    #include <QUdpSocket>

    void sendCommand(const QString &command) {
        QUdpSocket udpSocket;
        QByteArray datagram = command.toUtf8();
        udpSocket.writeDatagram(datagram, QHostAddress::LocalHost, 45454);
    }

    int main() {
        sendCommand("bringCameraDown");
        sendCommand("bringCameraUp");
        return 0;
    }
    */
}