#include "parameterprocess.h"

ParameterProcess::ParameterProcess(QObject *parent) : ThreadTemplet(parent)
{
    setRunFrq(10);

    status.m_Mode = Nop_Mode;
    status.recieve.type = _readtype::All;

}
ParameterProcess::~ParameterProcess()
{
    qDebug() << "stop parameter" << QThread::currentThreadId();
}

void ParameterProcess::process()//线程函数
{
    QThread::msleep(5000);//5s后再发送
    uint8_t count = 0;
    while (true)
    {
         count ++;
         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     : QThread::msleep(1000/frq());break;
            case RecieveMode  : ReadStateMachine();break;//QApplication::processEvents();break;
            case TransmitMode : WriteStateMachine();break;//QApplication::processEvents();break;
         }

         if(isInterruptionRequested())//退出
         {
             break;
         }
    }
}


void ParameterProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,uint8_t type)
{
    if(status.m_Mode == Nop_Mode)
    {
        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...";
    }
}

void ParameterProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,const char *id,uint8_t type,float value)
{
    if(status.m_Mode == Nop_Mode)
    {
        sysid = m_sysid;
        compid = m_compid;

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

        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...";




    }
}


void ParameterProcess::WriteCmd2(uint8_t m_sysid, uint8_t m_compid ,QVariant id,QVariant type,QVariant value)
{
    if(status.m_Mode == Nop_Mode)
    {

        sysid = m_sysid;
        compid = m_compid;

        float par = getvalue(value,gettype(type));

        WriteCmd(sysid,compid,getid(id.toString()),gettype(type),par);
    }
}

//这个函数类似中断，专门处理接收到的状态
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;

         emit showMessage(tr("收到参数 %1 %2 %3").arg(param_value.param_index).arg(setid(param_value.param_id)).arg(setvalue(param_value).toDouble()));
         qDebug() << "param_value" << param_value.param_index << param_value.param_value;

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

//写状态机
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(GCS_SysID,GCS_CompID, &msg,&param_set);
    Send(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(GCS_SysID,GCS_CompID, &msg,&param_value);
    Send(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(GCS_SysID,GCS_CompID, &msg,&param_ext_ack);
    Send(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(GCS_SysID,GCS_CompID, &msg,&param_ext_set);
    Send(msg);
}

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

    Q_UNUSED(value);


    mavlink_msg_param_ext_value_encode(GCS_SysID,GCS_CompID, &msg,&param_ext_value);
    Send(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(GCS_SysID,GCS_CompID, &msg,&param_request_list);
    Send(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(GCS_SysID,GCS_CompID, &msg,&param_request_read);
    Send(msg);
}




//================================

QVariant ParameterProcess::setvalue(mavlink_param_value_t param)
{
        QVariant value;

        switch (param.param_type)
        {
        case MAV_PARAM_TYPE_UINT8:
        {
            value = ((uint8_t *)&param.param_value)[0];
        }break;
        case MAV_PARAM_TYPE_INT8:
        {
            value = ((int8_t *)&param.param_value)[0];
        }break;
        case MAV_PARAM_TYPE_UINT16:
        {
           value = ((uint16_t *)&param.param_value)[0];
        }break;
        case MAV_PARAM_TYPE_INT16:
        {
            value = ((int16_t *)&param.param_value)[0];
        }break;
        case MAV_PARAM_TYPE_UINT32:
        {
            value = ((uint32_t *)&param.param_value)[0];
        }break;
        case MAV_PARAM_TYPE_INT32:
        {
            value = ((int32_t *)&param.param_value)[0];
        }break;
       case MAV_PARAM_TYPE_UINT64:
        {
#ifdef unix
            //value = ((ulong *)&param.param_value)[0];
#else
            value = ((uint64_t *)&param.param_value)[0];
#endif
        }break;
        case MAV_PARAM_TYPE_INT64:
        {
#ifdef unix
            //value = ((long *)&param.param_value)[0];
#else
            value = ((int64_t *)&param.param_value)[0];
#endif
        }break;
        case MAV_PARAM_TYPE_REAL32:
        {
            value = ((float *)&param.param_value)[0];
        }break;
        case MAV_PARAM_TYPE_REAL64:
        {
            value = ((double *)&param.param_value)[0];
        }break;
         default:
        {
            value = ((float *)&param.param_value)[0];
            //qDebug() << "Unsupported type " << param.param_type;
        }break;

        }

        return value;
}

float ParameterProcess::getvalue(QVariant param,uint8_t type)
{
        float value;
        union {
            uint8_t   u8[8];
            int8_t    i8[8];
            uint16_t  u16[4];
            int16_t   i16[4];
            uint32_t  u32[2];
            int32_t   i32[2];
            uint64_t  u64;
            int64_t   i64;
            float     f[2];
            double    d;
        }src;

        switch (type)
        {
        case MAV_PARAM_TYPE_UINT8:
        {
            src.u8[0] =  param.toUInt();
            ((uint8_t *)&value)[0] = src.u8[0];

        }break;
        case MAV_PARAM_TYPE_INT8:
        {
            src.i8[0] =  param.toUInt();
            ((int8_t *)&value)[0] = src.i8[0];
        }break;
        case MAV_PARAM_TYPE_UINT16:
        {
            src.u16[0] =  param.toUInt();
            ((uint16_t *)&value)[0] = src.u16[0];
        }break;
        case MAV_PARAM_TYPE_INT16:
        {
            src.i16[0] =  param.toUInt();
            ((int16_t *)&value)[0] = src.i16[0];
        }break;
        case MAV_PARAM_TYPE_UINT32:
        {
            src.u32[0] =  param.toUInt();
            ((uint32_t *)&value)[0] = (uint32_t)src.u32[0];
        }break;
        case MAV_PARAM_TYPE_INT32:
        {
            src.i32[0] =  param.toUInt();
            ((int32_t *)&value)[0] = (int32_t)src.i32[0];
        }break;
        case MAV_PARAM_TYPE_UINT64:
        {
            src.u64 =  param.toUInt();
            ((uint64_t *)&value)[0] = (uint64_t )src.u64;
        }break;
        case MAV_PARAM_TYPE_INT64:
        {
            src.i64 =  param.toUInt();
            ((int64_t *)&value)[0] = (int64_t)src.i64;
        }break;
        case MAV_PARAM_TYPE_REAL32:
        {
            src.f[0] =  param.toFloat();
            ((float *)&value)[0] = (float)src.f[0];
        }break;
        case MAV_PARAM_TYPE_REAL64:
        {
            src.d =  param.toDouble();
            ((double *)&value)[0]= (double)src.d;
        }break;
         default:
        {
            src.f[0] =  param.toFloat();
            ((float *)&value)[0] = (float)src.i32[0];
        }break;

        }

        return value;
}


QVariant ParameterProcess::settype(mavlink_param_value_t param)
{
        QVariant value = 0;

        switch (param.param_type)
        {
        case MAV_PARAM_TYPE_UINT8:
        {
            value = "uint8";
        }break;
        case MAV_PARAM_TYPE_INT8:
        {
            value = "int8";
        }break;
        case MAV_PARAM_TYPE_UINT16:
        {
            value = "uint16";
        }break;
        case MAV_PARAM_TYPE_INT16:
        {
            value = "int16";
        }break;
        case MAV_PARAM_TYPE_UINT32:
        {
            value = "uint32";
        }break;
        case MAV_PARAM_TYPE_INT32:
        {
            value = "int32";
        }break;
        case MAV_PARAM_TYPE_UINT64:
        {
            value = "uint64";
        }break;
        case MAV_PARAM_TYPE_INT64:
        {
            value = "int64";
        }break;
        case MAV_PARAM_TYPE_REAL32:
        {
            value = "float";
        }break;
        case MAV_PARAM_TYPE_REAL64:
        {
            value = "double";
        }break;
         default:
        {
            value = "unknow";
        }break;

        }

        return value;
}

uint8_t ParameterProcess::gettype(QVariant type)
{
        uint8_t value = 0;

        if(type.toString() == "uint8")
        {
            value = MAV_PARAM_TYPE_UINT8;
        }
        else if(type.toString() == "int8")
        {
            value = MAV_PARAM_TYPE_INT8;
        }
        else if(type.toString() == "uint16")
        {
            value = MAV_PARAM_TYPE_UINT16;
        }
        else if(type.toString() == "int16")
        {
            value = MAV_PARAM_TYPE_INT16;
        }
        else if(type.toString() == "uint32")
        {
            value = MAV_PARAM_TYPE_UINT32;
        }
        else if(type.toString() == "int32")
        {
            value = MAV_PARAM_TYPE_INT32;
        }
        else if(type.toString() == "uint64")
        {
            value = MAV_PARAM_TYPE_UINT64;
        }
        else if(type.toString() == "int64")
        {
            value = MAV_PARAM_TYPE_INT64;
        }
        else if(type.toString() == "float")
        {
            value = MAV_PARAM_TYPE_REAL32;
        }
        else if(type.toString() == "double")
        {
            value = MAV_PARAM_TYPE_REAL64;
        }
        else if(type.toString() == "unknow")
        {
            value = 0;
        }

        return value;
}


//浣跨敤QString
QString ParameterProcess::setid(char *id)//璁剧疆id,濡傛灉id灏忎簬16锛岄偅涔堣兘澶熸壘鍒°/0锛屽¦傛灉绛変簬鎴栬€呭ぇ浜庯紝閭ｄ箞浼氭壘涓嶅埌锛屽洜姝や贡鐮
{
     QString Name;

     Name.append(id);

     if(Name.size() >= 16)
     {
         Name = Name.left(16);
     }

     return Name;
}

char * ParameterProcess::getid(QString id)//寰楀埌id
{
     return id.toLatin1().data();
}