Geonet.cpp

Go to the documentation of this file.

#include <Geonet.h>

quint16 GeoNet::sequenceNumber = 0;

GeoNet::GeoNet()
{
    initGeoNetErrors();
    m_manager = 0;
    m_paramGPS = 0;
    m_paramCAN = 0;
    m_paramAppli = 0;
    m_paramConf = 0;
    m_paramHybrid = 0;
    m_RP = 0;
    m_locationTable = 0;
    m_HPB = 0;
    m_BCFPB = 0;
    m_UCFPB = 0;
    m_CBFPB = 0;
    m_RPB = 0;
    m_LSPB = 0;
    m_angleThreshold = itsGnBroadcastCBFDefSectorAngle;
    m_mutexLocT = new QMutex;
    m_hybridFlag = false;
}

GeoNet::GeoNet(Manager* m, bool geonetFlag, bool hybridFlag, QMutex* mutex) :
    m_geonetFlag(geonetFlag),
    m_securityManager(m)
{
    initGeoNetErrors();

    m_mutex = mutex;
    QMutexLocker locker(m_mutex);
    m_manager = m;
    m_paramGPS = (FakeGPSProvider*) m->hashMap["GPS"];
    m_paramCAN = (FakeCANProvider*) m->hashMap["CAN"];
    m_paramAppli = (FakeApplicationProvider*) m->hashMap["Application"];
    m_paramConf = (ParameterConfiguration*) m->hashMap["Configuration"];
    m_paramUpperTester = (ParameterUpperTester*) m->hashMap["UpperTester"];
    m_paramHybrid = (ParameterHybrid*) m->hashMap["Hybrid"];
    m_gnAddr = (GnAddr*) m->hashMap["GnAddr"];
    m_paramNetwork = (ParameterNetwork*) m->hashMap["Network"];
    if(m_paramConf->itsGnSecurity())
    {
        m_paramSecu = (ParameterSecurity*) m->hashMap["Security"];
        m_signatureSocket = new QUdpSocket;
        m_verifySocket = new QUdpSocket;
        m_signatureSocket->open(QIODevice::ReadWrite);
        m_verifySocket->open(QIODevice::ReadWrite);
    }
    else
        m_paramSecu = 0;
    m_angleThreshold = itsGnBroadcastCBFDefSectorAngle;
    m_mutexLocT = new QMutex;
    m_hybridFlag = hybridFlag;
    printf("Geonet constructor UNLOCKED\n");
    if(m_hybridFlag)
        initHybridSocket();

    if(InitRawSocket_send() != networkError_success)
        printf("RawSocketSend can't be initialized\n");

    gen2fac = new QUdpSocket(this);

    if(m_geonetFlag)
    {
        m_RP = new ReceivePackets(m_paramConf);
        QObject::connect(m_RP, SIGNAL(packetReceived(QByteArray)), this, SLOT(treatPackets(QByteArray)));
        QThread* threadReceive = new QThread;
        m_RP->moveToThread(threadReceive);
        QObject::connect(threadReceive, SIGNAL(started()), m_RP, SLOT(receive()));
        threadReceive->start();
    }

    m_locationTable = new QHash<QByteArray, LocationTableEntry>();


    m_HPB = new HistoryPacketBuffer(m_gnAddr);

    m_BCFPB = new BroadcastForwardingPacketBuffer();

    m_UCFPB = new UnicastForwardingPacketBuffer();

    m_CBFPB = new CBFPacketBuffer(m_paramNetwork, m_locationTable, m_gnAddr, m_paramGPS, m_HPB, &sequenceNumber, &m_CBFPBCounter);
    QObject::connect(m_CBFPB, SIGNAL(lpvSent()), this, SLOT(lpvUpdate()));

    m_RPB = new RepetitionPacketBuffer;
    QObject::connect(m_RPB, SIGNAL(sendPacketToGn(gnDataRequest_t)), this, SLOT(sendPacketFromRPB(gnDataRequest_t)));

    m_LSPB = new LocationServicePacketBuffer;

    if(m_paramUpperTester != 0)
    {
        gen2ut = new QUdpSocket(this);
        gen2ut->bind(m_paramUpperTester->upperTesterEventIndicationWritePort());
    }
    else
        gen2ut = 0;
}

void GeoNet::initHybridSocket()
{
    m_paramHybrid = new ParameterHybrid();

    QObject::connect(this, SIGNAL(packetReceived(QByteArray)), this, SLOT(treatPackets(QByteArray)));
    if(m_paramHybrid->sslUsed())
        setupSecureSocket();
    else
        setupUnsecureSocket();
    m_tcpSocket->setSocketOption(QAbstractSocket::KeepAliveOption, 1);
    QObject::connect(this, SIGNAL(packetReceived(QByteArray)), this, SLOT(treatPackets(QByteArray)));
}

void GeoNet::setupSecureSocket()
{
    m_sslSocket = new QSslSocket;

    QObject::connect(m_sslSocket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(socketError(QAbstractSocket::SocketError)));
    QObject::connect(m_sslSocket, SIGNAL(sslErrors(QList<QSslError>)), this, SLOT(sslErrors(QList<QSslError>)));
    QObject::connect(m_sslSocket, SIGNAL(readyRead()), this, SLOT(socketReadyRead()));

    m_sslSocket->connectToHostEncrypted(m_paramHybrid->serverAddress(), m_paramHybrid->serverPort());
    m_sslSocket->waitForConnected();
    m_tcpSocket = m_sslSocket;
}

void GeoNet::sslErrors(const QList<QSslError> &errors)
{
    int i = 1;
    for(const auto &error : errors)
        qDebug() << "SSL Error" << i++ << error.errorString();

    if(m_tcpSocket->state() != QAbstractSocket::ConnectedState)
    {
        qDebug() << "Deactivation of hybridization due to error(s)";
        m_hybridFlag = false;
    }
}

void GeoNet::socketError(QAbstractSocket::SocketError)
{
    qDebug() << "Socket error" << m_tcpSocket->errorString();
}

void GeoNet::socketReadyRead(){emit packetReceived(m_tcpSocket->readAll().prepend(14, 0));}
/*
{
    QByteArray payload = m_tcpSocket->readAll();
    char *btp = payload.data();
    QByteArray payloadWithoutBTP = QByteArray(btp + 4, payload.length() - 4);
    quint16 port = extractUInt(btp, 2);

    gen2fac->writeDatagram(payloadWithoutBTP, QHostAddress::LocalHost, port);
}
*/

void GeoNet::setupUnsecureSocket()
{
    m_tcpSocket = new QTcpSocket;

    QObject::connect(m_tcpSocket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(socketError(QAbstractSocket::SocketError)));
    QObject::connect(m_tcpSocket, SIGNAL(readyRead()), this, SLOT(socketReadyRead()));

    qDebug() << "Connecting to"  << QHostAddress(m_paramHybrid->serverAddress()).toString() << m_paramHybrid->serverAddress() << ":" << m_paramHybrid->serverPort();
    m_tcpSocket->connectToHost(m_paramHybrid->serverAddress(), m_paramHybrid->serverPort(), QIODevice::ReadWrite);
    m_tcpSocket->waitForConnected();
    if(m_tcpSocket->state() == QAbstractSocket::ConnectedState)
        qDebug() << "Connection success" ;
    else
        qDebug() << "Connection failed" << m_tcpSocket->errorString();
}

GeoNet::~GeoNet()
{
    close(m_paramNetwork->rawSocket);
}

void GeoNet::execute()
{
    QMutexLocker locker(m_mutex);
    QMutexLocker locker2(m_mutexLocT);
    m_beacon = new QTimer;
    QObject::connect(m_beacon, SIGNAL(timeout()), this, SLOT(sendBeacon()));
    m_beacon->setSingleShot(true);
    m_beacon->setInterval(itsGnBeaconServiceRetransmitTimer + (qrand() % itsGnBeaconServiceMaxJitter));
    m_beacon->start();
}

void GeoNet::calculAngleThreshold()
{
    double aireCommunication = M_PI * pow(itsGnDefaultMaxCommunicationRange, 2);
    int nbNeighbour = 0;
    for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();i != m_locationTable->end(); ++i)
    {
        if(i.value().isNeighbour() != 0 && *(i.value().isNeighbour()))
            nbNeighbour++;
    }

    double ratio = nbNeighbour/aireCommunication;

    if(ratio < 0.025)
        m_angleThreshold = 30;
    else if(ratio < 0.05)
        m_angleThreshold = 45;
    else
        m_angleThreshold = 60;

}

void GeoNet::verifLocT()
{
    QMutexLocker locker(m_mutexLocT);
    int j = 0;
    if(m_locationTable)
    {
        printf("m_locationTable size %d\n", m_locationTable->size());
        if(!m_locationTable->isEmpty())
            for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();j < m_locationTable->size(); j++)
            {
                if(LocationTableEntry(i.value()).isPending())
                {
                    i++;
                    continue;
                }
                quint64 last = LocationTableEntry(i.value()).lastTimestamp();
                quint64 end = last + itsGnLifetimeLocTE * 1000;
                quint64 now = m_paramGPS->timestamp() % 0x100000000;
                if(end <= now)
                {
                    i = m_locationTable->erase(i);
                    calculAngleThreshold();
                }
                else
                    i++;
            }
    }
}

void GeoNet::utInitialize()
{
    if(m_geonetFlag)
        m_RP->m_changeFilter = true;
    QMutexLocker locker(m_mutexLocT);
    m_locationTable->clear();
    locker.unlock();

    m_HPB->clear();
    m_RPB->clear();
    m_BCFPB->clear();
    m_CBFPB->clear();
    m_CBFPBCounter.clear();
    m_LSPB->clear();
    m_UCFPB->clear();
    sequenceNumber = 0;
    m_beacon->start();
}

void GeoNet::lpvUpdate()
{
    m_beacon->start();
}

void GeoNet::initGeoNetErrors()
{
    geonetErrors[geonetError_success] = QString("Geonet Success\n");

    // Send Errors
    // Common Header Errors
    geonetErrors[geonetError_send_ch_headerType] = QString("CommonHeader Sending Error : Unknown HeaderType.\n");
    // Anycast
    geonetErrors[geonetError_send_ch_headerSubType_geoAnycast_unknown] = QString("CommonHeader Sending Error : HeaderSubType GeoAnycast unknown.\n");
    // Broadcast
    geonetErrors[geonetError_send_ch_headerSubType_geoBroadcast_unknown] = QString("CommonHeader Sending Error : HeaderSubType GeoBroadcast unknown.\n");
    // Topological
    geonetErrors[geonetError_send_ch_headerSubType_TSB_unknown] = QString("CommonHeader Sending Error : HeaderSubType TSB unknown.\n");

    // LSRequest Send Errors
    geonetErrors[tmp_geonetError_send_LSRequest_securedPacket] = QString("TMP LSRequest Processing Error : Can't send secured packet.\n");
    geonetErrors[geonetError_send_LSRequest_successfullyRespond] = QString("LSRequest Processing Error : Successfuly received a response.\n");
    geonetErrors[geonetError_send_LSRequest_noResponse] = QString("LSRequest Processing Error : Maximum repetition time reached without response.\n");
    geonetErrors[geonetError_send_LSRequest_buffered] = QString("LSRequest Sending : Packet buffered in LSPB.\n");

    // LSReply Send Errors
    geonetErrors[geonetError_send_LSReply_destinationUnknown] = QString("LSReply Sending : Destination not found.\n");
    geonetErrors[tmp_geonetError_send_LSReply_securedPacket] = QString("TMP LSRequest Processing Error : Can't send secured packet.\n");
    geonetErrors[geonetError_send_LSReply_bufferedPacket] = QString("LSReply Sending : Packet buffered.\n");
    geonetErrors[geonetError_send_LSReply_discardedPacket] = QString("LSReply Sending : Packet discarded.\n");
    geonetErrors[geonetError_send_LSReply_destinationAddress] = QString("LSReply Sending Error : DestinationAddress isn't MAC.\n");
    // LSReply Forward Errors
    geonetErrors[geonetError_forward_LSReply_bufferedPacket] = QString("LsReply Forwarding : Packet buffered.\n");
    geonetErrors[geonetError_forward_LSReply_discardedPacket] = QString("LsReply Forwarding : Packet discarded.\n");
    geonetErrors[geonetError_forward_LSReply_destinationAddress] = QString("LsReply Forwarding Error : DestinationAddress isn't MAC.\n");

    // GAC Send Errors
    geonetErrors[tmp_geonetError_send_gac_securedPacket] = QString("TMP GAC Processing Error : Can't send secured packet.\n");
    geonetErrors[geonetError_send_gac_noDestinationArea] = QString("GAC Sending Error : No destination area.\n");
    geonetErrors[geonetError_send_gac_areaSize] = QString("GAC Sending Error : Area size is too large.\n");
    geonetErrors[geonetError_send_gac_unknownCommunicationProfile] = QString("GAC Sending Error : Communication Profile isn't G5A.\n");
    geonetErrors[geonetError_send_gac_unicast_bufferedPacket] = QString("GAC Sending : Packet buffered in UCPB.\n");
    geonetErrors[geonetError_send_gac_unicast_discardedPacket] = QString("GAC Sending : Packet discarded after unicast.\n");
    geonetErrors[geonetError_send_gac_broadcast_bufferedPacket] = QString("GAC Sending : Packet buffered in BCPB.\n");
    geonetErrors[geonetError_send_gac_broadcast_discardedPacket] = QString("GAC Sending : Packet discarded after broadcast.\n");
    geonetErrors[geonetError_send_gac_destinationAddress] = QString("GAC Sending Error : DestinationAddress isn't MAC.\n");
    geonetErrors[geonetError_send_gac_noAreaLineForwarding] = QString("GAC Sending Error : Area line forwarding is not enable, can't send.\n");
    // GAC Forward Errors
    geonetErrors[geonetError_forward_gac_areaSize] = QString("GAC Forwarding Error : Area size is too large.\n");
    geonetErrors[geonetError_forward_gac_bufferedPacket] = QString("GAC Forwarding : Packet buffered.\n");
    geonetErrors[geonetError_forward_gac_discardedPacket] = QString("GAC Forwarding : Packet discarded.\n");
    geonetErrors[geonetError_forward_gac_destinationAddress] = QString("GAC Forwarding Error : DestinationAddress isn't MAC.\n");
    geonetErrors[geonetError_forward_gac_noAreaLineForwarding] = QString("GAC Forwarding Error : Area line forwarding is not enable, can't forward.\n");

    // GBC Send Errors
    geonetErrors[tmp_geonetError_send_gbc_securedPacket] = QString("TMP GBC Processing Error : Can't send secured packet.\n");
    geonetErrors[geonetError_send_gbc_noDestinationArea] = QString("GBC Sending Error : No destination area.\n");
    geonetErrors[geonetError_send_gbc_areaSize] = QString("GBC Sending Error : Area size is too large.\n");
    geonetErrors[geonetError_send_gbc_unknownCommunicationProfile] = QString("GBC Sending Error : Communication Profile isn't G5A.\n");
    geonetErrors[geonetError_send_gbc_bufferedPacket] = QString("GBC Sending : Packet buffered.\n");
    geonetErrors[geonetError_send_gbc_discardedPacket] = QString("GBC Sending : Packet discarded.\n");
    geonetErrors[geonetError_send_gbc_destinationAddress] = QString("GBC Sending Error : DestinationAddress isn't MAC.\n");
    // GBC Forward Errors
    geonetErrors[geonetError_forward_gbc_areaSize] = QString("GBC Forwarding Error : Area size is too large.\n");
    geonetErrors[geonetError_forward_gbc_bufferedPacket] = QString("GBC Forwarding : Packet buffered.\n");
    geonetErrors[geonetError_forward_gbc_discardedPacket] = QString("GBC Forwarding : Packet discarded.\n");
    geonetErrors[geonetError_forward_gbc_destinationAddress] = QString("GBC Forwarding Error : DestinationAddress isn't MAC.\n");

    // GUC Send Errors
    geonetErrors[geonetError_send_guc_noMID] = QString("GUC Sending Error : No destination address.\n");
    geonetErrors[tmp_geonetError_send_guc_MIDNotFound] = QString("TMP GUC Sending Error : Destination not found in LocT.\n");
    geonetErrors[tmp_geonetError_send_guc_securedPacket] = QString("TMP GUC Processing Error : Can't send secured packet.\n");
    geonetErrors[geonetError_send_guc_unknownCommunicationProfile] = QString("GUC Sending Error : Communication Profile isn't G5A.\n");
    geonetErrors[geonetError_send_guc_bufferedPacket] = QString("GUC Sending : Packet buffered.\n");
    geonetErrors[geonetError_send_guc_discardedPacket] = QString("GUC Sending : Packet discarded.\n");
    geonetErrors[geonetError_send_guc_destinationAddress] = QString("GUC Sending Error : DestinationAddress isn't MAC.\n");
    // GUC Forward Errors
    geonetErrors[geonetError_forward_guc_bufferedPacket] = QString("GUC Forwarding : Packet buffered.\n");
    geonetErrors[geonetError_forward_guc_discardedPacket] = QString("GUC Forwarding : Packet discarded.\n");
    geonetErrors[geonetError_forward_guc_destinationAddress] = QString("GUC Forwarding Error : DestinationAddress isn't MAC.\n");

    // SHB Send Errors
    geonetErrors[tmp_geonetError_send_shb_securedPacket] = QString("TMP SHB Processing Error : Can't send secured packet.\n");
    geonetErrors[geonetError_send_shb_unknownCommunicationProfile] = QString("SHB Sending Error : Communication Profile isn't G5A.\n");
    geonetErrors[geonetError_send_shb_bufferedPacket] = QString("SHB Sending : Packet buffered.\n");

    // TSB Send Errors
    geonetErrors[tmp_geonetError_send_tsb_securedPacket] = QString("TMP TSB Processing Error : Can't send secured packet.\n");
    geonetErrors[geonetError_send_tsb_unknownCommunicationProfile] = QString("TSB Sending Error : Communication Profile isn't G5A.\n");
    geonetErrors[geonetError_send_tsb_bufferedPacket] = QString("TSB Sending : Packet buffered.\n");
    // TSB Forward Errors
    geonetErrors[geonetError_forward_tsb_unknownCommunicationProfile] = QString("TSB Forward Error : Communication Profile isn't G5A.\n");
    geonetErrors[geonetError_forward_tsb_bufferedPacket] = QString("TSB Forward : Packet buffered.\n");

    // Receive Errors
    // Basic Header Errors
    geonetErrors[geonetError_processing_bh_gnProtocolVersion] = QString("BasicHeader Processing Error : Protocol version is different from the current version (version ").append(QString::number(itsGnProtocolVersion)).append(").\n");
    geonetErrors[geonetError_processing_bh_nextHeader] = QString("BasicHeader Processing Error : Next header invalide.\n");
    geonetErrors[geonetError_processing_bh_securedHeader] = QString("BasicHeader Processing  Error : Secured header.\n");

    // Common Header Errors
    geonetErrors[geonetError_processing_ch_hopLimit] = QString("CommonHeader Processing Error : RemainingHopLimit is greater than MaximumHopLimit.\n");
    geonetErrors[geonetError_processing_ch_headerType] = QString("CommonHeader Processing Error : HeaderType is equal to any or isn't known.\n");
    geonetErrors[geonetError_processing_ch_headerSubType_TSB_unknown] = QString("CommonHeader Processing Error : HeaderSubType TSB is unknown\n");
    geonetErrors[geonetError_processing_ch_headerSubType_LS_unknown] = QString("CommonHeader Processing Error : HeaderSubType Location Service is unknown.\n");

    // Duplicate Packet Address Errors
    geonetErrors[geonetError_processing_beacon_DAD] = QString("Beacon Processing Error : Duplicate address detected.\n");
    geonetErrors[geonetError_processing_LSRequest_DAD] = QString("LSRequest Processing Error : Duplicate address detected.\n");
    geonetErrors[geonetError_processing_LSReply_DAD] = QString("LSReply Processing Error : Duplicate address detected.\n");
    geonetErrors[geonetError_processing_gac_DAD] = QString("GAC Processing Error : Duplicate address detected.\n");
    geonetErrors[geonetError_processing_gbc_DAD] = QString("GBC Processing Error : Duplicate address detected.\n");
    geonetErrors[geonetError_processing_guc_DAD] = QString("GUC Processing Error : Duplicate address detected.\n");
    geonetErrors[geonetError_processing_shb_DAD] = QString("SHB Processing Error : Duplicate address detected.\n");
    geonetErrors[geonetError_processing_tsb_DAD] = QString("TSB Processing Error : Duplicate address detected.\n");

    // Duplicate Packet Timestamp &&/|| SN Errors
    geonetErrors[geonetError_processing_beacon_DPD] = QString("Beacon Processing Error : Duplicate packet detected.\n");
    geonetErrors[geonetError_processing_LSRequest_DPD] = QString("LSRequest Processing Error : Duplicate packet detected.\n");
    geonetErrors[geonetError_processing_LSReply_DPD] = QString("LSReply Processing Error : Duplicate packet detected.\n");
    geonetErrors[geonetError_processing_gac_DPD] = QString("GAC Processing Error : Duplicate packet detected.\n");
    geonetErrors[geonetError_processing_gbc_DPD] = QString("GBC Processing Error : Duplicate packet detected.\n");
    geonetErrors[geonetError_processing_guc_DPD] = QString("GUC Processing Error : Duplicate packet detected.\n");
    geonetErrors[geonetError_processing_shb_DPD] = QString("SHB Processing Error : Duplicate packet detected.\n");
    geonetErrors[geonetError_processing_tsb_DPD] = QString("TSB Processing Error : Duplicate packet detected.\n");
}

int GeoNet::InitRawSocket_send()
{
    int rawSocket;
    struct ifreq if_idx;
    struct sockaddr_ll socket_address;
    char sendbuf[BUF_SIZ];
    struct ether_header *eh = (struct ether_header *) sendbuf;
    char ifName[IFNAMSIZ] = {'\0'};

    eh = (struct ether_header *) sendbuf;

    memset(&socket_address, 0, sizeof socket_address);

    /* Get interface name */
    strcpy(ifName, m_paramConf->interface().toStdString().c_str());

    /* Open RAW socket to send on */
    if((rawSocket = socket(AF_PACKET, SOCK_RAW, htons(0x8947))) == -1)
    {
        qDebug() << "socket" << strerror(errno);
        return networkError_rawsocket_open;
    }

    /* Get the index of the interface to send on */
    memset(&if_idx, 0, sizeof(struct ifreq));
    strncpy(if_idx.ifr_name, ifName, IFNAMSIZ - 1);
    if(ioctl(rawSocket, SIOCGIFINDEX, &if_idx) < 0)
    {
        qDebug() << "SIOCGIFINDEX geonet" << strerror(errno) << ifName;
        return networkError_rawsocket_siocgifindex;
    }

    /* Index of the network device */
    socket_address.sll_ifindex = if_idx.ifr_ifindex;

    /* Address length */
    socket_address.sll_halen = ETH_ALEN;

    socket_address.sll_family = AF_INET;

    /* Ethertype field */
    eh->ether_type = htons(0x8947);

    m_paramNetwork->rawSocket = rawSocket;
    m_paramNetwork->if_idx = if_idx;
    m_paramNetwork->socket_address = socket_address;
    memcpy(m_paramNetwork->sendbuf, sendbuf, BUF_SIZ);
    m_paramNetwork->eh = *eh;
    memcpy(m_paramNetwork->ifName, ifName, IFNAMSIZ);

    m_manager->hashMap["Network"] = m_paramNetwork;

    return networkError_success;
}

void GeoNet::sendEventIndicationToUT(uchar* packetFull, int packetLen)
{
    if(m_paramUpperTester != 0)
    {
        quint8 mt = UpperTesterPrimitives::PRIMITIVE_utGnEventInd;

        QByteArray resUt;
        QDataStream ds(&resUt, QIODevice::WriteOnly);
        ds << mt;
        ds << (quint16)packetLen;
        ds.writeRawData((char*)packetFull, packetLen);

        if(gen2ut != 0)
            gen2ut->writeDatagram(resUt, QHostAddress::LocalHost, m_paramUpperTester->upperTesterEventIndicationListenPort());
    }
}

int GeoNet::lsRetransmit(GnAddr destination, QByteArray lsRequestData, BasicHeader bh)
{
    QMutexLocker mutexLocT(m_mutexLocT);
    if(m_lsRetransmission[destination.toHex()] > 0 && !((*m_locationTable)[destination.toHex()].isPending()))
    {
        m_lsRetransmission.remove(destination.toHex());
        return geonetError_send_LSRequest_successfullyRespond;
    }

    /* Construct the Ethernet header : destination addr */
    for(int i = 0 ; i < 6 ; i++)
        m_paramNetwork->eh.ether_dhost[i] = 0xFF;

    int ret = send(lsRequestData, true, bh);

    m_lsRetransmission[destination.toHex()]+=1;

    if(m_lsRetransmission[destination.toHex()] > itsGnLocationServiceMaxRetrans)
    {
        m_LSPB->erase(destination);
        m_lsRetransmission.remove(destination.toHex());
        m_locationTable->remove(destination.toHex());
        return geonetError_send_LSRequest_noResponse;
    }
    QTimer::singleShot(itsGnLocationServiceRetransmitTimer, [this, destination, lsRequestData, bh]()
    {lsRetransmit(destination, lsRequestData, bh);});

    return ret;
}

/* -------------------------PACKET-------------------------SENDING------------------------- */

void GeoNet::sendPacketFromRPB(gnDataRequest_t rawPacket)
{
    send(rawPacket);
}

/* CommonHeader verification */
int GeoNet::send(gnDataRequest_t gnDR)
{
    QByteArray payload;
    QDataStream dsPayload(&payload, QIODevice::WriteOnly);

    verifLocT();
    if(gnDR.destinationPort != 0)
    {
        unsigned char btp[4] = {0x00};
        format(btp, gnDR.destinationPort, 2);
        format(&btp[2], gnDR.destinationPortInfo, 2);
        dsPayload.writeRawData((const char*)btp, 4);
        dsPayload.writeRawData(gnDR.payload.data(), gnDR.payload.length());
        gnDR.payload = payload;
    }

    switch(gnDR.packetTransportType)
    {
    case CommonHeader::headerType_geoAnycast :
        switch(gnDR.packetTransportSubType)
        {
        case CommonHeader::headerSubTypeGeoAnycast_circle :
        case CommonHeader::headerSubTypeGeoAnycast_rectangle :
        case CommonHeader::headerSubTypeGeoAnycast_ellipse :
            return sendGAC(gnDR);
            break;
        default:
            return geonetError_send_ch_headerSubType_geoAnycast_unknown;
            break;
        }
        break;
    case CommonHeader::headerType_geoBroadcast :
        switch(gnDR.packetTransportSubType)
        {
        case CommonHeader::headerSubTypeGeoBroadcast_circle :
        case CommonHeader::headerSubTypeGeoBroadcast_rectangle :
        case CommonHeader::headerSubTypeGeoBroadcast_ellipse :
            return sendGBC(gnDR);
            break;
        default:
            return geonetError_send_ch_headerSubType_geoBroadcast_unknown;
            break;
        }
        break;
    case CommonHeader::headerType_geoUnicast :
        return sendGUC(gnDR);
        break;
    case CommonHeader::headerType_topologicallyScopedBroadcast :
        switch(gnDR.packetTransportSubType)
        {
        /* SHB */
        case CommonHeader::headerSubTypeTopologicallyScopedBroadcast_singleHop :
            return sendSHB(gnDR);
            break;
        /* TSB */
        case CommonHeader::headerSubTypeTopologicallyScopedBroadcast_multiHop :
            return sendTSB(gnDR);
            break;
        default:
            return geonetError_send_ch_headerSubType_TSB_unknown;
            break;
        }
        break;
    case CommonHeader::headerType_beacon:
    default:
        return geonetError_send_ch_headerType;
        break;
    }
    return geonetError_send_ch_headerType;
}

/* Beacon Packet creation */
void GeoNet::sendBeacon()
{
    /* Creating a Beacon to send */
    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence < itsGnPaiInterval / 2 ? true << 15 : false << 15;
    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);
    BeaconPacket beacon(lpv);

    CommonHeader ch(CommonHeader::nextHeaderType_any, CommonHeader::headerType_beacon, CommonHeader::headerSubTypeBeacon_unspecified, TrafficClass(m_paramConf->trafficClass()), m_paramAppli->data().itsGnIsMobile(), 0, 1);

    LifeTime lt(60, LifeTime::base_1_s);
    BasicHeader bh;

    if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, lt, 1);
    else
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, lt, 1);

    QByteArray beaconData;
    QDataStream ds(&beaconData, QIODevice::WriteOnly);

    ds.writeRawData(ch.toHex().data(), ch.toHex().length());
    ds.writeRawData(beacon.toHex().data(), beacon.toHex().length());

    for(int i = 0 ; i < 6 ; i++)
        m_paramNetwork->eh.ether_dhost[i] = 0xFF;

    int ret = send(beaconData, true, bh);
    if(ret == networkError_success)
    {
        m_HPB->addPacket(beacon);
        printf("\n--> Beacon successfuly send!\n");
    }
    else
        printf("\n--> Beacon sending failure. Error code : %s", geonetErrors[ret].toStdString().c_str());

    m_beacon->setInterval(itsGnBeaconServiceRetransmitTimer + (qrand() % itsGnBeaconServiceMaxJitter));
    m_beacon->start();
}

/* LsRequest creation */
int GeoNet::sendLsRequest(GnAddr destination, gnDataRequest_t gnDR)
{
    if(m_locationTable->find(destination.toHex()) != m_locationTable->end() && (*m_locationTable)[destination.toHex()].isPending())
    {
        m_LSPB->addPacket(destination, gnDR, (m_paramGPS->timestamp() % 0x100000000));
        return geonetError_send_LSRequest_buffered;
    }
    else
    {
        quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
        quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;
        LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);
        LSRequestPacket lsR(sequenceNumber, lpv, destination);

        CommonHeader ch(0, CommonHeader::headerType_locationService, CommonHeader::headerSubTypeLocationService_Request, m_paramConf->trafficClass(), m_paramAppli->data().itsGnIsMobile(), 0, itsGnDefaultHopLimit);

        BasicHeader bh;
        if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
            bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, LifeTime(60, LifeTime::base_1_s), itsGnDefaultHopLimit);
        else
            bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, LifeTime(60, LifeTime::base_1_s), itsGnDefaultHopLimit);

        m_LSPB->addPacket(destination, gnDR, tmpstmp);

        m_lsRetransmission[destination.toHex()]=0;

        /* Construct the packet */
        QByteArray lsRequestData;
        QDataStream ds(&lsRequestData, QIODevice::WriteOnly);

        ds.writeRawData(ch.toHex().data(), ch.toHex().length());
        ds.writeRawData(lsR.toHex().data(), lsR.toHex().length());


        int ret = lsRetransmit(destination.toHex(), lsRequestData, bh);
        if(ret == networkError_success)
        {
            m_HPB->addPacket(lsR);
            sequenceNumber++;
            (*m_locationTable)[destination.toHex()] = LocationTableEntry();
            (*m_locationTable)[destination.toHex()].setLsPending(true);
            (*m_locationTable)[destination.toHex()].setNeverReceived(true);
            //(*m_locationTable)[destination.toHex()].setLastTimestamp(m_paramGPS->timestamp() % 0x100000000);
            (*m_locationTable)[destination.toHex()].setLastTimestamp(0);
            return ret;
        }
        else
        {
            m_locationTable->remove(destination.toHex());
            return ret;
        }
    }
}

/* GACPacket creation */
int GeoNet::sendGAC(gnDataRequest_t gnDR)
{
    if(gnDR.destinationAddress.present != destinationAddress_PR_DestinationArea)
        return geonetError_send_gac_noDestinationArea;

    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;

    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);
    GACPacket gac(sequenceNumber, lpv, DestinationArea(gnDR.destinationAddress.choice.destinationArea));

    CommonHeader ch;
    ch = CommonHeader(gnDR.upperProtocolEntity, gnDR.packetTransportType, gnDR.packetTransportSubType, gnDR.trafficClass, m_paramAppli->data().itsGnIsMobile(), gnDR.payload.length(), gnDR.maximumHopLimit==0?itsGnDefaultHopLimit:*gnDR.maximumHopLimit);

    LifeTime lt = (gnDR.maximumPacketLifeTime != 0)?*gnDR.maximumPacketLifeTime: LifeTime(60, LifeTime::base_1_s);
    int rhl = (gnDR.maximumHopLimit != 0)?*gnDR.maximumHopLimit:itsGnDefaultHopLimit;

    BasicHeader bh;

    if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, lt, rhl);
    else
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, lt, rhl);

    if(gnDR.repetitionInterval != NULL && *gnDR.repetitionInterval > 0 && gnDR.maximumRepetitionTime != NULL && *gnDR.maximumRepetitionTime > 0 && gnDR.maximumHopLimit != NULL && *gnDR.maximumHopLimit > 1)
    {
        m_RPB->addPacket(gnDR);
    }

    e_shape shape;
    switch(ch.headerSubType())
    {
    case CommonHeader::headerSubTypeGeoBroadcast_circle:
        shape = shape_circle;
        if((gac.destinationArea().distanceA() * gac.destinationArea().distanceA() * M_PI) > ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
            return geonetError_send_gac_areaSize;
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_rectangle:
        shape = shape_rectangle;
        if((gac.destinationArea().distanceA() * gac.destinationArea().distanceB()) > ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
            return geonetError_send_gac_areaSize;
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_ellipse:
        shape = shape_ellipse;
        if((gac.destinationArea().distanceA() * gac.destinationArea().distanceB() * M_PI) > ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
            return geonetError_send_gac_areaSize;
        break;
    default :
        shape = shape_none;
        break;
    }

    double x, y;
    x = diameter(m_paramGPS->data().referencePosition().longitude, gac.destinationArea().geoAreaPosLongitude(), circAtLat((m_paramGPS->data().referencePosition().latitude + gac.destinationArea().geoAreaPosLatitude())/2));
    y = LAT2METER*((m_paramGPS->data().referencePosition().latitude - gac.destinationArea().geoAreaPosLatitude())/10000000.);
    y *= y<0?-1:1;

    double inArea;
    if(shape == shape_circle)
        inArea = f(x, y, gac.destinationArea().distanceA(), gac.destinationArea().distanceB(), shape);
    else
        inArea = f(
                x*cos(gac.destinationArea().angle())-y*sin(gac.destinationArea().angle()),
                x*sin(gac.destinationArea().angle())+y*cos(gac.destinationArea().angle()),
                gac.destinationArea().distanceA(), gac.destinationArea().distanceB(), shape);

    ForwardingAlgorithmResponse res;

    if(inArea<0 && m_paramConf->itsGnGeoAreaLineForwarding())
    {
        switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
        {
        default:
        case itsGnGeoUnicastForwardingAlgorithm_unspecified:
        case itsGnGeoUnicastForwardingAlgorithm_greedy:
            res = greedy(bh, ch, gac, gnDR.payload, QByteArray(), m_gnAddr->mid(), m_gnAddr->mid());
            break;
        case itsGnGeoUnicastForwardingAlgorithm_CBF:
            uchar NH[MAC_ADDR_SIZE];
            for(int i = 0; i < MAC_ADDR_SIZE; i++)
                NH[i] = 0xFF;
            res.setMacAddr(NH);
            res.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
            break;
        }

        if(res.isInteger() && res.integer() == 0)
            return geonetError_send_gac_unicast_bufferedPacket;
        else if(res.isInteger() && res.integer() == -1)
            return geonetError_send_gac_unicast_discardedPacket;
    }
    else if(inArea >= 0)
    {
        switch(m_paramConf->itsGnGeoBroadcastForwardingAlgorithm())
        {
        default:
        case itsGnGeoBroadcastForwardingAlgorithm_unspecified:
        case itsGnGeoBroadcastForwardingAlgorithm_simple:
            res = simpleForwardingAlgorithm(bh, ch, gac, gnDR.payload, shape, QByteArray(), true);
            break;
        case itsGnGeoBroadcastForwardingAlgorithm_CBF:
            uchar NH[MAC_ADDR_SIZE];
            for(int i = 0; i < MAC_ADDR_SIZE; i++)
                NH[i] = 0xFF;
            res.setMacAddr(NH);
            res.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
            break;
        case itsGnGeoBroadcastForwardingAlgorithm_advanced:
            res = advancedForwarding(bh, ch, gac, gnDR.payload, shape, QByteArray(), m_gnAddr->mid(), m_gnAddr->mid(), true);
            break;
        }

        if(res.isInteger() && res.integer() == 0)
            return geonetError_send_gac_broadcast_bufferedPacket;
        else if(res.isInteger() && res.integer() == -1)
            return geonetError_send_gac_broadcast_discardedPacket;
    }
    else
        return geonetError_send_gac_noAreaLineForwarding;

    /* If CommunicationProfile is not supported or unknown */
    if(gnDR.gnCommunicationProfile != gnCommunicationProfile_ITS_G5A)
        return geonetError_send_gac_unknownCommunicationProfile;

    /* Construct the packet */
    QByteArray gacData;
    QDataStream ds(&gacData, QIODevice::WriteOnly);

    ds.writeRawData(ch.toHex().data(), ch.toHex().length());
    ds.writeRawData(gac.toHex().data(), gac.toHex().length());
    ds.writeRawData(gnDR.payload.data(), gnDR.payload.length());

    if(res.isMacAddr())
        /* Construct the Ethernet header : destination addr */
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
    else
        return geonetError_send_gac_destinationAddress;

    int ret = send(gacData, true, bh);
    if(ret == networkError_success)
    {
        m_HPB->addPacket(gac);
        sequenceNumber++;
    }

    return ret;
}

/* GBCPacket creation */
int GeoNet::sendGBC(gnDataRequest_t gnDR)
{

    if(gnDR.destinationAddress.present != destinationAddress_PR_DestinationArea)
        return geonetError_send_gbc_noDestinationArea;

    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence < itsGnPaiInterval / 2 ?
                true << 15 : false << 15;

    LongPositionVector lpv(*m_gnAddr, tmpstmp, m_paramGPS->data().referencePosition().latitude,
                           m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue,
                           m_paramGPS->data().heading().headingValue);
    GBCPacket gbc(sequenceNumber, lpv, DestinationArea(gnDR.destinationAddress.choice.destinationArea));

    CommonHeader ch;
    ch = CommonHeader(gnDR.upperProtocolEntity, gnDR.packetTransportType, gnDR.packetTransportSubType, gnDR.trafficClass,
                      m_paramAppli->data().itsGnIsMobile(), gnDR.payload.length(),
                      gnDR.maximumHopLimit==0?itsGnDefaultHopLimit:*gnDR.maximumHopLimit);

    LifeTime lt = (gnDR.maximumPacketLifeTime != 0)?*gnDR.maximumPacketLifeTime: LifeTime(60, LifeTime::base_1_s);
    int rhl = (gnDR.maximumHopLimit != 0)?*gnDR.maximumHopLimit:itsGnDefaultHopLimit;

    BasicHeader bh;

    if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, lt, rhl);
    else
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, lt, rhl);

    if(gnDR.repetitionInterval != NULL && *gnDR.repetitionInterval > 0 && gnDR.maximumRepetitionTime != NULL &&
            *gnDR.maximumRepetitionTime > 0 && gnDR.maximumHopLimit != NULL && *gnDR.maximumHopLimit > 1)
    {
        m_RPB->addPacket(gnDR);
    }

    e_shape shape;
    switch(ch.headerSubType())
    {
    case CommonHeader::headerSubTypeGeoBroadcast_circle:
        shape = shape_circle;
        if((gbc.destinationArea().distanceA() * gbc.destinationArea().distanceA() * M_PI) >
                ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
            return geonetError_send_gbc_areaSize;
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_rectangle:
        shape = shape_rectangle;
        if((gbc.destinationArea().distanceA() * gbc.destinationArea().distanceB()) >
                ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
            return geonetError_send_gbc_areaSize;
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_ellipse:
        shape = shape_ellipse;
        if((gbc.destinationArea().distanceA() * gbc.destinationArea().distanceB() * M_PI) >
                ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
            return geonetError_send_gbc_areaSize;
        break;
    default :
        shape = shape_none;
        break;
    }

    ForwardingAlgorithmResponse res;

    switch(m_paramConf->itsGnGeoBroadcastForwardingAlgorithm())
    {
    default:
    case itsGnGeoBroadcastForwardingAlgorithm_unspecified:
    case itsGnGeoBroadcastForwardingAlgorithm_simple:
        res = simpleForwardingAlgorithm(bh, ch, gbc, gnDR.payload, shape, QByteArray(), true);
        break;
    case itsGnGeoBroadcastForwardingAlgorithm_CBF:
        res = contentionBasedForwardingAlgorithm(bh, ch, gbc, gnDR.payload, shape, QByteArray(), true);
        break;
    case itsGnGeoBroadcastForwardingAlgorithm_advanced:
        res = advancedForwarding(bh, ch, gbc, gnDR.payload, shape, QByteArray(), m_gnAddr->mid(), m_gnAddr->mid(), true);
        break;
    }

    if(res.isInteger() && res.integer() == 0)
        return geonetError_send_gbc_bufferedPacket;
    else if(res.isInteger() && res.integer() == -1)
        return geonetError_send_gbc_discardedPacket;

    /* If CommunicationProfile is not supported or unknown */
    if(gnDR.gnCommunicationProfile != gnCommunicationProfile_ITS_G5A)
        return geonetError_send_gbc_unknownCommunicationProfile;

    /* Construct the packet */
    QByteArray gbcData;
    QDataStream ds(&gbcData, QIODevice::WriteOnly);

    ds.writeRawData(ch.toHex().data(), ch.toHex().length());
    ds.writeRawData(gbc.toHex().data(), gbc.toHex().length());
    ds.writeRawData(gnDR.payload.data(), gnDR.payload.length());

    if(res.isMacAddr())
        /* Construct the Ethernet header : destination addr */
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
    else
        return geonetError_send_gbc_destinationAddress;

    int ret = send(gbcData, true, bh);
    if(ret == networkError_success)
    {
        m_HPB->addPacket(gbc);
        sequenceNumber++;
    }

    return ret;
}

/* GUCPacket creation */
int GeoNet::sendGUC(gnDataRequest_t gnDR)
{
    GUCPacket guc;

    if(gnDR.destinationAddress.present != destinationAddress_PR_MID)
        return geonetError_send_guc_noMID;

    QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(GnAddr(gnDR.destinationAddress.choice.gnAddr).toHex());

    if((pos == m_locationTable->end()) || LocationTableEntry(pos.value()).isPending() || (LocationTableEntry(pos.value()).longPositionVector().pai() == 0))
        return sendLsRequest(GnAddr(gnDR.destinationAddress.choice.gnAddr), gnDR);


    /*if(pos != m_locationTable->end() && !LocationTableEntry(pos.value()).isPending() &&  LocationTableEntry(pos.value()).longPositionVector().pai())
    {*/
    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;

    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);
    guc = GUCPacket(sequenceNumber, lpv, ShortPositionVector(LocationTableEntry(pos.value()).longPositionVector().gnAddr(), LocationTableEntry(pos.value()).longPositionVector().timestamp(), LocationTableEntry(pos.value()).longPositionVector().latitude(), LocationTableEntry(pos.value()).longPositionVector().longitude()));
    /*}
    else
        return sendLsRequest(GnAddr(gnDR.destinationAddress.choice.gnAddr), gnDR);*/

    CommonHeader ch;
    ch = CommonHeader(gnDR.upperProtocolEntity, gnDR.packetTransportType, gnDR.packetTransportSubType, gnDR.trafficClass, m_paramAppli->data().itsGnIsMobile(), gnDR.payload.length(), gnDR.maximumHopLimit==0?itsGnDefaultHopLimit:*gnDR.maximumHopLimit);

    LifeTime lt = (gnDR.maximumPacketLifeTime != 0)?*gnDR.maximumPacketLifeTime: LifeTime(60, LifeTime::base_1_s);
    int rhl = (gnDR.maximumHopLimit != 0)?*gnDR.maximumHopLimit:itsGnDefaultHopLimit;

    BasicHeader bh;

    if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, lt, rhl);
    else
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, lt, rhl);

    if(gnDR.repetitionInterval != NULL && *gnDR.repetitionInterval > 0 && gnDR.maximumRepetitionTime != NULL && *gnDR.maximumRepetitionTime > 0 && gnDR.maximumHopLimit != NULL && *gnDR.maximumHopLimit > 1)
    {
        m_RPB->addPacket(gnDR);
    }

    ForwardingAlgorithmResponse res;
    switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
    {
    default:
    case itsGnGeoUnicastForwardingAlgorithm_unspecified:
    case itsGnGeoUnicastForwardingAlgorithm_greedy:
        res = greedy(bh, ch, guc, gnDR.payload, QByteArray());
        break;
    case itsGnGeoUnicastForwardingAlgorithm_CBF:
        //res = CBFUnicast(bh, ch, guc, gnDR.payload);
        uchar NH[MAC_ADDR_SIZE];
        for(int i = 0; i < MAC_ADDR_SIZE; i++)
            NH[i] = 0xFF;
        res.setMacAddr(NH);
        res.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
        break;
    }

    if(res.isInteger() && res.integer() == 0)
        return geonetError_send_guc_bufferedPacket;
    else if(res.isInteger() && res.integer() == -1)
        return geonetError_send_guc_discardedPacket;

    /* If CommunicationProfile is not supported or unknown */
    if(gnDR.gnCommunicationProfile != gnCommunicationProfile_ITS_G5A)
        return geonetError_send_guc_unknownCommunicationProfile;

    /* Construct the packet */
    QByteArray gucData;
    QDataStream ds(&gucData, QIODevice::WriteOnly);

    ds.writeRawData(ch.toHex().data(), ch.toHex().length());
    ds.writeRawData(guc.toHex().data(), guc.toHex().length());
    ds.writeRawData(gnDR.payload.data(), gnDR.payload.length());


    if(res.isMacAddr())
        /* Construct the Ethernet header : destination addr */
        for(int i = 0 ; i < 6 ; i++){
            m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
    printf("PARAMNETWORK [%d] -> %d\n", i, m_paramNetwork->eh.ether_dhost[i]);}
    else
        return geonetError_send_guc_destinationAddress;

    int ret = send(gucData, true, bh);
    if(ret == networkError_success)
    {
        m_HPB->addPacket(guc);
        sequenceNumber++;
    }

    return ret;
}

/* SHBPacket creation */
int GeoNet::sendSHB(gnDataRequest_t gnDR)
{
    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;
    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);
    SHBPacket shb(lpv);

    CommonHeader ch(gnDR.upperProtocolEntity, gnDR.packetTransportType, gnDR.packetTransportSubType, gnDR.trafficClass, m_paramAppli->data().itsGnIsMobile(), gnDR.payload.length(), 1);

    LifeTime lt = (gnDR.maximumPacketLifeTime != 0)?*gnDR.maximumPacketLifeTime: LifeTime(60, LifeTime::base_1_s);
    quint8 rhl = (gnDR.maximumHopLimit != 0)?*gnDR.maximumHopLimit:1;

    BasicHeader bh;

    if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, lt, rhl);
    else
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, lt, rhl);

    bool noNeighbour = true;
    QMutexLocker locker(m_mutexLocT);
    if(m_locationTable && !m_locationTable->isEmpty())
        for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();i != m_locationTable->end(); ++i)
            if(LocationTableEntry(i.value()).isNeighbour() != 0 && *(LocationTableEntry(i.value()).isNeighbour()))
                noNeighbour = false;
    locker.unlock();

    if(noNeighbour && gnDR.trafficClass.SCF() == 1)
    {
        m_BCFPB->addPacket(bh, ch, shb, gnDR.payload, QByteArray());
        return geonetError_send_shb_bufferedPacket;
    }
    else
    {
        /* If CommunicationProfile is not supported or unknown */
        if(gnDR.gnCommunicationProfile != gnCommunicationProfile_ITS_G5A)
            return geonetError_send_shb_unknownCommunicationProfile;

        if(gnDR.repetitionInterval != NULL && *gnDR.repetitionInterval > 0 && gnDR.maximumRepetitionTime != NULL && *gnDR.maximumRepetitionTime > 0 && gnDR.maximumHopLimit != NULL && *gnDR.maximumHopLimit > 1)
        {
            m_RPB->addPacket(gnDR);
        }

        /* Construct the packet */
        QByteArray shbData;
        QDataStream ds(&shbData, QIODevice::WriteOnly);

        ds.writeRawData(ch.toHex().data(), ch.toHex().length());
        ds.writeRawData(shb.toHex().data(), shb.toHex().length());
        ds.writeRawData(gnDR.payload.data(), gnDR.payload.length());

        /* Construct the Ethernet header : destination addr */
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = 0xFF;

        int ret = send(shbData, true, bh);
        if(ret == networkError_success)
            m_HPB->addPacket(shb);

        return ret;
    }
}

/* TSBPacket creation */
int GeoNet::sendTSB(gnDataRequest_t gnDR)
{
    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;
    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);
    TSBPacket tsb(sequenceNumber, lpv);

    CommonHeader ch(gnDR.upperProtocolEntity, gnDR.packetTransportType, gnDR.packetTransportSubType, gnDR.trafficClass, m_paramAppli->data().itsGnIsMobile(), gnDR.payload.length(), 1);

    LifeTime lt = (gnDR.maximumPacketLifeTime != 0)?*gnDR.maximumPacketLifeTime: LifeTime(60, LifeTime::base_1_s);
    quint8 rhl = (gnDR.maximumHopLimit != 0)?*gnDR.maximumHopLimit:itsGnDefaultHopLimit;

    BasicHeader bh;

    if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, lt, rhl);
    else
        bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, lt, rhl);

    bool noNeighbour = true;
    QMutexLocker locker(m_mutexLocT);
    if(!m_locationTable->isEmpty())
        for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();i != m_locationTable->end(); ++i)
            if(LocationTableEntry(i.value()).isNeighbour() != 0 && *(LocationTableEntry(i.value()).isNeighbour()))
                noNeighbour = false;
    locker.unlock();

    if(noNeighbour && gnDR.trafficClass.SCF() == 1)
    {
        m_BCFPB->addPacket(bh, ch, tsb, gnDR.payload, QByteArray());
        return geonetError_send_tsb_bufferedPacket;
    }
    else
    {
        /* If CommunicationProfile is not supported or unknown */
        if(gnDR.gnCommunicationProfile != gnCommunicationProfile_ITS_G5A)
            return geonetError_send_tsb_unknownCommunicationProfile;

        if(gnDR.repetitionInterval != NULL && *gnDR.repetitionInterval > 0 && gnDR.maximumRepetitionTime != NULL && *gnDR.maximumRepetitionTime > 0 && gnDR.maximumHopLimit != NULL && *gnDR.maximumHopLimit > 1)
        {
            m_RPB->addPacket(gnDR);
        }

        /* Construct the packet */
        QByteArray tsbData;
        QDataStream ds(&tsbData, QIODevice::WriteOnly);

        ds.writeRawData(ch.toHex().data(), ch.toHex().length());
        ds.writeRawData(tsb.toHex().data(), tsb.toHex().length());
        ds.writeRawData(gnDR.payload.data(), gnDR.payload.length());

        /* Construct the Ethernet header : destination addr */
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = 0xFF;

        int ret = send(tsbData, true, bh);
        if(ret == networkError_success)
        {
            m_HPB->addPacket(tsb);
            sequenceNumber++;
        }

        return ret;
    }
}

/* Sending the packet */
int GeoNet::send(QByteArray payloadToSend, bool containLPV, BasicHeader bh)
{
    QByteArray payload;
    size_t tx_len = sizeof(struct ether_header);
    int error;

    /* Security Header */
    if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
        payload = getSecuredPayload(payloadToSend, bh);
    else
        payload = bh.toHex() + payloadToSend;

    /* Construct the Ethernet header : my mac addr */
    for(int i = 0 ; i < 6 ; i++)
        m_paramNetwork->eh.ether_shost[i] = m_paramConf->mid(i);

    memcpy(m_paramNetwork->sendbuf, &m_paramNetwork->eh, sizeof(struct ether_header));
    for(int i = 0 ; i < payload.length(); i++)
        m_paramNetwork->sendbuf[tx_len++] = payload.data()[i];

    /* Send packet */
    if(m_geonetFlag)
    {
        if(sendto(m_paramNetwork->rawSocket, m_paramNetwork->sendbuf, tx_len, 0, (struct sockaddr*)(&(m_paramNetwork->socket_address)), sizeof(struct sockaddr_ll)) < 0)
        {
            qDebug() << "sendto" << strerror(errno);
            return networkError_rawsocket_send;
        }
    }
    if(m_hybridFlag)
    {
        error = hybridSend(payload);
        if(error != hybridError_success)
        {
            qDebug() << "hybridSend" << m_tcpSocket->errorString();
            return error;
        }
        else
            qInfo() << "HybridSend success";
    }

    if(containLPV)
        QTimer::singleShot(0, this, SLOT(lpvUpdate()));

    return networkError_success;
}

int GeoNet::hybridSend(QByteArray payload)
{
    int p = -1;
    qDebug() << /*"Sending" << payload.toHex() << "of" << payload.size() << "bytes to"
             << m_tcpSocket->peerName() << "(" << m_tcpSocket->peerAddress().toString() << ") :" << m_tcpSocket->peerPort()
             << */"connection state" << m_tcpSocket->state();
    if(m_tcpSocket->state() == QAbstractSocket::ConnectedState)
    {
        p = m_tcpSocket->write(payload.data(), payload.length());
        m_tcpSocket->flush();
        m_tcpSocket->waitForBytesWritten();
    }
    else
    {
        if(!m_paramHybrid->sslUsed())
        {
            m_tcpSocket->connectToHost(m_paramHybrid->serverAddress(), m_paramHybrid->serverPort(), QIODevice::ReadWrite);
            m_tcpSocket->waitForConnected();
        }
        else
        {
            m_sslSocket->connectToHostEncrypted(m_paramHybrid->serverAddress(), m_paramHybrid->serverPort());
            m_sslSocket->waitForConnected();
            m_tcpSocket = m_sslSocket;
        }
        if(m_tcpSocket->state() == QAbstractSocket::ConnectedState)
        {
            p = m_tcpSocket->write(payload.data(), payload.length());
            m_tcpSocket->flush();
            m_tcpSocket->waitForBytesWritten();
        }
    }

    if(p < 0)
        return hybridError_send;
    qDebug() << p << "bytes sent";


    return hybridError_success;
}

/* -------------------------PACKET-------------------------READING------------------------- */

/* EthernetHeader Processing */
void GeoNet::treatPackets(QByteArray rawPacket)
{
    uchar* packet = (uchar*) rawPacket.data();
    sniff_ethernet_t* eth = (sniff_ethernet_t*) packet;
    int len = rawPacket.length() - SIZE_ETHERNET;
    int res = 0;
    res = basicHeaderProcessing(packet + SIZE_ETHERNET, len, eth);

    if(res == networkError_success)
        printf("\n--> Successfuly received and treated a packet.\n");
    else
        printf("\n--> Failure while treating a packet. Error code : %s", geonetErrors[res].toStdString().c_str());

}

int GeoNet::basicHeaderProcessing(uchar* packet, int len, sniff_ethernet_t* eth)
{

    qDebug() << "BH processing";
    sniff_basicHeader_t* tmpbh = (sniff_basicHeader_t*) packet;
    BasicHeader bh(*tmpbh);
    bool *verifySecuredPacket = 0;
    QByteArray securedPacket;

    if(bh.version() != itsGnProtocolVersion)
    {
        return geonetError_processing_bh_gnProtocolVersion;
    }
    else
    {
        switch(bh.nextHeader())
        {
        case BasicHeader::nextHeader_any:
        case BasicHeader::nextHeader_commonHeader:
            /* CommonHeader processing */
            return commonHeaderProcessing(packet + BasicHeader::helper_size, eth, bh);
            break;
        case BasicHeader::nextHeader_securedPacket:
            /* SecuredPacket processing */
            if(m_paramConf->itsGnSecurity())
            {
                verifySecuredPacket = (bool*)calloc(1, sizeof *verifySecuredPacket);
                securedPacket = verifySecuredPayload(QByteArray((const char*)packet, len), verifySecuredPacket);
                if(verifySecuredPacket)
                {
                    free(verifySecuredPacket);
                    return commonHeaderProcessing((unsigned char*)securedPacket.data(), eth, bh);
                }
                free(verifySecuredPacket);
            }
            return geonetError_processing_bh_securedHeader;
            break;
        default:
            return geonetError_processing_bh_nextHeader;
            break;
        }
    }
}

int GeoNet::commonHeaderProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh)
{
    sniff_commonHeader_t* tmpch = (sniff_commonHeader_t*) packet;
    CommonHeader ch(*tmpch);

    if(ch.maximumHopLimit() < bh.rhl())
    {
        return geonetError_processing_ch_hopLimit;
    }
    else
    {
        bh.setRHL(bh.rhl() - 1);
        switch(ch.headerType())
        {
        default:
        case CommonHeader::headerType_any:
            return geonetError_processing_ch_headerType;
            break;
        case CommonHeader::headerType_beacon:
            return beaconPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
            break;
        case CommonHeader::headerType_geoAnycast:
            return gacPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
            break;
        case CommonHeader::headerType_geoBroadcast:
            return gbcPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
            break;
        case CommonHeader::headerType_geoUnicast:
            return gucPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
            break;
        case CommonHeader::headerType_topologicallyScopedBroadcast:
            switch(ch.headerSubType())
            {
            case CommonHeader::headerSubTypeTopologicallyScopedBroadcast_singleHop:
                return shbPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
                break;
            case CommonHeader::headerSubTypeTopologicallyScopedBroadcast_multiHop:
                return tsbPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
                break;
            default :
                return geonetError_processing_ch_headerSubType_TSB_unknown;
                break;
            }
            break;
        case CommonHeader::headerType_locationService:
            switch(ch.headerSubType())
            {
            case CommonHeader::headerSubTypeLocationService_Request:
                return LSRequestPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
                break;
            case CommonHeader::headerSubTypeLocationService_Reply:
                return LSReplyPacketProcessing(packet + CommonHeader::helper_size, eth, bh, ch);
                break;
            default :
                return geonetError_processing_ch_headerSubType_LS_unknown;
                break;
            }
            break;
        }
    }
}

int GeoNet::beaconPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    Q_UNUSED(ch);
    BeaconPacket beac;
    beac.extract(packet);

    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)beac.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* DuplicatePacketDetection */
    if(m_locationTable->find(packetIndex) != m_locationTable->end())
        if(m_HPB->isDuplicateTimestamp(beac) || isDuplicatePacketDetectionTimestamp((*m_locationTable)[packetIndex].lastTimestamp(), beac.soPv().timestamp()))
            return geonetError_processing_beacon_DPD;

    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(beac.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (beac.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            return geonetError_processing_beacon_DAD;
        }
    }

    QMutexLocker locker(m_mutexLocT);
    (*m_locationTable)[packetIndex].setLongPositionVector(beac.soPv());
    (*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
    (*m_locationTable)[packetIndex].setLastTimestamp(beac.soPv().timestamp());
    (*m_locationTable)[packetIndex].setVersion(bh.version());
    (*m_locationTable)[packetIndex].setStationType(beac.soPv().gnAddr().stationType());
    (*m_locationTable)[packetIndex].setIsNeighbour(true);

    calculAngleThreshold();

    /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */
    locker.unlock();
    //m_HPB->addPacket(bh, ch, beac);

    checkBCFPB();
    checkUCFPB();

    return geonetError_success;
}

int GeoNet::LSRequestPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    Q_UNUSED(eth);
    LSRequestPacket LSRequest;
    LSRequest.extract(packet);

    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)LSRequest.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* DuplicatePacketDetection */
    if(m_locationTable->find(packetIndex) != m_locationTable->end())
        if(m_HPB->isDuplicateTimestamp(LSRequest) || isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].lastTimestamp(), LSRequest.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), LSRequest.sequenceNumber()))
            return geonetError_processing_LSRequest_DPD;

    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(LSRequest.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (LSRequest.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            return geonetError_processing_LSRequest_DAD;
        }
    }

    QMutexLocker locker(m_mutexLocT);
    /* The entry does not exist and will be created */
    if((*m_locationTable).find(packetIndex) == (*m_locationTable).end())
    {
        (*m_locationTable)[packetIndex].setLongPositionVector(LSRequest.soPv());
        //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
        (*m_locationTable)[packetIndex].setVersion(bh.version());
        (*m_locationTable)[packetIndex].setStationType(LSRequest.soPv().gnAddr().stationType());
        (*m_locationTable)[packetIndex].setIsNeighbour(false);
        (*m_locationTable)[packetIndex].setLastTimestamp(LSRequest.soPv().timestamp());
        (*m_locationTable)[packetIndex].setSequenceNumber(LSRequest.sequenceNumber());

        /* TODO update the PDR
        itsGnMaxPacketDataRateEmaBeta
        itsGnMaxPacketDataRate
        (*m_locationTable)[packetIndex].setPDR(...) */

    }
    else /* The entry exist and will be updated */
    {
        /* If the packet is more recent the entry will be update */
        if(!m_HPB->isDuplicateTimestamp(LSRequest) && !isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].longPositionVector().timestamp(), LSRequest.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), LSRequest.sequenceNumber()))
        {
            (*m_locationTable)[packetIndex].setLongPositionVector(LSRequest.soPv());
            //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
            (*m_locationTable)[packetIndex].setVersion(bh.version());
            (*m_locationTable)[packetIndex].setStationType(LSRequest.soPv().gnAddr().stationType());
            (*m_locationTable)[packetIndex].setLastTimestamp(LSRequest.soPv().timestamp());
            (*m_locationTable)[packetIndex].setSequenceNumber(LSRequest.sequenceNumber());

            /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */

        }
    }
    locker.unlock();

    flushPB(LSRequest.soPv().gnAddr());

    if(LSRequest.destination() == *m_gnAddr)
    {
        LSReplyPacket LSReply;

        QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(LSRequest.soPv().gnAddr().toHex());
        if(pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai())
        {
            quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
            quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;

            LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);
            LSReply = LSReplyPacket(sequenceNumber, lpv, ShortPositionVector(LocationTableEntry(pos.value()).longPositionVector().gnAddr(), LocationTableEntry(pos.value()).longPositionVector().timestamp(), LocationTableEntry(pos.value()).longPositionVector().latitude(), LocationTableEntry(pos.value()).longPositionVector().longitude()));
        }
        else
            return geonetError_send_LSReply_destinationUnknown;


        CommonHeader ch;
        ch = CommonHeader(CommonHeader::nextHeaderType_any, CommonHeader::headerType_locationService, CommonHeader::headerSubTypeLocationService_Reply, m_paramConf->trafficClass(), m_paramAppli->data().itsGnIsMobile(), 0, itsGnDefaultHopLimit);

        BasicHeader bh;

        if(m_paramConf->itsGnSecurity() == itsGnSecurity_enabled)
            bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_securedPacket, LifeTime(60, LifeTime::base_1_s), itsGnDefaultHopLimit);
        else
            bh = BasicHeader(itsGnProtocolVersion, BasicHeader::nextHeader_commonHeader, LifeTime(60, LifeTime::base_1_s), itsGnDefaultHopLimit);


        ForwardingAlgorithmResponse res;
        switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
        {
        default:
        case itsGnGeoUnicastForwardingAlgorithm_unspecified:
        case itsGnGeoUnicastForwardingAlgorithm_greedy:
            res = greedy(bh, ch, LSReply, QByteArray(), QByteArray());
            break;
        case itsGnGeoUnicastForwardingAlgorithm_CBF:
            //res = CBFUnicast(bh, ch, LSReply, QByteArray());
            uchar NH[MAC_ADDR_SIZE];
            for(int i = 0; i < MAC_ADDR_SIZE; i++)
                NH[i] = 0xFF;
            res.setMacAddr(NH);
            res.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
            break;
        }

        if(res.isInteger() && res.integer() == 0)
            return geonetError_send_LSReply_bufferedPacket;
        else if(res.isInteger() && res.integer() == -1)
            return geonetError_send_LSReply_discardedPacket;

        /* Construct the packet */
        QByteArray LSReplyData;
        QDataStream ds(&LSReplyData, QIODevice::WriteOnly);

        ds.writeRawData(ch.toHex().data(), ch.toHex().length());
        ds.writeRawData(LSReply.toHex().data(), LSReply.toHex().length());

        if(res.isMacAddr())
            /* Construct the Ethernet header : destination addr */
            for(int i = 0 ; i < 6 ; i++)
                m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
        else
            return geonetError_send_LSReply_destinationAddress;

        int ret = send(LSReplyData, true, bh);
        if(ret == networkError_success)
        {
            //m_HPB->addPacket(bh, ch, LSReply);
            sequenceNumber++;
        }

        return ret;
    }
    else
    {
        /* Forwarding LSRequest packet */

        if(bh.rhl() < 1)
            return geonetError_success;

        quint16 port, portInfo;
        get_UINT16(packet + LSRequestPacket::helper_size, &port);
        get_UINT16(packet + LSRequestPacket::helper_size + 2, &portInfo);

        gen2fac->writeDatagram(QByteArray((const char*)packet + LSRequestPacket::helper_size + 4, ch.payloadLength() - 4),
                               QHostAddress::LocalHost, m_paramNetwork->internalBTPPort(port, portInfo));
        qDebug() << "Sending packet to" << port << portInfo << m_paramNetwork->internalBTPPort(port, portInfo);

        sendEventIndicationToUT(packet + LSRequestPacket::helper_size, ch.payloadLength());

        /* Construct the packet */
        QByteArray LSRequestData;
        QDataStream ds(&LSRequestData, QIODevice::WriteOnly);

        ds.writeRawData(ch.toHex().data(), ch.toHex().length());
        ds.writeRawData(LSRequest.toHex().data(), LSRequest.toHex().length());

        /* Construct the Ethernet header : destination addr */
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = 0xFF;

        return send(LSRequestData, false, bh);
    }
}

int GeoNet::LSReplyPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    Q_UNUSED(eth);
    LSReplyPacket lsReply;
    lsReply.extract(packet);

    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)lsReply.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* DuplicatePacketDetection */
    if(m_locationTable->find(packetIndex) != m_locationTable->end() && !(*m_locationTable)[packetIndex].neverReceived())
        if(m_HPB->isDuplicateTimestamp(lsReply) || isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].lastTimestamp(), lsReply.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), lsReply.sequenceNumber()))
            return geonetError_processing_LSReply_DPD;

    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(lsReply.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (lsReply.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            return geonetError_processing_LSReply_DAD;
        }
    }

    QMutexLocker locker(m_mutexLocT);
    /* The entry does not exist and will be created */
    if((*m_locationTable).find(packetIndex) == (*m_locationTable).end())
    {
        (*m_locationTable)[packetIndex].setLongPositionVector(lsReply.soPv());
        //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
        (*m_locationTable)[packetIndex].setVersion(bh.version());
        (*m_locationTable)[packetIndex].setLastTimestamp(lsReply.soPv().timestamp());
        (*m_locationTable)[packetIndex].setSequenceNumber(lsReply.sequenceNumber());
        (*m_locationTable)[packetIndex].setStationType(lsReply.soPv().gnAddr().stationType());
        (*m_locationTable)[packetIndex].setIsNeighbour(false);

        /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */

    }
    else /* The entry exist and will be updated */
    {
        /* If the packet is more recent the entry will be update */
        if(!m_HPB->isDuplicateTimestamp(lsReply) && !isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].longPositionVector().timestamp(), lsReply.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), lsReply.sequenceNumber()))
        {
            (*m_locationTable)[packetIndex].setLongPositionVector(lsReply.soPv());
            //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
            (*m_locationTable)[packetIndex].setVersion(bh.version());
            (*m_locationTable)[packetIndex].setStationType(lsReply.soPv().gnAddr().stationType());
            (*m_locationTable)[packetIndex].setLastTimestamp(lsReply.soPv().timestamp());
            (*m_locationTable)[packetIndex].setSequenceNumber(lsReply.sequenceNumber());

            /* TODO update the PDR
                itsGnMaxPacketDataRateEmaBeta
                itsGnMaxPacketDataRate
                (*m_locationTable)[packetIndex].setPDR(...) */
        }
    }


    if(lsReply.dePv().gnAddr() == *m_gnAddr)
    {/*
        (*m_locationTable)[packetIndex].setLastTimestamp(lsReply.soPv().timestamp());
        (*m_locationTable)[packetIndex].setSequenceNumber(lsReply.sequenceNumber());
        (*m_locationTable)[packetIndex].setVersion(bh.version());
        (*m_locationTable)[packetIndex].setStationType(lsReply.soPv().gnAddr().stationType());
        (*m_locationTable)[packetIndex].setLongPositionVector(lsReply.soPv());
        (*m_locationTable)[packetIndex].setIsNeighbour(false);*/

        (*m_locationTable)[packetIndex].setLsPending(false);
        (*m_locationTable)[packetIndex].setNeverReceived(false);

        locker.unlock();
        flushPB(lsReply.soPv().gnAddr());

        /* TODO update the PDR
        itsGnMaxPacketDataRateEmaBeta
        itsGnMaxPacketDataRate
        (*m_locationTable)[packetIndex].setPDR(...) */


        return geonetError_success;
    }
    else
    {
        locker.unlock();
        flushPB(lsReply.soPv().gnAddr());
        if(bh.rhl() < 1)
            return geonetError_success;

        ForwardingAlgorithmResponse res;

        switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
        {
        default:
        case itsGnGeoUnicastForwardingAlgorithm_unspecified:
        case itsGnGeoUnicastForwardingAlgorithm_greedy:
            res = greedy(bh, ch, lsReply, packetIndex);
            break;
        case itsGnGeoUnicastForwardingAlgorithm_CBF:
            //res = CBFUnicast(bh, ch, lsReply, QByteArray());
            uchar NH[MAC_ADDR_SIZE];
            for(int i = 0; i < MAC_ADDR_SIZE; i++)
                NH[i] = 0xFF;
            res.setMacAddr(NH);
            res.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
            break;
        }

        if(res.isInteger() && res.integer() == 0)
            return geonetError_forward_LSReply_bufferedPacket;
        else if(res.isInteger() && res.integer() == -1)
            return geonetError_forward_LSReply_discardedPacket;

        /* Construct the Ethernet header : destination addr */
        if(res.isMacAddr())
            for(int i = 0 ; i < 6 ; i++)
                m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
        else
            return geonetError_forward_LSReply_destinationAddress;

        /* Construct the packet */
        QByteArray basicHeaderData;
        QDataStream ds(&basicHeaderData, QIODevice::WriteOnly);

        ds.writeRawData(ch.toHex().data(), ch.toHex().length());
        ds.writeRawData(lsReply.toHex().data(), lsReply.toHex().length());

        int ret = send(basicHeaderData, false, bh);
        if(ret == geonetError_success)
            printf("\nSuccesfully forwarded LSReply");

        return ret;
    }
}

int GeoNet::gacPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    QByteArray payload((char*)packet + GACPacket::helper_size, ch.payloadLength());
    GACPacket gac;
    gac.extract(packet);

    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)gac.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* DuplicatePacketDetection */

    if(m_locationTable->find(packetIndex) != m_locationTable->end())
        if(m_HPB->isDuplicateTimestamp(gac) || isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].lastTimestamp(), gac.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), gac.sequenceNumber()))
            return geonetError_processing_gac_DPD;

    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(gac.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (gac.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            return geonetError_processing_gac_DAD;
        }
    }

    QMutexLocker locker(m_mutexLocT);
    /* The entry does not exist and will be created */
    if((*m_locationTable).find(packetIndex) == (*m_locationTable).end())
    {
        (*m_locationTable)[packetIndex].setLongPositionVector(gac.soPv());
        //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
        (*m_locationTable)[packetIndex].setVersion(bh.version());
        (*m_locationTable)[packetIndex].setStationType(gac.soPv().gnAddr().stationType());
        (*m_locationTable)[packetIndex].setIsNeighbour(false);
        (*m_locationTable)[packetIndex].setLastTimestamp(gac.soPv().timestamp());
        (*m_locationTable)[packetIndex].setSequenceNumber(gac.sequenceNumber());

        /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */

    }
    else /* The entry exist and will be updated */
    {
        /* If the packet is more recent the entry will be update */
        if(isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].longPositionVector().timestamp(), gac.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), gac.sequenceNumber()))
        {
            (*m_locationTable)[packetIndex].setLongPositionVector(gac.soPv());
            //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
            (*m_locationTable)[packetIndex].setVersion(bh.version());
            (*m_locationTable)[packetIndex].setStationType(gac.soPv().gnAddr().stationType());
            (*m_locationTable)[packetIndex].setLastTimestamp(gac.soPv().timestamp());
            (*m_locationTable)[packetIndex].setSequenceNumber(gac.sequenceNumber());

            /* TODO update the PDR
                itsGnMaxPacketDataRateEmaBeta
                itsGnMaxPacketDataRate
                (*m_locationTable)[packetIndex].setPDR(...) */
        }
    }
    locker.unlock();

    flushPB(gac.soPv().gnAddr());

    if(bh.rhl() < 1)
        return geonetError_success;


    e_shape shape;
    switch(ch.headerSubType())
    {
    case CommonHeader::headerSubTypeGeoBroadcast_circle:
        shape = shape_circle;
        if(((gac.destinationArea().distanceA() * gac.destinationArea().distanceA()) * M_PI) > (itsGnMaxGeoAreaSize * 1000 * 1000))
            return geonetError_forward_gac_areaSize;
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_rectangle:
        shape = shape_rectangle;
        if((gac.destinationArea().distanceA() * gac.destinationArea().distanceB()) > (itsGnMaxGeoAreaSize * 1000 * 1000))
            return geonetError_forward_gac_areaSize;
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_ellipse:
        shape = shape_ellipse;
        if((gac.destinationArea().distanceA() * gac.destinationArea().distanceB() * M_PI) > (itsGnMaxGeoAreaSize * 1000 * 1000))
            return geonetError_forward_gac_areaSize;
        break;
    default :
        shape = shape_none;
        break;
    }

    double x, y;
    x = diameter(m_paramGPS->data().referencePosition().longitude, gac.destinationArea().geoAreaPosLongitude(), circAtLat((m_paramGPS->data().referencePosition().latitude + gac.destinationArea().geoAreaPosLatitude())/2)) / 10000000.;
    y = LAT2METER*((m_paramGPS->data().referencePosition().latitude - gac.destinationArea().geoAreaPosLatitude())/10000000.);
    y *= y < 0 ? -1 : 1;

    double inArea;
    if(shape == shape_circle)
        inArea = f(x, y, gac.destinationArea().distanceA(), gac.destinationArea().distanceB(), shape);
    else
        inArea = f(
                x*cos(gac.destinationArea().angle())-y*sin(gac.destinationArea().angle()),
                x*sin(gac.destinationArea().angle())+y*cos(gac.destinationArea().angle()),
                gac.destinationArea().distanceA(), gac.destinationArea().distanceB(), shape);
    /* Router is in the destination area */
    if(inArea >= 0)
    {
        quint16 port, portInfo;
        get_UINT16(packet + GACPacket::helper_size, &port);
        get_UINT16(packet + GACPacket::helper_size + 2, &portInfo);

        gen2fac->writeDatagram(QByteArray((const char*)packet + GACPacket::helper_size + 4, ch.payloadLength() - 4),
                               QHostAddress::LocalHost, m_paramNetwork->internalBTPPort(port, portInfo));
        qDebug() << "Sending packet to" << port << portInfo << m_paramNetwork->internalBTPPort(port, portInfo);

        sendEventIndicationToUT((uchar*)payload.data(), payload.length());

        return geonetError_success;
    }
    else
    {
        if(m_paramConf->itsGnGeoAreaLineForwarding())
        {
            ForwardingAlgorithmResponse res;

            switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
            {
            default:
            case itsGnGeoUnicastForwardingAlgorithm_unspecified:
            case itsGnGeoUnicastForwardingAlgorithm_greedy:
                res = greedy(bh, ch, gac, payload, packetIndex, eth->ether_dhost, eth->ether_shost);
                break;
            case itsGnGeoUnicastForwardingAlgorithm_CBF:
                res = CBFUnicast(bh, ch, gac, payload);
                break;
            }

            if(res.isInteger() && res.integer() == 0)
                return geonetError_forward_gac_bufferedPacket;
            else if(res.isInteger() && res.integer() == -1)
                return geonetError_forward_gac_discardedPacket;

            /* Construct the Ethernet header : destination addr */
            if(res.isMacAddr())
                for(int i = 0 ; i < 6 ; i++)
                    m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
            else
                return geonetError_forward_gac_destinationAddress;

            /* Construct the packet */
            QByteArray basicHeaderData;
            QDataStream ds(&basicHeaderData, QIODevice::WriteOnly);

            ds.writeRawData(ch.toHex().data(), ch.toHex().length());
            ds.writeRawData(gac.toHex().data(), gac.toHex().length());
            ds.writeRawData(payload, payload.length());

            int ret = send(basicHeaderData, false, bh);
            if(ret == geonetError_success)
                printf("\nSuccesfully forwarded GAC");

            return ret;
        }
        else
            return geonetError_forward_gac_noAreaLineForwarding;
    }
}

int GeoNet::gbcPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    QByteArray payload((char*)packet + GBCPacket::helper_size, ch.payloadLength());
    GBCPacket gbc;
    gbc.extract(packet);

    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)gbc.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* If itsGnGeoBroadcastForwardingAlgorithm is Unspecified or Simple */
    if(m_paramConf->itsGnGeoBroadcastForwardingAlgorithm() == itsGnGeoBroadcastForwardingAlgorithm_unspecified ||
            m_paramConf->itsGnGeoBroadcastForwardingAlgorithm() == itsGnGeoBroadcastForwardingAlgorithm_simple)
    {
        /* DuplicatePacketDetection */
        if(m_locationTable->find(packetIndex) != m_locationTable->end())
            if(m_HPB->isDuplicateTimestamp(gbc) || isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].lastTimestamp(), gbc.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), gbc.sequenceNumber()))
            {
                printf("DPD\n");
                return geonetError_processing_gbc_DPD;
            }
    }

    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(gbc.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (gbc.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            printf("DAD\n");
            return geonetError_processing_gbc_DAD;
        }
    }

    QMutexLocker locker(m_mutexLocT);
    /* The entry does not exist and will be created */
    if((*m_locationTable).find(packetIndex) == (*m_locationTable).end())
    {
        (*m_locationTable)[packetIndex].setLongPositionVector(gbc.soPv());
        //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
        (*m_locationTable)[packetIndex].setVersion(bh.version());
        (*m_locationTable)[packetIndex].setStationType(gbc.soPv().gnAddr().stationType());
        (*m_locationTable)[packetIndex].setIsNeighbour(false);
        (*m_locationTable)[packetIndex].setLastTimestamp(gbc.soPv().timestamp());
        (*m_locationTable)[packetIndex].setSequenceNumber(gbc.sequenceNumber());

        /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */

    }
    else /* The entry exist and will be updated */
    {
        /* If the packet is more recent the entry will be update */
        if(!m_HPB->isDuplicateTimestamp(gbc) && !isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].longPositionVector().timestamp(), gbc.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), gbc.sequenceNumber()))
        {
            (*m_locationTable)[packetIndex].setLongPositionVector(gbc.soPv());
            //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
            (*m_locationTable)[packetIndex].setVersion(bh.version());
            (*m_locationTable)[packetIndex].setStationType(gbc.soPv().gnAddr().stationType());
            (*m_locationTable)[packetIndex].setLastTimestamp(gbc.soPv().timestamp());
            (*m_locationTable)[packetIndex].setSequenceNumber(gbc.sequenceNumber());
            /* TODO update the PDR
                itsGnMaxPacketDataRateEmaBeta
                itsGnMaxPacketDataRate
                (*m_locationTable)[packetIndex].setPDR(...) */

        }
    }
    locker.unlock();
    //m_HPB->addPacket(bh, ch, gbc, payload);
    flushPB(gbc.soPv().gnAddr());

    e_shape shape;
    switch(ch.headerSubType())
    {
    case CommonHeader::headerSubTypeGeoBroadcast_circle:
        shape = shape_circle;
        if(((gbc.destinationArea().distanceA() * gbc.destinationArea().distanceA()) * M_PI) > (itsGnMaxGeoAreaSize * 1000 * 1000))
        {
            printf("GBC_AREA_SIZE_C\n");
            return geonetError_forward_gbc_areaSize;
        }
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_rectangle:
        shape = shape_rectangle;
        if((gbc.destinationArea().distanceA() * gbc.destinationArea().distanceB()) > (itsGnMaxGeoAreaSize * 1000 * 1000))
        {
            printf("GBC_AREA_SIZE_R\n");
            return geonetError_forward_gbc_areaSize;
        }
        break;
    case CommonHeader::headerSubTypeGeoBroadcast_ellipse:
        shape = shape_ellipse;
        if((gbc.destinationArea().distanceA() * gbc.destinationArea().distanceB() * M_PI) > (itsGnMaxGeoAreaSize * 1000 * 1000))
        {
            printf("GBC_AREA_SIZE_E\n");
            return geonetError_forward_gbc_areaSize;
        }
        break;
    default :
        shape = shape_none;
        break;
    }

    double x, y;
    x = diameter(m_paramGPS->data().referencePosition().longitude, gbc.destinationArea().geoAreaPosLongitude(),
                 circAtLat((m_paramGPS->data().referencePosition().latitude + gbc.destinationArea().geoAreaPosLatitude())/2)) / 10000000.;
    y = LAT2METER*((m_paramGPS->data().referencePosition().latitude - gbc.destinationArea().geoAreaPosLatitude())/10000000.);
    y *= y < 0 ? -1 : 1;

    double inArea;
    if(shape == shape_circle)
        inArea = f(x, y, gbc.destinationArea().distanceA(), gbc.destinationArea().distanceB(), shape);
    else
        inArea = f(
                x*cos(gbc.destinationArea().angle())-y*sin(gbc.destinationArea().angle()),
                x*sin(gbc.destinationArea().angle())+y*cos(gbc.destinationArea().angle()),
                gbc.destinationArea().distanceA(), gbc.destinationArea().distanceB(), shape);
    /* Router is in the destination area */
    if(inArea>=0)
    {
        quint16 port, portInfo;
        get_UINT16(packet + GBCPacket::helper_size, &port);
        get_UINT16(packet + GBCPacket::helper_size + 2, &portInfo);

        printf("PACKET send to FAC\n");
        gen2fac->writeDatagram(QByteArray((const char*)packet + GBCPacket::helper_size + 4, ch.payloadLength() - 4),
                               QHostAddress::LocalHost, m_paramNetwork->internalBTPPort(port, portInfo));
        qDebug() << "Sending packet to" << port << portInfo << m_paramNetwork->internalBTPPort(port, portInfo);

        sendEventIndicationToUT((uchar*)payload.data(), payload.length());
    }
    else
        printf("Not in area\n");

    /* Forwarding GBC packet */

    if(bh.rhl() < 1)
        return geonetError_success;

    ForwardingAlgorithmResponse res;
    switch(m_paramConf->itsGnGeoBroadcastForwardingAlgorithm())
    {
    default:
    case itsGnGeoBroadcastForwardingAlgorithm_unspecified:
    case itsGnGeoBroadcastForwardingAlgorithm_simple:
        res = simpleForwardingAlgorithm(bh, ch, gbc, payload, shape, packetIndex, false);
        break;
    case itsGnGeoBroadcastForwardingAlgorithm_CBF:
        res = contentionBasedForwardingAlgorithm(bh, ch, gbc, payload, shape, packetIndex, false);
        break;
    case itsGnGeoBroadcastForwardingAlgorithm_advanced:
        res = advancedForwarding(bh, ch, gbc, payload, shape, packetIndex, eth->ether_dhost, eth->ether_shost, false);
        break;
    }

    if(res.isInteger() && res.integer() == 0)
        return geonetError_forward_gbc_bufferedPacket;
    else if(res.isInteger() && res.integer() == -1)
        return geonetError_forward_gbc_discardedPacket;

    /* Construct the Ethernet header : destination addr */
    if(res.isMacAddr())
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
    else
        return geonetError_forward_gbc_destinationAddress;

    /* Construct the packet */
    QByteArray basicHeaderData;
    QDataStream ds(&basicHeaderData, QIODevice::WriteOnly);

    ds.writeRawData(ch.toHex().data(), ch.toHex().length());
    ds.writeRawData(gbc.toHex().data(), gbc.toHex().length());
    ds.writeRawData(payload, payload.length());

    int ret = send(basicHeaderData, false, bh);
    if(ret == geonetError_success)
        printf("\nSuccesfully forwarded GBC");

    return ret;
}

int GeoNet::gucPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    Q_UNUSED(eth);
    QByteArray payload((char*)packet + GUCPacket::helper_size, ch.payloadLength());
    GUCPacket guc;
    guc.extract(packet);

    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)guc.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* If itsGnGeoBroadcastForwardingAlgorithm is Unspecified or Simple */
    if(m_paramConf->itsGnGeoUnicastForwardingAlgorithm() == itsGnGeoBroadcastForwardingAlgorithm_unspecified ||
            m_paramConf->itsGnGeoUnicastForwardingAlgorithm() == itsGnGeoBroadcastForwardingAlgorithm_simple)
    {
        /* DuplicatePacketDetection */
        if(m_locationTable->find(packetIndex) != m_locationTable->end() && !(*m_locationTable)[packetIndex].isPending())
            if(m_HPB->isDuplicateTimestamp(guc) || isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].lastTimestamp(), guc.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), guc.sequenceNumber()))
                return geonetError_processing_guc_DPD;
    }

    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(guc.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (guc.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            return geonetError_processing_guc_DAD;
        }
    }

    QMutexLocker locker(m_mutexLocT);
    /* The entry does not exist and will be created */
    if((*m_locationTable).find(packetIndex) == (*m_locationTable).end())
    {
        (*m_locationTable)[packetIndex].setLongPositionVector(guc.soPv());
        //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
        (*m_locationTable)[packetIndex].setVersion(bh.version());
        (*m_locationTable)[packetIndex].setStationType(guc.soPv().gnAddr().stationType());
        (*m_locationTable)[packetIndex].setIsNeighbour(false);
        (*m_locationTable)[packetIndex].setLastTimestamp(guc.soPv().timestamp());
        (*m_locationTable)[packetIndex].setSequenceNumber(guc.sequenceNumber());

        /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */

    }
    else /* The entry exist and will be updated */
    {
        /* If the packet is more recent the entry will be update */
        if((!m_HPB->isDuplicateTimestamp(guc) &&
            !isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].longPositionVector().timestamp(), guc.soPv().timestamp(),
                                                  (*m_locationTable)[packetIndex].sequenceNumber(), guc.sequenceNumber()))
          || (*m_locationTable)[packetIndex].isPending())
        {
            (*m_locationTable)[packetIndex].setLongPositionVector(guc.soPv());
            //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
            (*m_locationTable)[packetIndex].setVersion(bh.version());
            (*m_locationTable)[packetIndex].setStationType(guc.soPv().gnAddr().stationType());
            (*m_locationTable)[packetIndex].setLastTimestamp(guc.soPv().timestamp());
            (*m_locationTable)[packetIndex].setSequenceNumber(guc.sequenceNumber());
            if((*m_locationTable)[packetIndex].isPending())
            {
                (*m_locationTable)[packetIndex].setNeverReceived(false);
                (*m_locationTable)[packetIndex].setLsPending(false);
            }

            /* TODO update the PDR
                itsGnMaxPacketDataRateEmaBeta
                itsGnMaxPacketDataRate
                (*m_locationTable)[packetIndex].setPDR(...) */

        }
    }

    locker.unlock();

    flushPB(guc.soPv().gnAddr());

    /* If this routeur is the destination */
    if(guc.dePv().gnAddr() == *m_gnAddr)
    {
        quint16 port, portInfo;
        get_UINT16(packet + GUCPacket::helper_size, &port);
        get_UINT16(packet + GUCPacket::helper_size + 2, &portInfo);

        gen2fac->writeDatagram(QByteArray((const char*)packet + GUCPacket::helper_size + 4, ch.payloadLength() - 4),
                               QHostAddress::LocalHost, m_paramNetwork->internalBTPPort(port, portInfo));
        qDebug() << "Sending packet to" << port << portInfo << m_paramNetwork->internalBTPPort(port, portInfo);

        sendEventIndicationToUT((uchar*)payload.data(), payload.length());

        return geonetError_success;
    }

    locker.relock();

    QByteArray destIndex = QByteArray((const char*)guc.dePv().gnAddr().toHex(), GnAddr::helper_size);

    QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(destIndex);
    if(pos == m_locationTable->end() || LocationTableEntry(pos.value()).isNeighbour() == 0)
        if(bh.nextHeader()==0 || bh.nextHeader()==1)
            (*m_locationTable)[destIndex].setLongPositionVector(LongPositionVector(guc.dePv().gnAddr(), guc.dePv().timestamp(), guc.dePv().latitude(), guc.dePv().longitude(), 0, 0, 0));


    /* If the destination position vector is different than our we update it */
    if((*m_locationTable)[destIndex].lastTimestamp() < guc.dePv().timestamp())
    {
        (*m_locationTable)[destIndex].setLastTimestamp(guc.dePv().timestamp());
        (*m_locationTable)[destIndex].setLongPositionVector(guc.dePv());
    }
    else
    {
        guc.setDePv((*m_locationTable)[destIndex].longPositionVector());
    }


    locker.unlock();

    if(bh.rhl() < 1)
        return geonetError_success;

    ForwardingAlgorithmResponse res;

    switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
    {
    default:
    case itsGnGeoUnicastForwardingAlgorithm_unspecified:
    case itsGnGeoUnicastForwardingAlgorithm_greedy:
        res = greedy(bh, ch, guc, payload, packetIndex);
        break;
    case itsGnGeoUnicastForwardingAlgorithm_CBF:
        res = CBFUnicast(bh, ch, guc, payload);
        break;
    }

    if(res.isInteger() && res.integer() == 0)
        return geonetError_forward_guc_bufferedPacket;
    else if(res.isInteger() && res.integer() == -1)
        return geonetError_forward_guc_discardedPacket;

    /* Construct the Ethernet header : destination addr */
    if(res.isMacAddr())
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = res.macAddr()[i];
    else
        return geonetError_forward_guc_destinationAddress;

    /* Construct the packet */
    QByteArray basicHeaderData;
    QDataStream ds(&basicHeaderData, QIODevice::WriteOnly);

    ds.writeRawData(ch.toHex().data(), ch.toHex().length());
    ds.writeRawData(guc.toHex().data(), guc.toHex().length());
    ds.writeRawData(payload, payload.length());

    int ret = send(basicHeaderData, false, bh);
    if(ret == geonetError_success)
        printf("\nSuccesfully forwarded GUC");

    return ret;
}

int GeoNet::shbPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    SHBPacket shb;
    shb.extract(packet);
    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)shb.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* DuplicatePacketDetection */
    if(m_locationTable->find(packetIndex) != m_locationTable->end())
        if(m_HPB->isDuplicateTimestamp(shb) || isDuplicatePacketDetectionTimestamp((*m_locationTable)[packetIndex].lastTimestamp(), shb.soPv().timestamp()))
            return geonetError_processing_shb_DPD;
    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(shb.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (shb.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            return geonetError_processing_shb_DAD;
        }
    }

    QMutexLocker locker(m_mutexLocT);
    (*m_locationTable)[packetIndex].setLongPositionVector(shb.soPv());
    (*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
    (*m_locationTable)[packetIndex].setLastTimestamp(shb.soPv().timestamp());
    (*m_locationTable)[packetIndex].setVersion(bh.version());
    (*m_locationTable)[packetIndex].setStationType(shb.soPv().gnAddr().stationType());
    (*m_locationTable)[packetIndex].setIsNeighbour(true);

    calculAngleThreshold();

    /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */
    locker.unlock();

    checkBCFPB();
    checkUCFPB();

    quint16 port, portInfo;
    get_UINT16(packet + SHBPacket::helper_size, &port);
    get_UINT16(packet + SHBPacket::helper_size + 2, &portInfo);

    gen2fac->writeDatagram(QByteArray((const char*)packet + SHBPacket::helper_size + 4, ch.payloadLength() - 4),
                           QHostAddress::LocalHost, m_paramNetwork->internalBTPPort(port, portInfo));
    qDebug() << "Sending packet to" << port << portInfo << m_paramNetwork->internalBTPPort(port, portInfo);

    sendEventIndicationToUT(packet + SHBPacket::helper_size, ch.payloadLength());
    return geonetError_success;
}

int GeoNet::tsbPacketProcessing(uchar* packet, sniff_ethernet_t* eth, BasicHeader bh, CommonHeader ch)
{
    Q_UNUSED(eth);
    QByteArray payload((const char*)packet + TSBPacket::helper_size, ch.payloadLength());
    TSBPacket tsb;
    tsb.extract(packet);

    verifLocT();

    QByteArray packetIndex = QByteArray((const char*)tsb.soPv().gnAddr().toHex(), GnAddr::helper_size);

    /* DuplicatePacketDetection */
    if(m_locationTable->find(packetIndex) != m_locationTable->end())
        if(m_HPB->isDuplicateTimestamp(tsb) || isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].lastTimestamp(), tsb.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), tsb.sequenceNumber()))
            return geonetError_processing_tsb_DPD;
    /*  DuplicateAddressDetection */
    if(m_paramConf->itsGnLocalAddrConfMethod() == itsGnLocalAddrConfMethod_auto)
    {
        bool a = true;
        bool b = true;

        for(int i=0; i<6; i++)
            if(tsb.soPv().gnAddr().mid()[i] != m_gnAddr->mid(i))
                a = false;
        b = (tsb.soPv().gnAddr().MStationTypeCountryCode() == m_gnAddr->MStationTypeCountryCode());

        if(a && b)
        {
            emit m_manager->changeMID();
            m_RP->m_changeFilter = true;
            return geonetError_processing_tsb_DAD;
        }
    }


    QMutexLocker locker(m_mutexLocT);
    /* The entry does not exist and will be created */
    if((*m_locationTable).find(packetIndex) == (*m_locationTable).end())
    {
        (*m_locationTable)[packetIndex].setLongPositionVector(tsb.soPv());
        //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
        (*m_locationTable)[packetIndex].setVersion(bh.version());
        (*m_locationTable)[packetIndex].setStationType(tsb.soPv().gnAddr().stationType());
        (*m_locationTable)[packetIndex].setIsNeighbour(false);
        (*m_locationTable)[packetIndex].setLastTimestamp(tsb.soPv().timestamp());
        (*m_locationTable)[packetIndex].setSequenceNumber(tsb.sequenceNumber());

        /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */

    }
    else /* The entry exist and will be updated */
    {
        /* If the packet is more recent the entry will be update */
        if(!m_HPB->isDuplicateTimestamp(tsb) && !isDuplicatePacketDetectionTimestampSN((*m_locationTable)[packetIndex].longPositionVector().timestamp(), tsb.soPv().timestamp(), (*m_locationTable)[packetIndex].sequenceNumber(), tsb.sequenceNumber()))
        {
            (*m_locationTable)[packetIndex].setLongPositionVector(tsb.soPv());
            //(*m_locationTable)[packetIndex].setLlAddr(eth->ether_shost);
            (*m_locationTable)[packetIndex].setVersion(bh.version());
            (*m_locationTable)[packetIndex].setStationType(tsb.soPv().gnAddr().stationType());
            (*m_locationTable)[packetIndex].setLastTimestamp(tsb.soPv().timestamp());
            (*m_locationTable)[packetIndex].setSequenceNumber(tsb.sequenceNumber());

            /* TODO update the PDR
                itsGnMaxPacketDataRateEmaBeta
                itsGnMaxPacketDataRate
                (*m_locationTable)[packetIndex].setPDR(...) */

        }
    }


    /* TODO update the PDR
            itsGnMaxPacketDataRateEmaBeta
            itsGnMaxPacketDataRate
            (*m_locationTable)[packetIndex].setPDR(...) */
    locker.unlock();
    flushPB(tsb.soPv().gnAddr());

    quint16 port, portInfo;
    get_UINT16(packet + TSBPacket::helper_size, &port);
    get_UINT16(packet + TSBPacket::helper_size + 2, &portInfo);

    gen2fac->writeDatagram(QByteArray((const char*)packet + TSBPacket::helper_size + 4, ch.payloadLength() - 4),
                           QHostAddress::LocalHost, m_paramNetwork->internalBTPPort(port, portInfo));
    qDebug() << "Sending packet to" << port << portInfo << m_paramNetwork->internalBTPPort(port, portInfo);

    sendEventIndicationToUT(packet + TSBPacket::helper_size, ch.payloadLength());

    /* Forwarding TSB packet */

    if(bh.rhl() < 1)
        return geonetError_success;

    bool noNeighbour = true;
    locker.relock();
    if(!m_locationTable->isEmpty())
        for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();i != m_locationTable->end(); ++i)
            if(LocationTableEntry(i.value()).isNeighbour())
                noNeighbour = false;
    locker.unlock();

    if(noNeighbour && ch.trafficClass().SCF() == 1)
    {
        m_BCFPB->addPacket(bh, ch, tsb, payload, packetIndex);
        return geonetError_forward_tsb_bufferedPacket;
    }
    else
    {
        /* Construct the packet */
        QByteArray tsbData;
        QDataStream ds(&tsbData, QIODevice::WriteOnly);

        ds.writeRawData(ch.toHex().data(), ch.toHex().length());
        ds.writeRawData(tsb.toHex().data(), tsb.toHex().length());
        ds.writeRawData(payload.data(), payload.length());

        /* Construct the Ethernet header : destination addr */
        for(int i = 0 ; i < 6 ; i++)
            m_paramNetwork->eh.ether_dhost[i] = 0xFF;

        int ret = send(tsbData, false, bh);

        return ret;
    }
    return geonetError_success;
}

/* -------------------------FORWARDING-------------------------ALGORITHM------------------------- */

int GeoNet::TO_CBF_GBC(int distance)
{
    if(distance > itsGnDefaultMaxCommunicationRange)
        return itsGnGeoBroadcastCbfMinTime;
    else
        return itsGnGeoBroadcastCbfMaxTime + (((itsGnGeoBroadcastCbfMinTime - itsGnGeoBroadcastCbfMaxTime) / itsGnDefaultMaxCommunicationRange) * distance);
}

int GeoNet::TO_CBF_GUC(int progress)
{
    if(progress > itsGnDefaultMaxCommunicationRange)
        return itsGnGeoBroadcastCbfMinTime;
    else
        return itsGnGeoBroadcastCbfMaxTime + (((itsGnGeoBroadcastCbfMinTime - itsGnGeoBroadcastCbfMaxTime) / itsGnDefaultMaxCommunicationRange) * progress);
}

ForwardingAlgorithmResponse GeoNet::greedy(BasicHeader bh, CommonHeader ch, GACPacket packet, QByteArray payload, QByteArray sender, uchar* destination, uchar* forwarder)
{return greedy(PacketBuffersEntry(bh, ch, packet, payload, destination, forwarder), sender);}

ForwardingAlgorithmResponse GeoNet::greedy(BasicHeader bh, CommonHeader ch, GBCPacket packet, QByteArray payload, QByteArray sender, uchar* destination, uchar* forwarder)
{return greedy(PacketBuffersEntry(bh, ch, packet, payload, destination, forwarder), sender);}

ForwardingAlgorithmResponse GeoNet::greedy(BasicHeader bh, CommonHeader ch, GUCPacket packet, QByteArray payload, QByteArray sender)
{return greedy(PacketBuffersEntry(bh, ch, packet, payload), sender);}

ForwardingAlgorithmResponse GeoNet::greedy(BasicHeader bh, CommonHeader ch, LSReplyPacket packet, QByteArray sender)
{return greedy(PacketBuffersEntry (bh, ch, packet), sender);}

ForwardingAlgorithmResponse GeoNet::greedy(PacketBuffersEntry packet, QByteArray sender)
{
    ForwardingAlgorithmResponse retour;
    uchar NH[6];
    double distDest;
    qint32 packetLat, packetLong;
    switch(packet.commonHeader().headerType())
    {
    case CommonHeader::headerType_geoAnycast:
        packetLat = packet.gacPacket().destinationArea().geoAreaPosLatitude();
        packetLong = packet.gacPacket().destinationArea().geoAreaPosLongitude();
        break;
    case CommonHeader::headerType_geoBroadcast:
        packetLat = packet.gbcPacket().destinationArea().geoAreaPosLatitude();
        packetLong = packet.gbcPacket().destinationArea().geoAreaPosLongitude();
        break;
    case CommonHeader::headerType_geoUnicast:
        packetLat = packet.gucPacket().dePv().latitude();
        packetLong = packet.gucPacket().dePv().longitude();
        break;
    case CommonHeader::headerType_locationService:
        if(packet.commonHeader().headerSubType() == CommonHeader::headerSubTypeLocationService_Reply)
        {
            packetLat = packet.lsReplyPacket().dePv().latitude();
            packetLong = packet.lsReplyPacket().dePv().longitude();
        }
        else
        {
            retour.setInteger(-1);
            return retour;
        }
        break;
    case CommonHeader::headerType_any:
    default:
        retour.setInteger(-1);
        return retour;
        break;
    }

    distDest = dist(packetLat, packetLong, m_paramGPS->data().referencePosition().latitude,
                    m_paramGPS->data().referencePosition().longitude);

    double MFR = distDest;
    bool hasNeighbour = false;

    int cpt = 0;

    QMutexLocker locker(m_mutexLocT);
    for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();i != m_locationTable->end(); ++i, cpt++)
    {
        LocationTableEntry tmp(i.value());
        if(i.key() != sender && tmp.isNeighbour() != 0 && *tmp.isNeighbour())
        {
            hasNeighbour = true;
            double distNeighbour = dist(packetLat, packetLong, tmp.longPositionVector().latitude(),
                                        tmp.longPositionVector().longitude());
            if(distNeighbour<MFR)
            {
                for(int i = 0; i < MAC_ADDR_SIZE; i++)
                    NH[i] = tmp.llAddr()[i];
                MFR = distNeighbour;
            }
        }
    }
    locker.unlock();

    if(MFR<distDest)
    {
        retour.setMacAddr(NH);

        retour.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
        return retour;
    }
    else
    {
        if(!hasNeighbour && packet.commonHeader().trafficClass().SCF())
        {
            /* Ajouter le packet au buffer (bc ou uc) */
            if(packet.commonHeader().headerType() == CommonHeader::headerType_geoUnicast)
            {
                m_UCFPB->addPacket(packet.basicHeader(), packet.commonHeader(), packet.gucPacket(), packet.payload(), sender);
                printf("Packet stocké dans le UnicastPacketBuffer\n");
            }
            else if(packet.commonHeader().headerType() == CommonHeader::headerType_geoBroadcast)
            {
                m_BCFPB->addPacket(packet.basicHeader(), packet.commonHeader(), packet.gbcPacket(), packet.payload(), sender, packet.destination(), packet.forwarder());
                printf("Packet stocké dans le BroadcastPacketBuffer\n");
            }
            retour.setPresent(ForwardingAlgorithmResponse_PR_Integer);
            retour.setInteger(0);
            return retour;
        }
        else
        {
            for(int i = 0; i < MAC_ADDR_SIZE; i++)
                NH[i] = 0xFF;
            retour.setMacAddr(NH);
            retour.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
            return retour;
        }
    }
}

ForwardingAlgorithmResponse GeoNet::simpleForwardingAlgorithm(BasicHeader bh, CommonHeader ch, GBCPacket packet, QByteArray packetPayload, e_shape shape, QByteArray sender, bool flagSrc)
{
    ForwardingAlgorithmResponse addr;
    double x, y;

    x = diameter(m_paramGPS->data().referencePosition().longitude, packet.destinationArea().geoAreaPosLongitude(),
                 circAtLat((m_paramGPS->data().referencePosition().latitude + packet.destinationArea().geoAreaPosLatitude()) / 2));
    y = LAT2METER * ((m_paramGPS->data().referencePosition().latitude - packet.destinationArea().geoAreaPosLatitude()) / 10000000.);
    y *= y < 0 ? -1 : 1;

    double inArea;
    if(shape == shape_circle)
        inArea = f(x, y, packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
    else
        inArea = f(
                x*cos(packet.destinationArea().angle())-y*sin(packet.destinationArea().angle()),
                x*sin(packet.destinationArea().angle())+y*cos(packet.destinationArea().angle()),
                packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
    if(inArea>=0)
    {
        uchar NH[MAC_ADDR_SIZE];
        for(int i = 0; i < MAC_ADDR_SIZE; i++)
            NH[i] = 0xFF;
        addr.setMacAddr(NH);
        addr.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
        return addr;
    }
    else
    {
        if(flagSrc)
        {
            return greedy(bh, ch, packet, packetPayload, sender, m_gnAddr->mid(), m_gnAddr->mid());
        }
        else
        {
            QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(sender);
            if(pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai())
            {
                LocationTableEntry lte = LocationTableEntry(pos.value());

                x = diameter(lte.longPositionVector().longitude(), packet.destinationArea().geoAreaPosLongitude(),
                             circAtLat(lte.longPositionVector().latitude() + packet.destinationArea().geoAreaPosLatitude())/2);
                y = LAT2METER*((lte.longPositionVector().latitude() - packet.destinationArea().geoAreaPosLatitude())/10000000.);
                y *= y<0?-1:1;

                double inAreaExp;
                if(shape == shape_circle)
                    inAreaExp = f(x, y, packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
                else
                    inAreaExp = f(
                            x*cos(packet.destinationArea().angle())-y*sin(packet.destinationArea().angle()),
                            x*sin(packet.destinationArea().angle())+y*cos(packet.destinationArea().angle()),
                            packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);

                if(inAreaExp<0)
                {
                    return greedy(bh, ch, packet, packetPayload, sender, m_gnAddr->mid(), m_gnAddr->mid());
                }
                else
                {
                    addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                    addr.setInteger(-1);
                    return addr;
                }
            }
            else
            {
                uchar NH[MAC_ADDR_SIZE];
                for(int i = 0; i < MAC_ADDR_SIZE; i++)
                    NH[i] = 0xFF;
                addr.setMacAddr(NH);
                addr.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
                return addr;
            }
        }
    }
}

ForwardingAlgorithmResponse GeoNet::contentionBasedForwardingAlgorithm(BasicHeader bh, CommonHeader ch, GBCPacket packet, QByteArray packetPayload, e_shape shape, QByteArray sender, bool flagSrc)
{
    ForwardingAlgorithmResponse addr;
    double x, y;

    if(m_CBFPB->isDuplicateTimestamp(packet))
    {
        m_CBFPB->erase(packet);
        addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
        addr.setInteger(-1);
        return addr;
    }
    else
    {

        x = diameter(m_paramGPS->data().referencePosition().longitude, packet.destinationArea().geoAreaPosLongitude(), circAtLat((m_paramGPS->data().referencePosition().latitude + packet.destinationArea().geoAreaPosLatitude())/2));
        y = LAT2METER*((m_paramGPS->data().referencePosition().latitude - packet.destinationArea().geoAreaPosLatitude())/10000000.);
        y *= y<0?-1:1;

        double inArea;
        if(shape == shape_circle)
            inArea = f(x, y, packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
        else
            inArea = f(
                    x*cos(packet.destinationArea().angle())-y*sin(packet.destinationArea().angle()),
                    x*sin(packet.destinationArea().angle())+y*cos(packet.destinationArea().angle()),
                    packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);

        if(inArea>=0)
        {
            QMutexLocker locker(m_mutexLocT);
            QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(packet.soPv().gnAddr().toHex());
            int timer_CBF_GBC;
            if(pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai())
            {
                double distance = dist(LocationTableEntry(pos.value()).longPositionVector().latitude(), LocationTableEntry(pos.value()).longPositionVector().longitude(), m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude);
                timer_CBF_GBC = TO_CBF_GBC(distance);
            }
            else
                timer_CBF_GBC = itsGnGeoBroadcastCbfMaxTime;
            locker.unlock();

            if(m_CBFPB->addPacket(bh, ch, packet, packetPayload, timer_CBF_GBC) > 0)
                printf("Succès de l'ajout du packet au cbfpb!\n");
            else
                printf("Echec de l'ajout du packet au cbfpb : DuplicatePacket (Ceci n'est pas censé arrivé)\n");

            addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
            addr.setInteger(0);
            return addr;
        }
        else
        {
            if(flagSrc)
            {
                return greedy(bh, ch, packet, packetPayload, sender, m_gnAddr->mid(), m_gnAddr->mid());
            }
            else
            {
                QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(sender);
                if(pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai())
                {
                    LocationTableEntry lte = LocationTableEntry(pos.value());

                    x = diameter(lte.longPositionVector().longitude(), packet.destinationArea().geoAreaPosLongitude(), circAtLat(lte.longPositionVector().latitude() + packet.destinationArea().geoAreaPosLatitude())/2);
                    y = LAT2METER*((lte.longPositionVector().latitude() - packet.destinationArea().geoAreaPosLatitude())/10000000.);
                    y *= y<0?-1:1;

                    double inAreaExp;
                    if(shape == shape_circle)
                        inAreaExp = f(x, y, packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
                    else
                        inAreaExp = f(
                                x*cos(packet.destinationArea().angle())-y*sin(packet.destinationArea().angle()),
                                x*sin(packet.destinationArea().angle())+y*cos(packet.destinationArea().angle()),
                                packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);

                    if(inAreaExp<0)
                        return greedy(bh, ch, packet, packetPayload, sender, m_gnAddr->mid(), m_gnAddr->mid());
                    else
                    {
                        addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                        addr.setInteger(-1);
                        return addr;
                    }
                }
                else
                {
                    uchar NH[MAC_ADDR_SIZE];
                    for(int i = 0; i < MAC_ADDR_SIZE; i++)
                        NH[i] = 0xFF;
                    addr.setMacAddr(NH);
                    addr.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
                    return addr;
                }
            }
        }
    }
}

ForwardingAlgorithmResponse GeoNet::advancedForwarding(BasicHeader bh, CommonHeader ch, GBCPacket packet, QByteArray packetPayload, e_shape shape, QByteArray sender, uchar* destEth, uchar* forwarderEth, bool flagSrc)
{
    ForwardingAlgorithmResponse addr;

    double x, y;
    x = diameter(m_paramGPS->data().referencePosition().longitude, packet.destinationArea().geoAreaPosLongitude(), circAtLat((m_paramGPS->data().referencePosition().latitude + packet.destinationArea().geoAreaPosLatitude())/2));
    y = LAT2METER*((m_paramGPS->data().referencePosition().latitude - packet.destinationArea().geoAreaPosLatitude())/10000000.);
    y *= y<0?-1:1;

    double xSender, ySender;
    if(flagSrc)
    {
        xSender = x;
        ySender = y;
    }
    else
    {
        xSender = diameter(packet.soPv().longitude(), packet.destinationArea().geoAreaPosLongitude(), circAtLat((packet.soPv().latitude() + packet.destinationArea().geoAreaPosLatitude())/2));
        ySender = LAT2METER*((packet.soPv().latitude() - packet.destinationArea().geoAreaPosLatitude())/10000000.);
        ySender *= ySender<0?-1:1;
    }

    double xFrwrd = 0, yFrwrd = 0;
    if(flagSrc)
    {
        xFrwrd = x;
        yFrwrd = y;
    }
    else
    {
        for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();i != m_locationTable->end(); ++i)
        {
            bool isForwarder = true;
            for(int j = 0; j < ETHER_ADDR_LEN; j++)
                if(i->llAddr(j) != forwarderEth[j])
                    isForwarder = false;
            if(isForwarder)
            {
                xFrwrd = diameter(i->longPositionVector().longitude(), packet.destinationArea().geoAreaPosLongitude(), circAtLat((i->longPositionVector().latitude() + packet.destinationArea().geoAreaPosLatitude())/2));
                yFrwrd = LAT2METER*((i->longPositionVector().longitude() - packet.destinationArea().geoAreaPosLatitude())/10000000.);
                yFrwrd *= yFrwrd<0?-1:1;
            }
        }
    }

    double inout1;
    if(shape == shape_circle)
        inout1 = f(x, y, packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
    else
        inout1 = f(
                x*cos(packet.destinationArea().angle())-y*sin(packet.destinationArea().angle()),
                x*sin(packet.destinationArea().angle())+y*cos(packet.destinationArea().angle()),
                packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);

    QByteArray packetCPT = QByteArray(QByteArray(packet.soPv().gnAddr().toHex(),GnAddr::helper_size)+QByteArray(packet.soPv().timestamp(),32)+QByteArray(packet.sequenceNumber(),16), GnAddr::helper_size + 32 + 16);

    if(inout1 >= 0)
    {
        if(m_CBFPB->isDuplicateTimestamp(packet))
        {
            if(m_CBFPBCounter[packetCPT] >= m_paramConf->maxRepetitionAdvancedContention())
            {
                m_CBFPB->erase(packet);
                m_CBFPBCounter.erase(m_CBFPBCounter.find(packetCPT));
                addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                addr.setInteger(-1);
                return addr;
            }
            else
            {
//                double dist_r = dist(m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, packet.soPv().latitude(), packet.soPv().longitude());
//                double dist_f = dist(frwrdLat, frwrdLong, senderLat, senderLong);
//                double dist_fr = dist(m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, frwrdLat, frwrdLong);
                double dist_r = dist(x, y, xSender, ySender);
                double dist_f = dist(xFrwrd, yFrwrd, xSender, ySender);
                double dist_fr = dist(x, y, xFrwrd, yFrwrd);
                double inout2 = g(dist_r, dist_f, dist_fr, m_angleThreshold);

                if(inout2 >=0)
                {
                    m_CBFPB->erase(packet);
                    m_CBFPBCounter.erase(m_CBFPBCounter.find(packetCPT));
                    addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                    addr.setInteger(-1);
                    return addr;
                }
                else
                {
                    m_CBFPBCounter[packetCPT] += 1;
                    QMutexLocker locker(m_mutexLocT);
                    QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(packet.soPv().gnAddr().toHex());
                    int timer_CBF_GBC;
                    if(pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai())
                    {
                        double distance = dist(LocationTableEntry(pos.value()).longPositionVector().latitude(), LocationTableEntry(pos.value()).longPositionVector().longitude(), m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude);
                        timer_CBF_GBC = TO_CBF_GBC(distance);
                    }
                    else
                        timer_CBF_GBC = itsGnGeoBroadcastCbfMaxTime;
                    locker.unlock();

                    if(m_CBFPB->update(packet, timer_CBF_GBC))
                        printf("\n\nTIMER PROLONGER\n\n");
                    else
                        printf("Echec de l'update du timer du packet dans le cbfpb : PacketNotFound (Ceci n'est pas censé arrivé)\n");

                    addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                    addr.setInteger(0);
                    return addr;
                }
            }
        }
        else
        {
            bool iAmDest = true, destIsBC = true;
            for(int j = 0; j < ETHER_ADDR_LEN; j++)
                if(iAmDest && m_gnAddr->mid(j) != destEth[j])
                    iAmDest = false;
            if(*destEth != 0xff)
                destIsBC = false;


            if(iAmDest || destIsBC)
            {
                m_CBFPBCounter[packetCPT] = 1;

                addr = greedy(bh, ch, packet, packetPayload, sender, destEth, forwarderEth);

                int timer_CBF_GBC;
                timer_CBF_GBC = itsGnGeoBroadcastCbfMaxTime;

                if(m_CBFPB->addPacket(bh, ch, packet, packetPayload, timer_CBF_GBC) > 0)
                    printf("Succès de l'ajout du packet au cbfpb!\n");
                else
                    printf("Echec de l'ajout du packet au cbfpb : DuplicatePacket (Ceci n'est pas censé arrivé)\n");

                addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                addr.setInteger(0);
                return addr;
            }
            else
            {
                QMutexLocker locker(m_mutexLocT);
                int timer_CBF_GBC;
                QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(packet.soPv().gnAddr().toHex());
                if(pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai())
                {
                    double distance = dist(LocationTableEntry(pos.value()).longPositionVector().latitude(), LocationTableEntry(pos.value()).longPositionVector().longitude(), m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude);
                    timer_CBF_GBC = TO_CBF_GBC(distance);
                }
                else
                    timer_CBF_GBC = itsGnGeoBroadcastCbfMaxTime;
                locker.unlock();

                if(m_CBFPB->addPacket(bh, ch, packet, packetPayload, timer_CBF_GBC) > 0)
                    printf("Succès de l'ajout du packet au cbfpb!\n");
                else
                    printf("Echec de l'ajout du packet au cbfpb : DuplicatePacket (Ceci n'est pas censé arrivé)\n");

                addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                addr.setInteger(0);
                return addr;
            }
        }
    }
    else
    {
        QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(packet.soPv().gnAddr().toHex());
        if(flagSrc || (pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai()))
        {
            double inout3;

            if(shape == shape_circle)
                inout3 = f(xSender, ySender, packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
            else
                inout3 = f(
                            x*cos(packet.destinationArea().angle())-y*sin(packet.destinationArea().angle()),
                            x*sin(packet.destinationArea().angle())+y*cos(packet.destinationArea().angle()),
                            packet.destinationArea().distanceA(), packet.destinationArea().distanceB(), shape);
            if(inout3<0)
            {
                return greedy(bh, ch, packet, packetPayload, sender, destEth, forwarderEth);
            }
            else
            {
                addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                addr.setInteger(-1);
                return addr;
             }
        }
        else
        {
            uchar NH[MAC_ADDR_SIZE];
            for(int i = 0; i < MAC_ADDR_SIZE; i++)
                NH[i] = 0xFF;
            addr.setMacAddr(NH);
            addr.setPresent(ForwardingAlgorithmResponse_PR_ForwardingAlgorithmResponseMacAddress);
            return addr;
        }
    }

}

ForwardingAlgorithmResponse GeoNet::CBFUnicast(BasicHeader bh, CommonHeader ch, GACPacket packet, QByteArray packetPayload)
{
    return CBFUnicast(bh, ch, GUCPacket(packet.sequenceNumber(), packet.soPv(), ShortPositionVector(GnAddr(), 0, packet.destinationArea().geoAreaPosLatitude(), packet.destinationArea().geoAreaPosLongitude())), packetPayload);
}

ForwardingAlgorithmResponse GeoNet::CBFUnicast(BasicHeader bh, CommonHeader ch, GUCPacket packet, QByteArray packetPayload)
{
    ForwardingAlgorithmResponse addr;

    QMutexLocker mutexCBFPB(m_CBFPB->m_mutex);
    if(m_CBFPB->isDuplicateTimestamp(packet))
    {
        m_CBFPB->erase(packet);
        qDebug() << "CBFUnicast erase";
        addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
        addr.setInteger(-1);
        return addr;
    }
    else
    {
        QMutexLocker locker(m_mutexLocT);
        QHash<QByteArray, LocationTableEntry>::Iterator pos = m_locationTable->find(packet.soPv().gnAddr().toHex());
        locker.unlock();
        if(pos != m_locationTable->end() && LocationTableEntry(pos.value()).longPositionVector().pai())
        {
            double progress = dist(packet.dePv().latitude(), packet.dePv().longitude(), packet.soPv().latitude(), packet.soPv().longitude()) - dist(packet.dePv().latitude(), packet.dePv().longitude(), m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude);
            if(progress > 0)
            {
                int timer_CBF_GUC = TO_CBF_GUC(progress);

                if(m_CBFPB->addPacket(bh, ch, packet, packetPayload, timer_CBF_GUC) > 0)
                    printf("Succès de l'ajout du packet au cbfpb!\n");
                else
                    printf("Echec de l'ajout du packet au cbfpb : DuplicatePacket (Ceci n'est pas censé arrivé)\n");

                addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                addr.setInteger(0);
                return addr;
            }
            else
            {
                addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
                addr.setInteger(-1);
                return addr;
            }
        }
        else
        {
            int timer_CBF_GUC = itsGnGeoBroadcastCbfMaxTime;
            if(m_CBFPB->addPacket(bh, ch, packet, packetPayload, timer_CBF_GUC) > 0)
                printf("Succès de l'ajout du packet au cbfpb!\n");
            else
                printf("Echec de l'ajout du packet au cbfpb : DuplicatePacket (Ceci n'est pas censé arrivé)\n");

            addr.setPresent(ForwardingAlgorithmResponse_PR_Integer);
            addr.setInteger(0);
            return addr;
        }
    }
}

/* Packet Buffer Management */

void GeoNet::flushPB(GnAddr destination)
{
    flushLSPB(destination);
    flushUCFPB(destination);
    checkBCFPB();
}

void GeoNet::flushLSPB(GnAddr destination)
{
    foreach(LocationServicePacketEntry i, m_LSPB->lsPacketBuffer())
    {
        if (i.destination() == destination)
        {
            //bool ltIsOver = false;
            /*if(i.packet().maximumPacketLifeTime != 0 && (nowTimestamp - i.timestamp() > i.packet().maximumPacketLifeTime->lifeTime() || ((i.packet().maximumPacketLifeTime->lifeTime() - (m_paramGPS->timestamp() % 0x100000000) - i.timestamp())/50) < 1))
                ltIsOver = true;*/

            gnDataRequest_t tmp = i.packet();
            quint32 tmpTimestamp = i.timestamp();
            m_LSPB->remove(i);
            m_LSPB->nbLSForwardingPacketQueued--;

            quint32 nowTimestamp = (m_paramGPS->timestamp() % 0x100000000);

            //i.packet().maximumPacketLifeTime == 0;
            //||

            //quint32 LTTEST = i.packet().maximumPacketLifeTime->lifeTime();
            if(i.packet().maximumPacketLifeTime == 0 || (nowTimestamp - tmpTimestamp < tmp.maximumPacketLifeTime->lifeTime() && (((tmp.maximumPacketLifeTime->lifeTime() - nowTimestamp - tmpTimestamp)/50) > 0)))
            //if(!ltIsOver)
            {
                tmp.maximumPacketLifeTime->setMultiplierBase(tmp.maximumPacketLifeTime->lifeTime() - nowTimestamp - tmpTimestamp);
                emit sendGUC(tmp);
            }
        }
        //else ++i;
    }
}

void GeoNet::checkUCFPB()
{
    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;
    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);

    QMutexLocker locker(m_mutex);
    for(QVector<UnicastForwardingPacketEntry>::Iterator i = m_UCFPB->ucForwardPacketBuffer().begin();i != m_UCFPB->ucForwardPacketBuffer().end(); ++i)
    {
        UnicastForwardingPacketEntry tmp(*i);
        tmp.updateSourcePacketSoPv(lpv);

        quint64 lt;
        if(tmp.basicHeader().lifeTime().lifeTime() - tmp.m_elapsed.elapsed() > 0)
        {
            lt = tmp.basicHeader().lifeTime().lifeTime() - tmp.m_elapsed.elapsed();
            if(lt % 50 != 0)
                lt = (lt/50)*50;
        }
        else
        {
            printf("LifeTime outdated\n");
            m_UCFPB->erase(tmp.gucPacket());
            return;
        }


        tmp.basicHeader().setLifeTime(LifeTime(lt));

        QByteArray packet;
        QDataStream ds(&packet, QIODevice::WriteOnly);
        ds.writeRawData(tmp.basicHeader().toHex().data(), tmp.basicHeader().toHex().length());
        ds.writeRawData(tmp.commonHeader().toHex().data(), tmp.commonHeader().toHex().length());

        ForwardingAlgorithmResponse res;
        switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
        {
        default:
        case itsGnGeoUnicastForwardingAlgorithm_unspecified:
        case itsGnGeoUnicastForwardingAlgorithm_greedy:
            res = greedy(tmp.basicHeader(), tmp.commonHeader(), tmp.gucPacket(), tmp.payload(), tmp.sender());
            break;
        case itsGnGeoUnicastForwardingAlgorithm_CBF:
            res = CBFUnicast(tmp.basicHeader(), tmp.commonHeader(), tmp.gucPacket(), tmp.payload());
            break;
        }

        if(res.isInteger() && res.integer() == 0)
        {
            printf("geonetError_checkUC_guc_bufferedPacket\n");
            return;
        }
        else if(res.isInteger() && res.integer() == -1)
        {
            printf("geonetError_checkUC_guc_discardedPacket\n");
            return;
        }

        ds.writeRawData(tmp.gucPacket().toHex().data(), tmp.gucPacket().toHex().length());
        ds.writeRawData(tmp.payload().data(), tmp.payload().length());

        if(res.isMacAddr())
            for(int k = 0 ; k < 6 ; k++)
                m_paramNetwork->eh.ether_dhost[k] = res.macAddr()[k];

        /* Construct the Ethernet header : my mac addr */
        for(int k = 0 ; k < 6 ; k++)
            m_paramNetwork->eh.ether_shost[k] = m_paramConf->mid(k);

        size_t tx_len = sizeof(struct ether_header);

        memcpy(m_paramNetwork->sendbuf, &m_paramNetwork->eh, sizeof(struct ether_header));
        for(int k = 0 ; k < packet.length(); k++)
            m_paramNetwork->sendbuf[tx_len++] = packet.data()[k];

        /* Send packet */
        if(sendto(m_paramNetwork->rawSocket, m_paramNetwork->sendbuf, tx_len, 0, (struct sockaddr*)(&(m_paramNetwork->socket_address)), sizeof(struct sockaddr_ll)) < 0)
            qDebug() << "sendto" << strerror(errno);
        else
            m_UCFPB->erase(tmp.gucPacket());
    }
}
void GeoNet::flushUCFPB(GnAddr destination)
{
    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;
    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);

    QMutexLocker locker(m_mutex);
    for(int priority = 0; priority < 4; priority++)
    {
        for(QVector<UnicastForwardingPacketEntry>::Iterator i = m_UCFPB->ucForwardPacketBuffer().begin();i != m_UCFPB->ucForwardPacketBuffer().end(); ++i)
        {
            if(i->gucPacket().dePv().gnAddr() == destination && i->commonHeader().trafficClass().TCID() == priority)
            {
                i->updateSourcePacketSoPv(lpv);
                quint64 lt = i->basicHeader().lifeTime().lifeTime() - i->m_elapsed.elapsed();
                if(lt % 50 != 0)
                    lt = (lt/50)*50;
                i->basicHeader().setLifeTime(LifeTime(lt));

                QByteArray packet;
                QDataStream ds(&packet, QIODevice::WriteOnly);
                ds.writeRawData(i->basicHeader().toHex().data(), i->basicHeader().toHex().length());
                ds.writeRawData(i->commonHeader().toHex().data(), i->commonHeader().toHex().length());

                ForwardingAlgorithmResponse res;
                switch(m_paramConf->itsGnGeoUnicastForwardingAlgorithm())
                {
                default:
                case itsGnGeoUnicastForwardingAlgorithm_unspecified:
                case itsGnGeoUnicastForwardingAlgorithm_greedy:
                    res = greedy(i->basicHeader(), i->commonHeader(), i->gucPacket(), i->payload(), i->sender());
                    break;
                case itsGnGeoUnicastForwardingAlgorithm_CBF:
                    res = CBFUnicast(i->basicHeader(), i->commonHeader(), i->gucPacket(), i->payload());
                    break;
                }

                if(res.isInteger() && res.integer() == 0)
                {
                    printf("geonetError_flushUC_guc_bufferedPacket\n");
                    return;
                }
                else if(res.isInteger() && res.integer() == -1)
                {
                    printf("geonetError_flushUC_guc_discardedPacket\n");
                    return;
                }

                ds.writeRawData(i->gucPacket().toHex().data(), i->gucPacket().toHex().length());
                ds.writeRawData(i->payload().data(), i->payload().length());

                if(res.isMacAddr())
                    for(int k = 0 ; k < 6 ; k++)
                        m_paramNetwork->eh.ether_dhost[k] = res.macAddr()[k];

                /* Construct the Ethernet header : my mac addr */
                for(int k = 0 ; k < 6 ; k++)
                    m_paramNetwork->eh.ether_shost[k] = m_paramConf->mid(k);

                size_t tx_len = sizeof(struct ether_header);

                memcpy(m_paramNetwork->sendbuf, &m_paramNetwork->eh, sizeof(struct ether_header));
                for(int k = 0 ; k < packet.length(); k++)
                    m_paramNetwork->sendbuf[tx_len++] = packet.data()[k];

                /* Send packet */
                if(sendto(m_paramNetwork->rawSocket, m_paramNetwork->sendbuf, tx_len, 0, (struct sockaddr*)(&(m_paramNetwork->socket_address)), sizeof(struct sockaddr_ll)) < 0)
                    qDebug() << "sendto" << strerror(errno);
                else
                    m_UCFPB->erase(i->gucPacket());
            }
        }
    }
}

void GeoNet::checkBCFPB()
{
    printf("\n\nCHECK BCFPB\n\n");
    quint32 tmpstmp = m_paramGPS->timestamp() % 0x100000000;
    quint8 pai = m_paramGPS->data().referencePosition().positionConfidenceEllipse.semiMajorConfidence<itsGnPaiInterval/2?true << 15:false << 15;
    LongPositionVector lpv(*m_gnAddr, tmpstmp , m_paramGPS->data().referencePosition().latitude, m_paramGPS->data().referencePosition().longitude, pai, m_paramGPS->data().speed().speedValue, m_paramGPS->data().heading().headingValue);

    int j = 0;
    for(QVector<BroadcastForwardingPacketEntry>::Iterator i = m_BCFPB->bcForwardPacketBuffer().begin(); j < m_BCFPB->bcForwardPacketBuffer().size(); ++i, j++)
    {
        QMutexLocker locker(m_mutex);
        i->updateSourcePacketSoPv(lpv);
        if(i->basicHeader().lifeTime().lifeTime() - i->m_elapsed.elapsed() < 0)
        {
            m_BCFPB->bcForwardPacketBuffer().erase(i);
            continue;
        }
/*            printf("\n"
                   " ------------------------------\n"
                   "|                              |\n"
                   "|                              |\n"
                   "|                              |\n"
                   "|    BCFPB    LT    IS    OVER |\n"
                   "|                              |\n"
                   "|                              |\n"
                   "|                              |\n"
                   " ------------------------------\n");
        else
            printf("\n"
                   " ------------------------------\n"
                   "|                              |\n"
                   "|                              |\n"
                   "|                              |\n"
                   "|    %lld |\n"
                   "|                              |\n"
                   "|                              |\n"
                   "|                              |\n"
                   " ------------------------------\n", i->basicHeader().lifeTime().lifeTime() - i->m_elapsed.elapsed());
*/
        quint64 lt = i->basicHeader().lifeTime().lifeTime() - i->m_elapsed.elapsed();
        if(lt % 50 != 0)
            lt = (lt/50)*50;
        i->basicHeader().setLifeTime(LifeTime(lt));

        switch(i->commonHeader().headerType())
        {
        case CommonHeader::headerType_geoAnycast:
        case CommonHeader::headerType_geoBroadcast:
        {
            printf("\n\nCHECK BCFPB\n\n");
            DestinationArea da;
            GBCPacket geoPacket;
            if(i->commonHeader().headerType() == CommonHeader::headerType_geoAnycast)
                geoPacket = i->gacPacket();
            else if(i->commonHeader().headerType() == CommonHeader::headerType_geoBroadcast)
                geoPacket = i->gbcPacket();
            da = geoPacket.destinationArea();

            ForwardingAlgorithmResponse res;
            e_shape shape;
            switch(i->commonHeader().headerSubType())
            {
            case CommonHeader::headerSubTypeGeoBroadcast_circle:
                shape = shape_circle;
                if((da.distanceA() * da.distanceA() * M_PI) > ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
                {
                    printf("GBC Sending Error : Area size is too large.\n");
                    return;
                }
                break;
            case CommonHeader::headerSubTypeGeoBroadcast_rectangle:
                shape = shape_rectangle;
                if((da.distanceA() * da.distanceB()) > ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
                {
                    printf("GBC Sending Error : Area size is too large.\n");
                    return;
                }
                break;
            case CommonHeader::headerSubTypeGeoBroadcast_ellipse:
                shape = shape_ellipse;
                if((da.distanceA() * da.distanceB() * M_PI) > ((itsGnMaxGeoAreaSize * 1000)*(itsGnMaxGeoAreaSize * 1000)))
                {
                    printf("GBC Sending Error : Area size is too large.\n");
                    return;
                }
                break;
            default :
                shape = shape_none;
                break;
            }

            bool iamsender = (i->sender() == QByteArray());
            switch(m_paramConf->itsGnGeoBroadcastForwardingAlgorithm())
            {
            default:
            case itsGnGeoBroadcastForwardingAlgorithm_unspecified:
            case itsGnGeoBroadcastForwardingAlgorithm_simple:
                if(iamsender)
                    res = simpleForwardingAlgorithm(i->basicHeader(), i->commonHeader(), geoPacket, i->payload(), shape, QByteArray(), true);
                else
                    res = simpleForwardingAlgorithm(i->basicHeader(), i->commonHeader(), geoPacket, i->payload(), shape, i->sender(), false);
                break;
            case itsGnGeoBroadcastForwardingAlgorithm_CBF:
                if(iamsender)
                    res = contentionBasedForwardingAlgorithm(i->basicHeader(), i->commonHeader(), geoPacket, i->payload(), shape, QByteArray(), true);
                else
                    res = contentionBasedForwardingAlgorithm(i->basicHeader(), i->commonHeader(), geoPacket, i->payload(), shape, i->sender(), true);
                break;
            case itsGnGeoBroadcastForwardingAlgorithm_advanced:
                if(iamsender)
                    res = advancedForwarding(i->basicHeader(), i->commonHeader(), geoPacket, i->payload(), shape, QByteArray(), i->destination(), i->forwarder(), true);
                else
                    res = advancedForwarding(i->basicHeader(), i->commonHeader(), geoPacket, i->payload(), shape, i->sender(), i->destination(), i->forwarder(), true);
                break;
            }

            if(res.isInteger() && res.integer() == 0)
            {
                printf("checkBCFPB packet buffered GBC/GAC\n");
                return;
            }
            //m_BCFPB->addPacket(i->basicHeader(), i->commonHeader(), (i->commonHeader().headerType() == CommonHeader::headerType_geoAnycast)?i->gacPacket():i->gbcPacket(), i->payload(), i->sender());

            if(res.isMacAddr())
                for(int k = 0 ; k < 6 ; k++)
                    m_paramNetwork->eh.ether_dhost[k] = res.macAddr()[k];

            /* Construct the packet */
            QByteArray packet;
            QDataStream ds(&packet, QIODevice::WriteOnly);

            ds.writeRawData(i->commonHeader().toHex().data(), i->commonHeader().toHex().length());
            ds.writeRawData(geoPacket.toHex().data(), geoPacket.toHex().length());
            ds.writeRawData(i->payload().data(), i->payload().length());

            send(packet, true, i->basicHeader());
        }

            break;

        case CommonHeader::headerType_topologicallyScopedBroadcast:
        {
            bool noNeighbour = true;
            if(!m_locationTable->isEmpty())
                for(QHash<QByteArray, LocationTableEntry>::Iterator i = m_locationTable->begin();i != m_locationTable->end(); ++i)
                    if(LocationTableEntry(i.value()).isNeighbour() != 0 && *LocationTableEntry(i.value()).isNeighbour())
                        noNeighbour = false;

            if(noNeighbour && i->commonHeader().trafficClass().SCF() == 1)
            {
                locker.unlock();
                if(i->commonHeader().headerSubType()==CommonHeader::headerSubTypeTopologicallyScopedBroadcast_singleHop)
                    m_BCFPB->addPacket(i->basicHeader(), i->commonHeader(), i->shbPacket(), i->payload(), i->sender());
                else
                    m_BCFPB->addPacket(i->basicHeader(), i->commonHeader(), i->tsbPacket(), i->payload(), i->sender());
                locker.relock();
            }
            else
            {
                /* Construct the packet */
                QByteArray shbData;
                QDataStream ds(&shbData, QIODevice::WriteOnly);

                ds.writeRawData(i->commonHeader().toHex().data(), i->commonHeader().toHex().length());
                if(i->commonHeader().headerSubType()==CommonHeader::headerSubTypeTopologicallyScopedBroadcast_singleHop)
                    ds.writeRawData(i->shbPacket().toHex().data(), i->shbPacket().toHex().length());
                else
                    ds.writeRawData(i->tsbPacket().toHex().data(), i->tsbPacket().toHex().length());
                ds.writeRawData(i->payload().data(), i->payload().length());

                /* Construct the Ethernet header : destination addr */
                for(int i = 0 ; i < 6 ; i++)
                    m_paramNetwork->eh.ether_dhost[i] = 0xFF;

                /*int ret = */
                /*if(*/send(shbData, true, i->basicHeader());/* == networkError_success)
                    i = m_BCFPB->bcForwardPacketBuffer().erase(i);*/
                /*
                if(ret == networkError_success)
                {
                    if(i->commonHeader().headerSubType()==CommonHeader::headerSubTypeTopologicallyScopedBroadcast_singleHop)
                        m_HPB->addPacket(i->basicHeader(), i->commonHeader(), i->shbPacket(), i->payload());
                    else
                        m_HPB->addPacket(i->basicHeader(), i->commonHeader(), i->tsbPacket(), i->payload())
                }*/
            }
            break;
        }
        }
    }
    //m_BCFPB->clear();
}

/* Security layer */
QByteArray GeoNet::getSecuredPayload(QByteArray payload, BasicHeader bh)
{
    /*QNetworkDatagram response;

    //qDebug() << "sending" << payload.toHex() << "to security server";
    m_signatureSocket->writeDatagram(payload, QHostAddress(m_paramSecu->serverAddress()), m_paramSecu->signaturePort());

    if(m_signatureSocket->waitForReadyRead(m_paramSecu->timeout() * 1000))
    {
        response = m_signatureSocket->receiveDatagram();
        //qDebug() << response.data().toHex() << "received from security server";
        return bh.toHex() + response.data();
    }

    qDebug() << "no response from security server";
    bh.setNextHeader(BasicHeader::nextHeader_commonHeader);
    return bh.toHex() + payload;*/
#include <SecurityProfile_CAM.h>
#include <decodeSecurity.h>

    Certificate_t certif;
    decode_Certificate((unsigned char*)m_securityManager.certificate().data(), &certif);
    QByteArray signedPayload = m_securityManager.signPayload(payload, SecurityProfile::securityProfile_cam);

    return bh.toHex() + signedPayload;
}

QByteArray GeoNet::verifySecuredPayload(QByteArray payload, bool *success)
{
    QNetworkDatagram response;

    m_verifySocket->writeDatagram(payload, QHostAddress(m_paramSecu->serverAddress()), m_paramSecu->verifyPort());

    if(m_verifySocket->waitForReadyRead(m_paramSecu->timeout()))
    {
        response = m_verifySocket->receiveDatagram();
        qDebug() << response.data().toHex() << "received from verify";
        *success = (bool)response.data()[0];
        qDebug() << (int)response.data()[0] << *success;
        if(*success)
        {
            qDebug() << response.data().right(response.data().length() - 1).toHex();
            return response.data().right(response.data().length() - 1);
        }
    }
    *success = false;

    return payload;
}