#include "portbridge.h"

#include <QVector>
#include <QDebug>

#include <QNetworkDatagram>

#include "xlru_serial_over_udp.h"

#include <QLinkedList>
#include <QTimer>

class MyFifo: public QLinkedList<char>
{
public:
    bool pending() const
    {
        return size()>0;
    }
    char pop()
    {
        char c=this->first();
        this->pop_front();
        return c;
    }
    void push(const char* d, unsigned int len)
    {
        for(unsigned int i=0; i<len; ++i)
        {
            this->push_back(d[i]);
        }
    }
};

class PortBridge::Implementation
{
public:
    QVector<PortBridgeForwarder*> forwarder;

    unsigned long forwardIn;
    unsigned long forwardOut;

    LruSerialFilter* masterFilter;

    QVector<LruSerialPort*> udpForward;
    QVector<bool> udpSlow;

    xlru::xlru_udp_slave_t slave_pkt;

    QHostAddress slave_address;
    unsigned short slave_port;

    unsigned int udpIn;
    unsigned int udpOut;
};

PortBridge::PortBridge(QObject *parent):
    QObject(parent),
    forwardDisabled(false),
    p_(*new Implementation),
    s(0)
{
    p_.forwardOut=0;
    p_.forwardIn=0;
    p_.udpIn=0;
    p_.udpOut=0;
    p_.masterFilter=0;

    udpEnabled=true;
}

void PortBridge::close()
{
    foreach(PortBridgeForwarder* f, p_.forwarder)
    {
        f->disconnect();
        delete f;
    }

    this->disconnect();
}

void PortBridge::setMasterFilter(LruSerialFilter* f)
{
  p_.masterFilter=f;
}

void PortBridge::setSocket(QUdpSocket * sk, unsigned int port)
{
    s=sk;
    connect(s, &QUdpSocket::readyRead, this, &PortBridge::udpReadyRead);
    if (port==0)
    {
        port=51010;
    }
    p_.slave_port=port;
    unsigned long broadcast=s->localAddress().toIPv4Address();
    broadcast|=0x00FF;
    p_.slave_address=QHostAddress(broadcast);
}

void PortBridge::udpEnable(bool enable)
{
    udpEnabled=enable;
}

void PortBridge::addUdpBridge(LruSerialPort *s0, bool slaveSlow)
{
    p_.udpSlow.append(slaveSlow);
    p_.udpForward.append(s0);

    //connect(&s0->realPort, &QSerialPort::readyRead, this, &PortBridge::serialUdpReadyRead);
    //connect(s0, &LruSerialPort::readyRead, this, &PortBridge::serialUdpReadyRead);
}

void PortBridge::udpReadyRead()
{
    uint32_t dst=0;
    QHostAddress srcHost;
    //int srcPort=0;

    while(s->hasPendingDatagrams())
    {
        QNetworkDatagram d=s->receiveDatagram();

        if (!udpEnabled)
            continue;

        //unsigned int size=d.data().size();
        const xlru::xlru_udp_master_t& pkt=*reinterpret_cast<const xlru::xlru_udp_master_t*>(d.data().constData());
        if (pkt.marker==xlru::xlru_udp_master_t::k_marker)
        {
            srcHost=d.senderAddress();
            dst=d.destinationAddress().toIPv4Address();
            //srcPort=d.destinationPort();
            //unsigned int total_size=pkt.payload_size;
            p_.udpIn+=pkt.payload_size;
            QByteArray data((const char*)pkt.payload, pkt.payload_size);
            qDebug()<<"UDPIN"<<dst<<data;
            if (p_.masterFilter)
                p_.masterFilter->processMessages(data, 0);
        }
    }
#if 0
    qDebug()<<"UDPIN"<<dst<<srcHost;
    if (dst==0)
    {
        static xlru::xlru_udp_slave_t reply;
        reply.marker=xlru::xlru_udp_slave_t::k_marker;
        reply.override_enable_mask=0;
        reply.payload_size=0;
        s->writeDatagram((const char*)&reply, sizeof reply, srcHost, srcPort);
    }
#endif
}

unsigned int PortBridge::udpCharOut()
{
    return p_.udpOut;
}

unsigned int PortBridge::udpCharIn()
{
    return p_.udpIn;
}

void PortBridge::serialUdpReadyRead()
{
    xlru::xlru_udp_slave_t& pkt=p_.slave_pkt;
    pkt.marker=xlru::xlru_udp_slave_t::k_marker;
    pkt.override_enable_mask=0;
    xlru::xlru_udp_data_t* d=reinterpret_cast<xlru::xlru_udp_data_t*>(pkt.payload);

    LruSerialPort* s0=qobject_cast<LruSerialPort*>(sender());
    if (!s0)
        return;

    for(;;)
    {
        QByteArray data=s0->readData();
        int size=data.size();
        p_.forwardIn+=size;
        if (size)// && !fDisable)
        {
            qDebug()<<"MASTER IN:"<<s0->portName()<<data;

            unsigned int current_size=d->len;

            if ((current_size+size)<900)
            {
                d->source=1;
                memcpy(&d->data[d->len], data.constData(), size);
                d->len+=size;
            }
        }
        else
            break;
    }
    if (d->len)
        s->writeDatagram(reinterpret_cast<const char*>(&pkt), sizeof pkt, p_.slave_address, p_.slave_port);
}

 void PortBridge::addBridge(LruSerialPort* s0, LruSerialPort* s1, bool slaveSlow)
{
    PortBridgeForwarder* fw=0;

    foreach (PortBridgeForwarder* i, p_.forwarder)
    {
        if (i->masterPort==s1)
        {
            fw=i;
            break;
        }
    }
    if (fw==0)
    {
       qDebug()<<"New Forwarder:"<<s0->portName()<<"z->"<<s1->portName();
       fw=new PortBridgeForwarder;
       fw->portBridge=this;
       fw->setMaster(s1);
       fw->addSlave(s0, slaveSlow);
       p_.forwarder.append(fw);
       return; //fw->controlWidget();
    }
    else
    {
        qDebug()<<"Add Slave:"<<s0->portName()<<"<->"<<fw->masterPort->portName();
        fw->portBridge=this;
        fw->addSlave(s0, slaveSlow);
        return;
    }

}

void PortBridge::forwardCount(int inbytes, int outbytes)
{
    p_.forwardIn+=inbytes;
    p_.forwardOut+=outbytes;
}

void PortBridge::getForwardCounter(int& in, int& out)
{
    in=p_.forwardIn;
    out=p_.forwardOut;
}

class PortBridgeForwarder::Implementation
{
public:
    QVector<MyFifo> slowFifo;
    QTimer slowTimer;

    QByteArray masterMsg;
    QVector<QByteArray> slavesMsg;

};

PortBridgeForwarder::PortBridgeForwarder(QObject *parent):
    QObject(parent),
    p_(*new Implementation)
{
    connect(&p_.slowTimer, &QTimer::timeout, this, &PortBridgeForwarder::slowTimeout);
    p_.slowTimer.setSingleShot(true);
    p_.masterMsg.reserve(256);
}

void PortBridgeForwarder::setMaster(LruSerialPort* master)
{
    masterPort=master;
    //connect(&master->realPort, &QSerialPort::readyRead, this, &PortBridgeForwarder::serialForwardToSlaves);
    connect(master, &LruSerialPort::readyRead, this, &PortBridgeForwarder::serialForwardToSlaves);
}

void PortBridgeForwarder::addSlave(LruSerialPort* slave, bool slaveSlow)
{
    slavePort.append(slave);
    slaveSlowFlags.append(slaveSlow);
    p_.slowFifo.append(MyFifo());
    p_.slavesMsg.append(QByteArray());
    //connect(&slave->realPort, &QSerialPort::readyRead, this, &PortBridgeForwarder::serialForwardToMaster);
    connect(slave, &LruSerialPort::readyRead, this, &PortBridgeForwarder::serialForwardToMaster);
}

void PortBridgeForwarder::slowTimeout()
{
  bool pending=false;
  for(int i=0; i<p_.slowFifo.size(); ++i)
  {
      MyFifo& fifo=p_.slowFifo[i];
      if (fifo.size())
      {
          pending=true;
          char c=fifo.first();
          fifo.pop_front();
          if (c)
            slavePort[i]->writeChar(c); //write(&c, 1);
      }
  }
  if (pending)
      p_.slowTimer.start(10);
}

void PortBridgeForwarder::serialForwardToSlaves()
{
    int total_in=0;
    int total_out=0;

    LruSerialPort* s0=masterPort;
    if (!s0)
        return;

    int n=slavePort.size();

    bool fDisable=portBridge->forwardDisabled;

    bool slowPushed=false;

    for(;;)
    {
        QByteArray data=s0->readData();
        int size=data.size();
        total_in+=size;
        if (size && !fDisable)
        {
            //qDebug()<<"MASTER IN:"<<s0->portName()<<data;
            p_.masterMsg.append(data);
            if ((p_.masterMsg.size()>80) || data.contains('\r'))
            {
                qDebug()<<"MASTER IN:"<<masterPort->portName()<<p_.masterMsg;
                p_.masterMsg.resize(0);
            }
            for(int i=0; i<n; ++i)
            {
                LruSerialPort* slave=slavePort[i];
                qint64 written=0;
                qDebug()<<"Slave forward:"<<slave->portName();

                //scrive su tutte le com in slave la stringa ricevuta
                if (slaveSlowFlags[i])
                {
                    //qDebug()<<"slow send";
                    const char* d=data.constData();
                    p_.slowFifo[i].push(d, size);
                    slowPushed=true;
#if 0
                    for(int n=0; n<size; ++n)
                    {
                        slave->write(&d[n], 1);
                        slave->flush();
                    }
#endif
                }
                else
                    written=slave->write(data);

                if (written>=0)
                    total_out+=written;
            }
        }
        else
            break;
    }
    for(int i=0; i<n; ++i)
    {
        LruSerialPort* slave=slavePort[i];
        slave->flush();
    }
    if (slowPushed)
    {
        if (!p_.slowTimer.isActive())
            p_.slowTimer.start(10);
    }

    portBridge->forwardCount(total_in, total_out);
}

void PortBridgeForwarder::serialForwardToMaster()
{
    int total_in=0;
    int total_out=0;

    bool fDisale=portBridge->forwardDisabled;

    LruSerialPort* s0=qobject_cast<LruSerialPort*>(sender());
    if (!s0)
        return;

    for(;;)
    {
        QByteArray data=s0->readAll();
        int size=data.size();
        total_in+=size;
        if (size && !fDisale)
        {
#if 0
            qDebug()<<"SLAVE IN:"<<s0->portName()<<data;
#else
            //qDebug()<<"SLAVE IN:"<<s0->portName()<<data;
            s0->tempBuffer.append(data);
            if ((s0->tempBuffer.size()>80) || s0->tempBuffer.contains('\r'))
            {
                qDebug()<<"SLAVE IN:"<<s0->portName()<<s0->tempBuffer;

                //masterPort->inFilter->processMessages(msg);

                s0->tempBuffer.clear();
            }
#endif
            masterPort->inFilter->processMessages(data, s0->unitMonitor);

            qint64 written=masterPort->write(data);
            if (written>=0)
			{
                total_out+=written;
                //qDebug()<<"SLAVE OUT:"<<written<<data;
            }
        }
        else
            break;
    }
    masterPort->flush();

    portBridge->forwardCount(total_in, total_out);
}
