#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;

    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);
}


// 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;
}