#include "GDUHolderManager.h"
#include "GDUDroneManager.h"
GDUHolderManager::GDUHolderManager(GDUDroneManager* gduDroneManager)
{
    cmdGenerator = new GDUCmdGenerator();
    _gduDroneManager = gduDroneManager;
}


void GDUHolderManager::beginControlHolder(int roll, int pitch, int orientation)
{
    if(control == nullptr){
        control = new GDUHolderControl();
        moveToThread(control);
        control->start();
    }
    control->update(roll,pitch,orientation);
}

void GDUHolderManager::stopControlHolder()
{
    if(control != nullptr){
        control->stop();
    }
}

void GDUHolderManager::holderBack2Center(cmdComplete_t block)
{
    GeneratoredData_t cmdData
            = cmdGenerator->cmdHolderAdjustment(0x10,0,0);
    _gduDroneManager->sendMessage(cmdData.data,block,cmdData.fullMsgID);
}

void GDUHolderManager::setHolderPitchAngle(int angle, cmdComplete_t block)
{
    GeneratoredData_t cmdData = cmdGenerator->cmdHolderAngleControl(0,angle);
    _gduDroneManager->sendMessage(cmdData.data,block,cmdData.fullMsgID);
}

void GDUHolderManager::setHolderRoll(int roll,cmdComplete_t block)
{
    GeneratoredData_t cmdData = cmdGenerator->cmdHolderRollControl(roll);
    _gduDroneManager->sendMessage(cmdData.data,block,cmdData.fullMsgID);
}