#include "statusprocess.h"

//留出一个接口给界面去注册指令
//收到ACK后发送一个信号

statusprocess::statusprocess(QObject *parent) : QObject(parent)
{
    setRunFrq(1);

    status.m_Mode = _modetype::Nop_Mode;

}


void statusprocess::setRunFrq(uint32_t frq)
{
    if((frq != 0)||(frq <= 1000))
    {
        running_frq = frq;
        qDebug() << "set command thread running frquency:" <<frq <<"Hz";
    }
}

void statusprocess::start()
{
    if(thread == nullptr)
    {
        thread = new QThread();
        this->moveToThread(thread);
        connect(thread, &QThread::started, this, &statusprocess::process);
    }

    if(!thread->isRunning())
    {
        running_flag = true;
        thread->start();
        qDebug() << "thread start" << running_flag;
    }
    else
    {
        qDebug() << "thread has started";
    }
}

void statusprocess::stop()
{
    if(thread->isRunning())
    {
        running_flag = false;
        qDebug() << "thread stop"
                 << QThread::currentThreadId()
                 << QThread::currentThread()
                 << "running state:"
                 << running_flag;

        disconnect(thread, nullptr, nullptr, nullptr);

    }
    else
    {
        qDebug() << "thread is not running";
    }
}

void statusprocess::process()//线程函数
{
    uint8_t count = 0;

    while (running_flag)
    {
         count ++;
         QThread::msleep(1000.0/running_frq);

         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     :break;
            case RecieveMode  : ReadStateMachine();break;
            case TransmitMode :
             {
                 heartbeat(m_heartbeat.custom_mode,
                           m_heartbeat.type,
                           m_heartbeat.autopilot,
                           m_heartbeat.base_mode,
                           m_heartbeat.system_status,
                           m_heartbeat.mavlink_version);

                 //qDebug() << "heartbeat";
             }
         }
    }
    //退出线程
    disconnect(thread, nullptr, nullptr, nullptr);
    thread->quit();
    //thread->wait(200);//等待结束
    thread->deleteLater();
    thread = nullptr;
}

void statusprocess::setID(int m_sysid,int m_compid)
{
    sysid  = (uint8_t)m_sysid;
    compid = (uint8_t)m_compid;
}

void statusprocess::SendMessage(mavlink_message_t msg)
{
    uint8_t  buff[256+20];
    uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
    emit SendMessageTo(0,buff, len);//使用信号和槽
}

//发送函数
void statusprocess::setHeartbeat(QVariant state,QVariant frq)
{
     //给指令赋值

    qDebug() << state << frq;

     //开启线程开始传输
     status.transmit.type = 0;

     if(state.toBool() == true)
     {
          status.m_Mode = _modetype::TransmitMode;//发送模式
     }
     else
     {
          status.m_Mode = _modetype::Nop_Mode;//发送模式
     }

     setRunFrq(frq.toInt());

     start();//开启线程

}




//读状态机
void statusprocess::ReadStateMachine(void)//没有读指令这个说法
{

}

//写状态机
void statusprocess::WriteStateMachine(void)
{


}


/*all msg
  heartbeat
*/



void statusprocess::heartbeat(uint32_t custom_mode,uint8_t type,uint8_t autopilot,uint8_t base_mode,uint8_t system_status,uint8_t mavlink_version)
{
    static mavlink_message_t msg;
    static mavlink_heartbeat_t heartbeat;

    heartbeat.custom_mode = custom_mode;
    heartbeat.type = type;

    heartbeat.autopilot = autopilot;
    heartbeat.base_mode = base_mode;
    heartbeat.system_status = system_status;
    heartbeat.mavlink_version = mavlink_version;

    mavlink_msg_heartbeat_encode(Current_sysID,Current_CompID, &msg,&heartbeat);
    SendMessage(msg);
}