#include "rcprocess.h"


rcprocess::rcprocess(QObject *parent) : ThreadTemplet(parent)
{
    setRunFrq(0.001);//20hz


    rcin[0] = 1500;
    rcin[1] = 1500;
    rcin[2] = 1100;
    rcin[3] = 1500;
    rcin[4] = 1100;
    rcin[5] = 1100;
    rcin[6] = 1100;
    rcin[7] = 1100;
    rcin[8] = 1100;
    rcin[9] = 1100;
    rcin[10] = 1500;
    rcin[11] = 1100;
    rcin[12] = 1100;
    for (int i=13; i<20; ++i)
    {
        rcin[i] = 1100;
    }

    isRCActive = 0;

}

rcprocess::~rcprocess()
{
    qDebug() << "stop rc" << QThread::currentThreadId();
}

//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
void rcprocess::process()//线程函数
{
    QThread::msleep(5000);//5s后再发送
    uint64_t lastTime = 0;
    uint8_t count = 0;
    QByteArray datagram = nullptr;
    while (true)
    {
         count ++;

         QThread::msleep(1000/frq());

         //解码从串口来的
         datagram.clear();
         datagram = readbuff(SourceType::s_port);//每次全部读取


         if(datagram.size() > 0)
         {
              QApplication::processEvents();
              parse(SourceType::s_port,datagram);
         }

         quint64 currentTimestamp = (quint64)QDateTime::currentMSecsSinceEpoch();

         if((currentTimestamp - lastTime) > 66)
         {
             lastTime = currentTimestamp;
             update15Hz();
         }

         if(isInterruptionRequested())//退出
         {
             break;
         }
         //QThread::yieldCurrentThread();
    }


}



void rcprocess::parse(quint32 src,QByteArray datagram)
{
    for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
    {
        if(sbus.parse_char((uint8_t)*i))
        {
            rcin_update = true;

            if(isRCActive == 1)
            {
                for (int i=0; i<18; ++i)
                {
                    rcin[i] = sbus.Channel[i+1];
                }

                rcin_cnt = 13;
            }



            sbuslist.clear();
            for (int i=0; i<13; ++i)
            {
                sbuslist.append(sbus.Channel[i]);
            }

            emit new_remote_ctrl(sbuslist);


        }
    }
}


void rcprocess::update15Hz(void)
{
    uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
    quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;
    qToBigEndian(currentTimestamp, buff);
    mavlink_message_t msg;

    if (rcin_update)
    {
        if ((isRCActive != 0) || rcin_cnt > 0)
        {
            rcin_cnt--;
            mavlink_rc_channels_override_t command;
            command.chan1_raw = rcin[0];
            command.chan2_raw = rcin[1];
            command.chan3_raw = rcin[2];
            command.chan4_raw = rcin[3];
            command.chan5_raw = rcin[4];
            command.chan6_raw = rcin[5];
            command.chan7_raw = rcin[6];
            command.chan8_raw = rcin[7];
            command.chan9_raw = rcin[8];
            command.chan10_raw = rcin[9];
            command.chan11_raw = rcin[10];
            command.chan12_raw = rcin[11];
            command.chan13_raw = rcin[12];
            command.chan14_raw = rcin[13];
            command.chan15_raw = rcin[14];
            command.chan16_raw = rcin[15];
            command.chan17_raw = rcin[16];
            command.chan18_raw = rcin[17];
            command.target_component = (uint8_t)isRCActive;
            command.target_system = sysid;
            mavlink_msg_rc_channels_override_encode(GCS_SysID,GCS_CompID,&msg,&command);


            Send(msg);
            rcin_update = false;
        }

    }
}

void rcprocess::SoftRCUpdate(QList<uint16_t> rc)
{
    if(isRCActive == 2)
    {
        rcin_update = true;
        for (int i=0; i<18; ++i)
        {
            rcin[i] = rc.at(i+1);

        }

        rcin_cnt = 10;
    }
}