#include "parameterprocess.h"

ParameterProcess::ParameterProcess(QObject *parent) : QObject(parent)
{
    setRunFrq(20);
}

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

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

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

void ParameterProcess::stop()
{
    if(thread->isRunning())
    {
        running_flag = false;
        qDebug() << "thread stop" << running_flag;
    }
    else
    {
        qDebug() << "thread is not running";
    }
}

void ParameterProcess::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 : WriteStateMachine();break;
         }

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

void ParameterProcess::setID(int m_sysid,int m_compid)
{
    //qDebug() << "Parameter set id";
    sysid = (uint8_t)m_sysid;
    compid = (uint8_t)m_compid;
}

void ParameterProcess::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 ParameterProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,uint8_t type)
{
    sysid = m_sysid;
    compid = m_compid;
    status.m_Mode = RecieveMode;//接收模式
    status.recieve.type = (type)?(_readtype::One):(_readtype::All);

    qDebug() << "sysid" << sysid << endl
             << "compid" << compid << endl
             << "Mode" << status.m_Mode << endl
             << "type" << status.recieve.type << endl
             << "start...";

    start();//开启线程
}

void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,const char *id,uint8_t type,float value)
{
    //Q_UNUSED(count);

    sysid = m_sysid;
    compid = m_compid;

    status.m_Mode = TransmitMode;//发送模式

/*
    if(sizeof(id) < 16)
    {
        memcpy(param_set.param_id,id,sizeof(id));
    }
    else
    {

    }
    */
    memcpy(param_set.param_id,id,16);

    param_set.param_type = type;
    param_set.param_value = value;

    qDebug() << "sysid" << sysid << endl
             << "compid" << compid << endl
             << "param_id" << param_set.param_id << endl
             << "param_type" << param_set.param_type << endl
             << "param_value" << param_set.param_value << endl
             << "Mode" << status.m_Mode << endl
             << "type" << status.recieve.type << endl
             << "start...";

    start();//开启线程
}


//这个函数类似中断，专门处理接收到的状态
void ParameterProcess::Parse(mavlink_message_t msg)
{
    qDebug() << "Parameter" << msg.msgid;

    switch (msg.msgid) {
    case MAVLINK_MSG_ID_PARAM_SET:{
         //mavlink_msg_param_set_decode(&msg,&param_set);
    }break;
    case MAVLINK_MSG_ID_PARAM_VALUE:{//
         mavlink_msg_param_value_decode(&msg,&param_value);

         emit RecieveValue(msg);

         status.recieve.isWaitingforValue = false;
         status.transmit.isWaitingforValue = false;
         qDebug() << "param_value.param_index" << param_value.param_index;
    }break;
    case MAVLINK_MSG_ID_PARAM_EXT_ACK:{
         mavlink_msg_param_ext_ack_decode(&msg,&param_ext_ack);
    }break;
    case MAVLINK_MSG_ID_PARAM_EXT_SET:{
         mavlink_msg_param_ext_set_decode(&msg,&param_ext_set);
    }break;
    case MAVLINK_MSG_ID_PARAM_EXT_VALUE:{
         mavlink_msg_param_ext_value_decode(&msg,&param_ext_value);
    }break;
    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:{
         mavlink_msg_param_request_list_decode(&msg,&param_request_list);
    }break;
    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:{
         mavlink_msg_param_request_read_decode(&msg,&param_request_read);
    }break;
    case MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST:{
         mavlink_msg_param_ext_request_list_decode(&msg,&param_ext_request_list);
    }break;
    case MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ:{
         mavlink_msg_param_ext_request_read_decode(&msg,&param_ext_request_read);
    }break;

    }
}


//读状态机
void ParameterProcess::ReadStateMachine(void)//一整列
{
   static uint8_t step = 0;
   static uint8_t timeout_count = 0;
   static uint32_t time = 0;


   if(step == 0)
   {
       qDebug() << "start reading parameter";
       status.recieve.isWaitingforValue = true;

       request_list();//一整列,也可以一个,龚老师飞控并不支持这个

       step++;
       time = QTime::currentTime().msecsSinceStartOfDay();
   }
   else if(step == 1)//等待收到Item
   {
       //如果超时，那么结束
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
       {

          //做一个列表，记录谁丢了，后面单独请求
          //qmap list

          qDebug() << "parameter item time out ";
          timeout_count = 0;

          if(status.recieve.type == _readtype::All)//如果是ALL的情况下，那么再用单独一个一个去请求一次
          {
              qDebug() << "try One mode";
              status.recieve.type =_readtype::One;
              status.recieve.isWaitingforValue = true;
              time = QTime::currentTime().msecsSinceStartOfDay();
              request_read("xxx",0);//读取0
          }
          else
          {
              step = 2;
              qDebug() << "parameter read fail";
          }
       }
       else//如果没有超时
       {
           if(status.recieve.isWaitingforValue == false)//收到item
           {
              //qDebug() << "parameter item reccieved";
              timeout_count = 0;
              time = QTime::currentTime().msecsSinceStartOfDay();
              //如果参数count 大于当前index，那么继续等待
              if((param_value.param_index+1) < param_value.param_count)
              {

                status.recieve.isWaitingforValue = true;
                if(status.recieve.type == _readtype::One)
                {
                    request_read("xxx",param_value.param_index+1);//读取
                }

              }
              else
              {
                 step ++;
              }
           }
       }
   }
   else if(step == 2)//传输结束
   {
      qDebug() << "step" << step << "recieve complete";
      step = 0;
      status.m_Mode = Nop_Mode;//切换到什么都不做
      timeout_count = 0;
      status.recieve.isWaitingforValue = false;
      //停止线程,不能停止
      //stop();

   }
}

//写状态机
void ParameterProcess::WriteStateMachine(void)
{
   static uint8_t step = 0;
   static uint8_t timeout_count = 0;
   static uint32_t time = 0;

   if(step == 0)
   {
       //向其他线程或者自己读取航点的数量
       status.transmit.isWaitingforValue = true;

       set(param_set.param_id,param_set.param_type,param_set.param_value);//发送一个参数
       time = QTime::currentTime().msecsSinceStartOfDay();
       step++;//下一个阶段
   }
   else if(step == 1)//等待返回
   {
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
       {
          timeout_count ++;
          step = 0;//返回上一个步骤
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束发送
          {
             qDebug() << "parameter set value fail";
             step = 0;
             status.m_Mode = Nop_Mode;
             status.transmit.isWaitingforValue = false;
             timeout_count = 0;
          }
       }
       else
       {
           //收到Value
           if(status.transmit.isWaitingforValue == false)
           {
               qDebug() << "parameter set value ok";
               step = 0;
               status.m_Mode = Nop_Mode;
               status.transmit.isWaitingforValue = false;
               timeout_count = 0;
           }
       }
   }

}





/*all msg
 MAVLINK_MSG_ID_PARAM_SET//
 MAVLINK_MSG_ID_PARAM_VALUE//
 MAVLINK_MSG_ID_PARAM_EXT_ACK
 MAVLINK_MSG_ID_PARAM_EXT_SET
 MAVLINK_MSG_ID_PARAM_EXT_VALUE
 MAVLINK_MSG_ID_PARAM_REQUEST_LIST//
 MAVLINK_MSG_ID_PARAM_REQUEST_READ//
 MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST
 MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ
*/

void ParameterProcess::set(const char *id,uint8_t type,float value)//
{
    static mavlink_message_t msg;
    static mavlink_param_set_t param_set;

    memcpy(param_set.param_id,id,16);
    param_set.param_type = type;
    param_set.param_value = value;
    param_set.target_system = sysid;
    param_set.target_component = compid;

    mavlink_msg_param_set_encode(Current_sysID,Current_CompID, &msg,&param_set);
    SendMessage(msg);
}

void ParameterProcess::value(float value)//一般只接收不发送
{
    static mavlink_message_t msg;
    static mavlink_param_value_t param_value;


    //param_value.param_count
    //param_value.param_id
    //param_value.param_index
    //param_value.param_type
    param_value.param_value = value;
    //param_value. = sysid;
    //param_value.target_component = compid;

    mavlink_msg_param_value_encode(Current_sysID,Current_CompID, &msg,&param_value);
    SendMessage(msg);
}

void ParameterProcess::ext_ack(float value)
{
    static mavlink_message_t msg;
    static mavlink_param_ext_ack_t param_ext_ack;

    Q_UNUSED(value);

    //param_ext_ack.param_id
    //param_ext_ack.param_result
    //param_ext_ack.param_type

    //param_ext_ack.param_value = value;
    //param_ext_ack.target_system = sysid;
    //param_ext_ack.target_component = compid;

    mavlink_msg_param_ext_ack_encode(Current_sysID,Current_CompID, &msg,&param_ext_ack);
    SendMessage(msg);
}

void ParameterProcess::ext_set(const char *id,uint8_t type,float value)
{
    static mavlink_message_t msg;
    static mavlink_param_ext_set_t param_ext_set;

    Q_UNUSED(value);

    memcpy(param_ext_set.param_id,id,16);
    param_ext_set.param_type = type;
    //param_ext_set.param_value = value;
    param_ext_set.target_system = sysid;
    param_ext_set.target_component = compid;

    mavlink_msg_param_ext_set_encode(Current_sysID,Current_CompID, &msg,&param_ext_set);
    SendMessage(msg);
}

void ParameterProcess::ext_value(float value)
{
    static mavlink_message_t msg;
    static mavlink_param_ext_value_t param_ext_value;

    Q_UNUSED(value);

    //param_ext_value.param_value = value;


    //param_ext_value.target_system = sysid;
    //param_ext_value.target_component = compid;

    mavlink_msg_param_ext_value_encode(Current_sysID,Current_CompID, &msg,&param_ext_value);
    SendMessage(msg);
}

void ParameterProcess::request_list(void)//
{
    static mavlink_message_t msg;
    static mavlink_param_request_list_t param_request_list;

    param_request_list.target_system = sysid;
    param_request_list.target_component = compid;

    mavlink_msg_param_request_list_encode(Current_sysID,Current_CompID, &msg,&param_request_list);
    SendMessage(msg);
}

void ParameterProcess::request_read(const char *id, int16_t index)//
{
    static mavlink_message_t msg;
    static mavlink_param_request_read_t param_request_read;

    memcpy(param_request_read.param_id,id,16);
    param_request_read.param_index = index;
    param_request_read.target_system = sysid;
    param_request_read.target_component = compid;

    mavlink_msg_param_request_read_encode(Current_sysID,Current_CompID, &msg,&param_request_read);
    SendMessage(msg);
}