#include "qg1553overudp.h"

#include "qgscopechrono.h"
#include "qgscopeworkbench.h"
#include "qgscopechannel.h"
#include "qglogger.h"

#include "qgrexnetwork.h"

#include <QUdpSocket>

#include <QThread>
#include <QDebug>

#include "th_b1553_icd.h"

union ctrl_word_t
{
    struct coded_ctrl_word_t
    {
        uint16_t rt: 5;
        uint16_t tr: 1;
        uint16_t sa: 5;
        uint16_t wc: 5;
    } coded;
    uint16_t raw;
};

struct b1553_raw_message_t
{
    uint16_t ctrl_word; //real type: ctrl_word_t
    uint16_t data[32];
};

struct b1553_datagram_t
{
    uint16_t number_of_messages;
    b1553_raw_message_t msgs[20];
};

class Qg1553OverUdp::Implementation
{
public:
    QThread workerThread;

    QUdpSocket s;

    QgLogger logger;

    b1553_raw_message_t* msgmap[32];

    b1553_datagram_t msg;

    QgScopeChronoTimer* scheduler;

    unsigned int frame_counter;

    QHostAddress rtAddress;
    template<class T_> void addMsg(int sa, unsigned int tr)
    {
        static int msg_counter;
        msgmap[sa]=&msg.msgs[msg_counter];
        msg.msgs[msg_counter].ctrl_word=0;
        ctrl_word_t tmp;
        tmp.raw=0;
        tmp.coded.rt=20;
        tmp.coded.sa=sa;
        unsigned int sz=sizeof(T_);
        sz=sz/2;
        tmp.coded.wc=static_cast<uint16_t>(sz);
        tmp.coded.tr=tr;
        msg.msgs[msg_counter].ctrl_word=tmp.raw;
        ++msg_counter;
        msg.number_of_messages=msg_counter;
    }

    void BusGenerator()
    {
        QgRexNetwork nw;

        bool ok=s.bind(nw.networkInterface()/*QHostAddress::LocalHost*/, 60000+1553);
        if (!ok)
        {
            ok=s.bind(nw.networkInterface()/*QHostAddress::LocalHost*/);

        }

        s.setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption, 32*1024);
        s.setSocketOption(QAbstractSocket::SendBufferSizeSocketOption, 32*1024);

        rtAddress=nw.networkBroadcast();

        //s.connectToHost(QgRexNetwork().networkInterface() /*QHostAddress::LocalHost*/, 50000+1553);

        addMsg<icd1553::msg_rdr_settings_and_parameters_t>(1, 0);
        addMsg<icd1553::msg_rdr_operation_command_t>(2, 0);
        addMsg<icd1553::msg_graphic_setting_t>(3, 0);
        addMsg<icd1553::msg_nav_data_and_cursor_t>(4, 0);
        addMsg<icd1553::msg_inu_high_speed_t>(5, 0);

        addMsg<icd1553::msg_settings_tellback_t>(15, 1);
        addMsg<icd1553::msg_rdr_status_tellback_t>(16, 1);

    }

    void doWork()
    {
        //s.write((const char*)&msg, sizeof msg);
        /*qint64 res=*/s.writeDatagram(reinterpret_cast<const char*>(&msg), sizeof msg, rtAddress, 50000+1553);

        ++frame_counter;
    }

    Qg1553OverUdpWorker workerClass;

    Implementation(Qg1553OverUdp* parent):
        logger("B1553"),
        workerClass(parent)
    {
        frame_counter=0;
        BusGenerator();
    }

};

Qg1553OverUdpWorker::Qg1553OverUdpWorker(Qg1553OverUdp* bc):
    b(bc)
{
}

void Qg1553OverUdpWorker::bcFrameGenerate()
{
    //b->p_.logger.info("!!!!");
    b->p_.doWork();
}

void Qg1553OverUdpWorker::bcFrameReceive()
{
}

void Qg1553OverUdpWorker::bcFrameWritten(qint64)
{
}

Qg1553OverUdp::Qg1553OverUdp(QObject* /*parent*/):
    QgScopeDevice("b1553udp.grifo", "B1553"),
    //QObject(parent),
    p_(*new Implementation(this))
{
    p_.workerClass.moveToThread(&p_.workerThread);

    p_.scheduler=&this->chronoTimer();
    p_.scheduler->moveToThread(&p_.workerThread);

    connect(&p_.s, &QUdpSocket::readyRead, &p_.workerClass, &Qg1553OverUdpWorker::bcFrameReceive);
    connect(&p_.s, &QUdpSocket::bytesWritten, &p_.workerClass, &Qg1553OverUdpWorker::bcFrameWritten);
    connect(p_.scheduler, &QgScopeChronoTimer::periodExpired, &p_.workerClass, &Qg1553OverUdpWorker::bcFrameGenerate);

    p_.workerThread.start();

    p_.scheduler->start(10000);


    startTimer(100);
}

void* Qg1553OverUdp::getBuffer(int subaddress)
{
    return &(p_.msgmap[subaddress]->data[0]);
}

void Qg1553OverUdp::timerEvent(QTimerEvent* /*evn*/)
{
    static unsigned int p_fc;
    if (p_fc!=p_.frame_counter)
    {
        p_fc=p_.frame_counter;
        //p_.logger.info(QString("%1").arg(p_fc));
        QgScopeDevice::setStatusString(QString("%1").arg(p_fc));
    }
}
