#include "dlink.h"
#include <string.h>
//#include "windows.h"
#include "QDateTime"



DLink::DLink(QObject *parent) : QObject(parent)
{
     qDebug() << "Dlink " <<  QThread::currentThreadId();

     mavlinknode = new MavLinkNode();
}
DLink::~DLink()
{
    mavlinknode->stop();
    delete mavlinknode;
}


int DLink::SendMessageTo(uint8_t, uint8_t *msg, uint16_t len)
{
    if (DLink::Clientsock)
    {
        foreach(Node node,clientSockets)
        {
            DLink::Clientsock->writeDatagram((const char *)msg, len,
                                       node.addr, node.port);
        }
    }

    if (DLink::serialPort)
    {
        DLink::serialPort->write((const char *)msg,len);
    }

    return 0;
}



void DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity parity)
{
    if (serialPort)
    {
        serialPort->close();
        delete serialPort;
    }
    serialPort = new QSerialPort(port,this);

    if (serialPort->open(QIODevice::ReadWrite))
    {
        serialPort->setBaudRate(baudrate);
        serialPort->setParity(parity);
        serialPort->setDataBits(QSerialPort::Data8);
        serialPort->setStopBits(QSerialPort::OneStop);

        mavlinknode->start();

        connect(serialPort, SIGNAL(readyRead()), this, SLOT(readPendingDatagramsSerialPort()));

        qDebug() << "Serial Port Open Success";
    }
    else
    {
        delete serialPort;
        serialPort = nullptr;
    }
}

bool DLink::statesPort()
{
    return (serialPort != nullptr)?(true):(false);
}

void DLink::stopPort()
{
    serialPort->close();
    delete serialPort;
    serialPort = nullptr;
}



void DLink::setupClient(const QHostAddress &remote_addr, int remote_port, int local_port)
{    
    Clientsock = new QUdpSocket(this);
    if (Clientsock->bind(QHostAddress::Any, local_port))
    {
        if(Clientsock->open(QIODevice::ReadWrite))
        {
            connect(Clientsock, SIGNAL(readyRead()),
                    this, SLOT(readPendingDatagramsClient()));
            Node node;
            node.sock = Clientsock;
            node.addr = remote_addr;
            node.port = remote_port;

            clientSockets.append(node);
            qDebug() << "UdpSocket open:"
                     << remote_addr
                     << local_port
                     << remote_port;

            mavlinknode->start();
        }
        else
        {
            qDebug() << "sock not open";
        }
    }
    else
    {
        qWarning("sock is not binded.");
        delete Clientsock;
    }

}

void DLink::readPendingDatagramsSerialPort(void)
{
    QByteArray datagram = serialPort->readAll();
    mavlinknode->setbuff(SourceType::s_port,datagram);
}

void DLink::readPendingDatagramsClient(void)
{
   //轮番查询
    while (Clientsock->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(Clientsock->pendingDatagramSize());
        Clientsock->readDatagram(datagram.data(), datagram.size());
        mavlinknode->setbuff(SourceType::c_sock,datagram);
    }

}