#include "gresb.h"
#include <QTime>
#include <QHostAddress>

gresb::gresb(QWidget *parent) :
    QWidget(parent)
{
    RMAPSend_SOCKET = new QTcpSocket;
    RMAPReceive_SOCKET = new QTcpSocket;
    GRESBStatusQuery_SOCKET = new QTcpSocket;

    rmapPacketSEMAPHORE = new QSemaphore;
    ccsdsPacketSEMAPHORE = new QSemaphore;
    rmapPacket = (char*) malloc(RMAP_MAX_PACKET_LENGTH);
    ccsdsPacket = (unsigned char*) malloc(CCSDS_MAX_PACKET_LENGTH);
    spwPacket = (char*) malloc( qMax(RMAP_MAX_PACKET_LENGTH, CCSDS_MAX_PACKET_LENGTH) );

    commandCode = invalid0; // initialization of the command code for the RMAP transfers
    rmapTargetLogicalAddress = DEFAULT_TARGET;
    rmapSourceLogicalAddress = DEFAULT_SOURCE;

    //*** QLABEL ***//
    gresbBridgeIPLabel = new QLabel(tr("GRESB Bridge IP: "));
    gresbVirtualLinkLabel = new QLabel(tr("GRESB Virtual Link: "));
    spwLinkLabel = new QLabel(tr("GRESB SPW Link: "));
    rmapSendStateLabel = new QLabel(tr("RMAP Send Socket State: waiting for connection"));
    rmapReceiveStateLabel = new QLabel(tr("RMAP Receive Socket State: waiting for connection"));
    gresbStatusQueryLabel = new QLabel(tr("GRESB status query socket (port 3010): waiting for connection"));
    gresbStatusQueryDialogLabel = new QLabel(tr("sockets opened but SpaceWire link not running"));

    //*** SPINBOX ***//
    gresbVirtualLinkSpinBox = new QSpinBox;
    spwLinkSpinBox = new QSpinBox;;
    gresbVirtualLinkSpinBox->setRange(0, 4);
    gresbVirtualLinkSpinBox->setValue(1);
    spwLinkSpinBox->setRange(0, 2);
    spwLinkSpinBox->setValue(0);

    //*** QPUSHBUTTON ***//
    gresbStatusQueryRetryButton = new QPushButton(tr("Retry"));
    gresbStatusQueryAbortButton = new QPushButton(tr("Abort"));

    //*** LAYOUT ***//
    connectionLayout = new QGridLayout;

    //*** MISC ***//
    gresbStatusQueryDialog = new QDialog;
    gresbBridgeIPDialogBox = new QIPDialogBox;
    spwLinkStatusEnquiry = new gresbStatusEnquiry;

    readSettings(); // get the stored settings for the IP address

    connectionLayout->addWidget(gresbBridgeIPLabel, 0, 0, 0);
    connectionLayout->addWidget(gresbBridgeIPDialogBox, 0, 1, 0);
    connectionLayout->addWidget(gresbVirtualLinkLabel, 1, 0, 0);
    connectionLayout->addWidget(gresbVirtualLinkSpinBox, 1, 1, 0);
    connectionLayout->addWidget(spwLinkLabel, 2, 0, 0);
    connectionLayout->addWidget(spwLinkSpinBox, 2, 1, 0);
    connectionLayout->addWidget(rmapSendStateLabel, 3, 0, 1, 2);
    connectionLayout->addWidget(rmapReceiveStateLabel, 4, 0, 1, 2);
    connectionLayout->addWidget(gresbStatusQueryLabel, 5, 0, 1, 2);

    connectionLayout->setRowStretch(6, 1);
    connectionLayout->setColumnStretch(2, 1);

    // GRESB STATUS QUERY DIALOG
    gresbStatusQueryDialogLayout = new QGridLayout;
    gresbStatusQueryDialogLayout->addWidget(gresbStatusQueryDialogLabel, 0, 0, 1, 2);
    gresbStatusQueryDialogLayout->addWidget(gresbStatusQueryRetryButton, 1, 0, 0);
    gresbStatusQueryDialogLayout->addWidget(gresbStatusQueryAbortButton, 1, 1, 0);
    gresbStatusQueryDialog->setLayout(gresbStatusQueryDialogLayout);

    this->setLayout(connectionLayout);

    connect(RMAPSend_SOCKET, SIGNAL(stateChanged(QAbstractSocket::SocketState)), this, SLOT(RMAPSendConnectionState(QAbstractSocket::SocketState)));
    connect(RMAPReceive_SOCKET, SIGNAL(stateChanged(QAbstractSocket::SocketState)), this, SLOT(RMAPReceiveConnectionState(QAbstractSocket::SocketState)));
    connect(GRESBStatusQuery_SOCKET, SIGNAL(stateChanged(QAbstractSocket::SocketState)), this, SLOT(GRESBConnectionState(QAbstractSocket::SocketState)));
    connect(gresbStatusQueryRetryButton, SIGNAL(clicked()), this, SLOT(reTestSPWLink()));
    connect(gresbStatusQueryAbortButton, SIGNAL(clicked()), gresbStatusQueryDialog, SLOT(reject()));
    connect(spwLinkStatusEnquiry->readSPWStatusButton, SIGNAL(clicked()), this, SLOT(GRESBStatusQuery()));
    connect(this->RMAPReceive_SOCKET, SIGNAL(readyRead()), this, SLOT(receiveSPWPacketLoop()));
}

gresb::~gresb()
{
    free(rmapPacket);
    free(ccsdsPacket);
    free(spwPacket);

    writeSettings();
}

unsigned int gresb::Write(unsigned int *Value, unsigned int count, unsigned int address)
{
    unsigned int remainingCount = count;
    unsigned int iOffset = 0;
    QString console_message;
    char* data;

    if(rmapPacketSEMAPHORE->available()!=0)
    {
        emit appendToLog("WARNING === in function WRITE of rmapplugin *** RMAP request already running, WRITE access stopped");
        return 1;
    }

    RMAP_write_reply_setText("reply to the write command required\nlast reply status: unavailable");

    data = (char*) malloc(READ_WRITE_MAX_COUNTS*4);

    emit appendToLog(QString("*** START *** WRITE ")+ QString::number(count) + QString(" word(s) @0x")+ QString::number(address,16));

    while (remainingCount > READ_WRITE_MAX_COUNTS)
    {
        for (int i = 0; i<READ_WRITE_MAX_COUNTS; i++)
        {
            data[i*4+3] = (char) ((unsigned int) Value[i+iOffset]);
            data[i*4+2] = (char) ((unsigned int) Value[i+iOffset]>>8);
            data[i*4+1] = (char) ((unsigned int) Value[i+iOffset]>>16);
            data[i*4+0] = (char) ((unsigned int) Value[i+iOffset]>>24);
        }

        console_message.sprintf("remainingCount: %d => ", remainingCount);
        emit appendToLog(console_message + QString("Write ")+ QString::number(READ_WRITE_MAX_COUNTS*4) + QString(" byte(s) @0x")+ QString::number(address,16));

        if(WriteBLOCK(data, READ_WRITE_MAX_COUNTS*4, address)==0)
        {
            emit appendToLog("WARNING === in function WRITE of rmapplugin *** RMAP write command failed");
            return 1;
        }

        remainingCount = remainingCount - READ_WRITE_MAX_COUNTS;
        address = address + READ_WRITE_MAX_COUNTS * 4;
        iOffset = iOffset + READ_WRITE_MAX_COUNTS;
    }

    if (remainingCount > 0)
    {
        for (unsigned int i = 0; i<remainingCount; i++)
        {
            data[i*4+3] = (char) ((unsigned int) Value[i+iOffset]);
            data[i*4+2] = (char) ((unsigned int) Value[i+iOffset]>>8);
            data[i*4+1] = (char) ((unsigned int) Value[i+iOffset]>>16);
            data[i*4+0] = (char) ((unsigned int) Value[i+iOffset]>>24);
        }

        console_message.sprintf("remainingCount: %d => ", remainingCount);
        emit appendToLog(console_message + QString("Write ")+ QString::number(remainingCount*4) + QString(" byte(s) @0x")+ QString::number(address,16));

        if (WriteBLOCK(data, remainingCount*4, address)==0)
        {
            emit appendToLog("WARNING === in function WRITE of rmapplugin *** RMAP write command failed");
            return 1;
        }
    }

    emit appendToLog(QString("*** STOP *** WRITE"));
    free(data);
    return count;
}

unsigned int gresb::Read(unsigned int *Value, unsigned int count, unsigned int address)
{
    unsigned int remainingCount = count;
    unsigned int iOffset = 0;
    QString console_message;

    if(rmapPacketSEMAPHORE->available()!=0)
    {
        emit appendToLog("WARNING === in function READ of rmapplugin *** RMAP request already running, READ access stopped");
        return 1;
    }
    emit appendToLog(QString("*** START *** READ ")+ QString::number(count) + QString(" word(s) @0x")+ QString::number(address,16));

    while (remainingCount > READ_WRITE_MAX_COUNTS)
    {
        console_message.sprintf("remainingCount: %d => ", remainingCount);
        emit appendToLog(console_message + QString("Read ")+ QString::number(4*READ_WRITE_MAX_COUNTS) + QString(" byte(s) @0x")+ QString::number(address,16));

        if (ReadBLOCK(READ_WRITE_MAX_COUNTS*4, address)==0)
        {
            emit appendToLog("WARNING === in function READ of rmapplugin *** RMAP packet not received");
            return 1;
        }

        for(int i=0;i<READ_WRITE_MAX_COUNTS;i++)
        {
            Value[i+iOffset] = (unsigned char) rmapPacket[i*4+RMAP_READ_REPLY_HEADER_LENGTH];
            for(int j=1;j<4;j++)
            {
                Value[i+iOffset]= ((unsigned char)(rmapPacket[i*4+j+RMAP_READ_REPLY_HEADER_LENGTH])) + Value[i+iOffset]*256;
            }
        }

        remainingCount = remainingCount - READ_WRITE_MAX_COUNTS;
        address = address + READ_WRITE_MAX_COUNTS * 4;
        iOffset = iOffset + READ_WRITE_MAX_COUNTS;
        rmapPacketSEMAPHORE->acquire();
    }

    if (remainingCount > 0)
    {
        console_message.sprintf("remainingCount: %d => ", remainingCount);
        emit appendToLog(console_message + QString("Read ")+ QString::number(4*remainingCount) + QString(" byte(s) @0x")+ QString::number(address,16));

        if (ReadBLOCK(4*remainingCount, address)==0)
        {
            emit appendToLog("WARNING === in function READ of rmapplugin *** RMAP packet not received");
            return 1;
        }

        for(unsigned int i=0;i<remainingCount;i++)
        {
            Value[i+iOffset] = (unsigned char) rmapPacket[i*4+RMAP_READ_REPLY_HEADER_LENGTH];
            for(int j=1;j<4;j++)
            {
                Value[i+iOffset]= ((unsigned char)(rmapPacket[i*4+j+RMAP_READ_REPLY_HEADER_LENGTH])) + Value[i+iOffset]*256;
            }
        }
        rmapPacketSEMAPHORE->acquire();
    }

    emit appendToLog(QString("*** STOP *** READ "));
    return count;
}

unsigned int gresb::WriteBLOCK(char *data, unsigned int nbBytes, unsigned int address)
{
    QTime RMAPTimeout;
    RMAP *RMAPCommand;
    int errorCode;
    QString console_message;

    if (GRESBStatusQueryRequest(LinkStatus, spwLinkSpinBox->value()) == 1)
    {
        this->Close();
        return 1;
    }

    RMAPCommand = new RMAP(commandCode,
                           rmapTargetLogicalAddress,
                           rmapSourceLogicalAddress,
                           address,
                           nbBytes,
                           data);

    // SEND GRESB HEADER
    RMAPSend_SOCKET->write((char*) ((void*) &RMAPCommand->GRESBHeader), 4);
    // SEND SPACEWIRE PACKET HEADER
    RMAPSend_SOCKET->write((char*) ((void*) &RMAPCommand->RMAPHeader), sizeof(RMAPCommand->RMAPHeader));
    // SEND DATA
    RMAPSend_SOCKET->write( data, nbBytes);
    // SEND DATA CRC
    RMAPSend_SOCKET->write((char*) ((void*) &RMAPCommand->dataCRC), 1);
    RMAPTimeout.start();
    while(RMAPSend_SOCKET->bytesToWrite() > 0)
    {
        RMAPSend_SOCKET->waitForBytesWritten(100);
        if(RMAPTimeout.elapsed()>1000)
        {
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** sending Write RMAP Command timeout");
            return 0;
        }
    }

    if ( (commandCode == writeSingle_noVer_Rep) | (commandCode == writeInc_noVer_Rep) |
         (commandCode == writeSingle_ver_rep) | (commandCode == writeInc_ver_rep) )
    {
        // WAIT FOR THE RMAP REPLY PACKET
        errorCode = receiveSPWPacket(1);
        if (errorCode<=0)
        {
            emit appendToLog("WARNING === in function WriteBLOCK of rmapplugin *** RMAP packet reception failed with code " + QString::number(errorCode));
            return 0;
        }
        if(rmapPacketSize != 8)
         {
            console_message.sprintf("WARNING === in function WRITE (with reply) of rmapplugin *** write reply format not compliant\n");
            emit appendToLog(console_message);
            return 0;
         }
        switch (rmapPacket[3]) // byte 4 is the status byte in the reply
        {
        case 0:
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 0 Successfull");
            break;
        case 1:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** General error code");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 1 General error code");
            break;
        case 2:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Unused RMAP packet type or command code");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 2 Unused RMAP packet type or command code");
            break;
        case 3:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Invalid key");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 3 Invalid key");
            break;
        case 4:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Invalid data CRC");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 4 Invalid data CRC");
            break;
        case 5:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Early EOP");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 5 Early EOP");
            break;
        case 6:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Too much data");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 6 Too much data");
            break;
        case 7:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** EEP");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 7 EEP");
            break;
        case 8:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Reserved");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 8 Reserved");
            break;
        case 9:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Verify buffer overrun");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 9 Verify buffer overrun");
            break;
        case 10:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** RMAP command not implemented or not authorised");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 10 RMAP command not implemented or not authorised");
            break;
        case 11:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** RMW data length error");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 11 RMAP RMW data length error");
            break;
        case 12:
            emit appendToLog("WARNING === in function WRITE (with reply) of rmapplugin *** Invalid target logical address");
            RMAP_write_reply_setText("reply to the write command required\nlast reply status: 12 Invalid target logical address");
            break;
        }
        acquireRMAPSemaphore();
    }
    return nbBytes;
}

unsigned int gresb::ReadBLOCK(unsigned int nbBytes, unsigned int address)
{
    int errorCode;
    RMAP *RMAPCommand;
    QTime RMAPTimeout;
    unsigned int dataLength;
    if (GRESBStatusQueryRequest(LinkStatus, spwLinkSpinBox->value()) == 1)
    {
        this->Close();
        return 1;
    }

    if (nbBytes > 4)
    {
        RMAPCommand = new RMAP(read_Inc,
                               rmapTargetLogicalAddress,
                               rmapSourceLogicalAddress,
                               address,
                               nbBytes,
                               NULL);
    }
    else
    {
        RMAPCommand = new RMAP(read_Single,
                               rmapTargetLogicalAddress,
                               rmapSourceLogicalAddress,
                               address,
                               nbBytes,
                               NULL);
    }

    // SEND THE GRESB HEADER FOR THE RMAP READ COMMAND
    RMAPSend_SOCKET->write((char*) ((void*) &RMAPCommand->GRESBHeader), 4);
    // SEND THE SPACEWIRE PACKET FOR THE RMAP READ COMMAND
    RMAPSend_SOCKET->write((char*) ((void*) &RMAPCommand->RMAPHeader), sizeof(RMAPCommand->RMAPHeader));
    RMAPSend_SOCKET->waitForBytesWritten(100);
    RMAPTimeout.start();
    // write timeout
    while(RMAPSend_SOCKET->bytesToWrite() > 0)
    {
        RMAPSend_SOCKET->waitForBytesWritten(100);
        if(RMAPTimeout.elapsed()>1000)
        {
            emit appendToLog("WARNING === in function READ of rmapplugin *** sending Read RMAP Command timeout\n");
            return 0;
        }
    }

    // RECEIVE THE INCOMING RMAP PACKET
    errorCode = receiveSPWPacket(1); // request ID 1 is for RMAP packet
    if (errorCode<=0)
    {
        emit appendToLog("WARNING === in function ReadBLOCK of rmapplugin *** RMAP packet reception failed with code " + QString::number(errorCode));
        return 0;
    }
    dataLength = rmapPacketSize - RMAP_READ_REPLY_HEADER_LENGTH - RMAP_DATA_CRC_LENGTH;
    if(dataLength != nbBytes)
     {
        emit appendToLog("WARNING === in function READ of rmapplugin *** number of data received ("
                    +QString::number(dataLength)
                    +") not equal to number of data requested ("
                    +QString::number(nbBytes)
                    +")");
        return 0;
     }
    return dataLength;
}

unsigned int gresb::WriteSPW(char *Value, unsigned int count, char targetLogicalAddress, char userApplication)
{
    char protocoleIdentifier = 0x02;
    char reserved = 0x00;
    char gresbProtocole = 0x00;
    unsigned char size[3];
    unsigned int spwPacketSize = count + 4;
    QTime SPWTimeout;

    if (count>248)
    {
        appendToLog("WARNING === in function WRITE of rmapplugin *** CCSDS packet size > 248 bytes\n");
        return 1;
    }

    appendToLog(QString("*** START *** Send CCSDS packet of ")+ QString::number(count) + QString(" byte(s)"));

    if (GRESBStatusQueryRequest(LinkStatus, spwLinkSpinBox->value()) == 1)
    {
        this->Close();
        appendToLog("WARNING === in function WRITE of rmapplugin *** SPW link not running\n");
        return 1;
    }

    // SEND GRESB HEADER
    size[0] = (unsigned char) ((unsigned int) spwPacketSize>>16);
    size[1] = (unsigned char) ((unsigned int) spwPacketSize>>8);
    size[2] = (unsigned char) ((unsigned int) spwPacketSize);
    RMAPSend_SOCKET->write(&gresbProtocole, 1);
    RMAPSend_SOCKET->write((char*) size, 3);
    // SEND SPW HEADER
    RMAPSend_SOCKET->write(&targetLogicalAddress, 1);
    RMAPSend_SOCKET->write(&protocoleIdentifier, 1);
    RMAPSend_SOCKET->write(&reserved, 1);
    RMAPSend_SOCKET->write(&userApplication, 1);
    // SEND CCSDS PACKET
    RMAPSend_SOCKET->write(Value, count);
    SPWTimeout.start();
    while(RMAPSend_SOCKET->bytesToWrite() > 0)
    {
        RMAPSend_SOCKET->waitForBytesWritten(100);
        if(SPWTimeout.elapsed()>1000)
        {
            appendToLog("WARNING === in function WRITE of rmapplugin *** sending CCSDS packet timeout\n");
            return 1;
        }
    }

    appendToLog(QString("*** CCSDS packet sent"));

    return count;
}

int gresb::receiveSPWPacketLoop(unsigned char requestID)
{
    int result = 0;
    while (RMAPReceive_SOCKET->bytesAvailable())
    {
        result = receiveSPWPacket(requestID);
    }

    return result;
}

int gresb::receiveSPWPacket(unsigned char requestID) // SLOT
{
    QTime spwPacketReceiverTimeout;
    // GRESB HEADER
    char RES_TR_EP; // 6 bits REserved + 1 bit TRuncated + 1 bit EP error end of packet
    unsigned char packetLength2;
    unsigned char packetLength1;
    unsigned char packetLength0;
    unsigned int packetLength;

    if (requestID==1)
    {
        if (rmapPacketSEMAPHORE->available()) return rmapPacketSize;
    }

    RMAPReceive_SOCKET->blockSignals(1); // block the signals of the socket during packet reception
    // READ THE GRESB HEADER OF THE INCOMING PACKET
    spwPacketReceiverTimeout.start();
    while(RMAPReceive_SOCKET->bytesAvailable() < 4)
    {
        RMAPReceive_SOCKET->waitForReadyRead(100);
        if(spwPacketReceiverTimeout.elapsed()>1000) return -1; // ERROR === read GRSEB header TIMEOUT
    }
    RMAPReceive_SOCKET->read(&RES_TR_EP, 1);
    RMAPReceive_SOCKET->read( (char*) &packetLength2, 1);
    RMAPReceive_SOCKET->read( (char*) &packetLength1, 1);
    RMAPReceive_SOCKET->read( (char*) &packetLength0, 1);
    packetLength = (packetLength2<<16) + (packetLength1<<8) + (packetLength0);

    // READ THE SPW PACKET
    while(RMAPReceive_SOCKET->bytesAvailable() < packetLength)
    {
        RMAPReceive_SOCKET->waitForReadyRead(100);
        if(spwPacketReceiverTimeout.elapsed()>1000) return -2; // ERROR === read SPW packet TIMEOUT
    }
    RMAPReceive_SOCKET->read( spwPacket, packetLength );
    RMAPReceive_SOCKET->blockSignals(0);
    //emit sendMessage("Packet of size " + QString::number(packetLength) + " received");

    switch(spwPacket[1]) // byte 1 is the protocole identifier in the SPW packet
    {
    case 1: // 0x01 is the protocole identifier for RMAP packets
        if (rmapPacketSEMAPHORE->available()!=0) return -3; // ERROR === previous RMAP packet not processed yet
        for(unsigned int i=0; i<packetLength; i++) rmapPacket[i] = spwPacket[i];
        rmapPacketSize = packetLength;
        rmapPacketSEMAPHORE->release();
        emit sendMessage("RMAP packet of size " + QString::number(packetLength) + " received");
        return packetLength;

    case 2: // 0x02 is the protocole identifier for CCSDS packets
        /*if (ccsdsPacketSEMAPHORE->available()!=0)
        {
            emit sendMessage("in function [receiveSPWPacket] === ERROR === previous CCSDS packet not processed yet");
            return -4; // ERROR === previous CCSDS packet not processed yet
        }*/
        for(unsigned int i=0; i<packetLength; i++) ccsdsPacket[i] = spwPacket[i];
        ccsdsPacketSize = packetLength;
        storeCCSDSPacket(ccsdsPacket, packetLength);
        return packetLength;
    }
    return 0;
}

unsigned int gresb::storeCCSDSPacket(unsigned char *ccsdsPacket, unsigned int size)
{
    TMPacketToRead *generalPacket;

    generalPacket = new TMPacketToRead(ccsdsPacket, size);
    emit sendPacket(generalPacket);

    return 1;
}

void gresb::Open() // SLOT
{
    bool spwRunning = true;
    RMAPSend_SOCKET->connectToHost( QHostAddress(gresbBridgeIPDialogBox->getIP()),
                                    3000 + gresbVirtualLinkSpinBox->value()*2,
                                    QIODevice::WriteOnly);
    RMAPReceive_SOCKET->connectToHost( QHostAddress(gresbBridgeIPDialogBox->getIP()),
                                       3000 + gresbVirtualLinkSpinBox->value()*2+1,
                                       QIODevice::ReadOnly);
    GRESBStatusQuery_SOCKET->connectToHost( QHostAddress(gresbBridgeIPDialogBox->getIP()),
                                            3010,
                                            QIODevice::ReadWrite);
    GRESBStatusQuery_SOCKET->waitForConnected(10000);
    // initialize SPW packet semaphores
    while (rmapPacketSEMAPHORE->available()!=0) rmapPacketSEMAPHORE->acquire();
    while (ccsdsPacketSEMAPHORE->available()!=0) ccsdsPacketSEMAPHORE->acquire();
    if (GRESBStatusQueryRequest(LinkStatus, spwLinkSpinBox->value()) != 0)
    {
        spwRunning = gresbStatusQueryDialog->exec();
    }
    if (spwRunning == false) this->Close();
    else
    {
        emit appendToLog(QString("SpaceWire running on link ")+ QString::number(spwLinkSpinBox->value()));
        RMAPReceive_SOCKET->readAll();  // read all remaining data from the reception socket
        emit isOpen(true);
    }
}

void gresb::Close() // SLOT
{
    RMAPSend_SOCKET->disconnectFromHost();
    RMAPReceive_SOCKET->disconnectFromHost();
    GRESBStatusQuery_SOCKET->disconnectFromHost();
    emit isOpen(false);
}

int gresb::GRESBStatusQuery() // SLOT
{
    GRESBStatusQueryRequest(LinkStatus, 0);
    GRESBStatusQueryRequest(LinkStatus, 1);
    GRESBStatusQueryRequest(LinkStatus, 2);
    GRESBStatusQueryRequest(LinkStatistics, 0);
    GRESBStatusQueryRequest(LinkStatistics, 1);
    GRESBStatusQueryRequest(LinkStatistics, 2);
    return 0;
}

int gresb::GRESBStatusQueryRequest(GresbStatusQueryOption option, char link)
{
    gresb_status_query_t statusQueryCommand;
    gresb_link_status_reply_t linkStatusReply;
    gresb_link_statistics_reply_t linkStatisticsReply;
    QTime statusQueryTimeout;
    QString console_message;

    statusQueryCommand.protocolIdentifier = (char) 0x02;
    statusQueryCommand.reserved1 = (char) 0x00;
    statusQueryCommand.reserved0 = (char) 0x00;
    statusQueryCommand.option = (char) option;
    statusQueryCommand.value3 = (char) 0x00;
    statusQueryCommand.value2 = (char) 0x00;
    statusQueryCommand.value1 = (char) 0x00;
    statusQueryCommand.value0 = (char) link;

    GRESBStatusQuery_SOCKET->write((char*) ((void*) &statusQueryCommand), sizeof(statusQueryCommand));
    GRESBStatusQuery_SOCKET->flush();
    GRESBStatusQuery_SOCKET->waitForBytesWritten(1000);

    statusQueryTimeout.start();
    while(GRESBStatusQuery_SOCKET->bytesToWrite() > 0)
    {
        GRESBStatusQuery_SOCKET->waitForBytesWritten(100);
        if(statusQueryTimeout.elapsed()>1000)
        {
            emit appendToLog("WARNING === in function GRESBStatusQueryRequest of rmapplugin *** sending StatusQueryCommand timeout");
            return 1;
        }
    }

    switch (option)
    {
    case LinkStatus:
    {
        statusQueryTimeout.start();
        while(GRESBStatusQuery_SOCKET->bytesAvailable() < (int) sizeof(linkStatusReply))
        {
            GRESBStatusQuery_SOCKET->waitForReadyRead(100);
            if(statusQueryTimeout.elapsed()>1000)
            {
                console_message.sprintf("GRESBStatusQueryRequest / LinkStatus => error timeout bytesAvailable()\n");
                emit appendToLog(console_message);
                return 1;
            }
        }
        GRESBStatusQuery_SOCKET->read((char*) ((void*) &linkStatusReply), (int) sizeof(linkStatusReply));
        console_message.sprintf("%x", linkStatusReply.byte0);
        spwLinkStatusEnquiry->statusQueryTable->item(0, link)->setText(console_message);
        console_message.sprintf("%d", linkStatusReply.byte1);
        spwLinkStatusEnquiry->statusQueryTable->item(1, link)->setText(console_message);
        if (linkStatusReply.byte0 == 0) return 1;
        break;
    }
    case LinkStatistics:
    {
        statusQueryTimeout.start();
        while(GRESBStatusQuery_SOCKET->bytesAvailable() < (int) sizeof(linkStatisticsReply))
        {
            GRESBStatusQuery_SOCKET->waitForReadyRead(100);
            if(statusQueryTimeout.elapsed()>1000)
            {
                console_message.sprintf("GRESBStatusQueryRequest / LinkStatistics => error timeout bytesAvailable()\n");
                emit appendToLog(console_message);
                return 1;
            }
        }
        GRESBStatusQuery_SOCKET->read((char*) ((void*) &linkStatisticsReply), sizeof(linkStatisticsReply));
        /*console_message.sprintf("%d", charTab_TO_int(linkStatisticsReply.sizeOfDataTransmitted));
        UI->spwLinkStatusEnquiry->statusQueryTable->item(9, link)->setText(console_message);
        console_message.sprintf("%d", charTab_TO_int(linkStatisticsReply.numberOfPacketsTransmitted));
        UI->spwLinkStatusEnquiry->statusQueryTable->item(8, link)->setText(console_message);
        console_message.sprintf("%d", charTab_TO_int(linkStatisticsReply.numberOfTruncatedPacketsReceived));
        UI->spwLinkStatusEnquiry->statusQueryTable->item(6, link)->setText(console_message);
        console_message.sprintf("%d", charTab_TO_int(linkStatisticsReply.numberOfPacketsWithEEPReceived));
        UI->spwLinkStatusEnquiry->statusQueryTable->item(5, link)->setText(console_message);
        console_message.sprintf("%d", charTab_TO_int(linkStatisticsReply.sizeOfDataReceived));
        UI->spwLinkStatusEnquiry->statusQueryTable->item(4, link)->setText(console_message);
        console_message.sprintf("%d", charTab_TO_int(linkStatisticsReply.numberOfPacketsReceived));
        UI->spwLinkStatusEnquiry->statusQueryTable->item(3, link)->setText(console_message);*/
        break;
    }
    case NodeAddressStatistics:
    {
        break;
    }
    case GetRoute:
    {
        break;
    }
    }
    return 0;
}

void gresb::reTestSPWLink() // SLOT
{
    if (GRESBStatusQueryRequest(LinkStatus, spwLinkSpinBox->value()) == 0)
    {
        gresbStatusQueryDialog->accept();
    }
}

void gresb::RMAPSendConnectionState(QAbstractSocket::SocketState socketState) // SLOT
{
    rmapSendStateLabel->setText("");
    QString socketMessage = "RMAP Send Socket State: ";
    switch(socketState)
    {
    case QAbstractSocket::UnconnectedState :
        socketMessage.append("0 => Unconnected");
        break;
    case 1:
        socketMessage.append("1 => HostLookup");
        break;
    case 2:
        socketMessage.append("2 => Connecting");
        break;
    case 3:
        socketMessage.append("3 => Connected");
        break;
    case 4:
        socketMessage.append("4 => Bound");
        break;
    case 5:
        socketMessage.append("5 => Closing");
        break;
    case 6:
        socketMessage.append("6 => Listening");
        break;
    }
    rmapSendStateLabel->setText(socketMessage);
    emit appendToLog(socketMessage);
}

void gresb::RMAPReceiveConnectionState(QAbstractSocket::SocketState socketState) // SLOT
{
    rmapReceiveStateLabel->setText("");
    QString socketMessage = "RMAP Receive Socket State: ";
    switch(socketState)
    {
    case QAbstractSocket::UnconnectedState :
        socketMessage.append("0 => Unconnected");
        break;
    case 1:
        socketMessage.append("1 => HostLookup");
        break;
    case 2:
        socketMessage.append("2 => Connecting");
        break;
    case 3:
        socketMessage.append("3 => Connected");
        break;
    case 4:
        socketMessage.append("4 => Bound");
        break;
    case 5:
        socketMessage.append("5 => Closing");
        break;
    case 6:
        socketMessage.append("6 => Listening");
        break;
    }
    rmapReceiveStateLabel->setText(socketMessage);
    emit appendToLog(socketMessage);
}

void gresb::GRESBConnectionState(QAbstractSocket::SocketState socketState) // SLOT
{
    gresbStatusQueryLabel->setText("");
    QString socketMessage = "GRESB status query socket (port 3010): ";
    switch(socketState)
    {
    case QAbstractSocket::UnconnectedState :
        socketMessage.append("0 => Unconnected");
        break;
    case 1:
        socketMessage.append("1 => HostLookup");
        break;
    case 2:
        socketMessage.append("2 => Connecting");
        break;
    case 3:
        socketMessage.append("3 => Connected");
        break;
    case 4:
        socketMessage.append("4 => Bound");
        break;
    case 5:
        socketMessage.append("5 => Closing");
        break;
    case 6:
        socketMessage.append("6 => Listening");
        break;
    }
    gresbStatusQueryLabel->setText(socketMessage);
}

void gresb::readSettings()
{
    QSettings settings("lpp", "rmapplugin");
    gresbBridgeIP0 = (unsigned char) settings.value("gresbBridgeIP0", 129).toInt();
    gresbBridgeIP1 = (unsigned char) settings.value("gresbBridgeIP1", 104).toInt();
    gresbBridgeIP2 = (unsigned char) settings.value("gresbBridgeIP2", 27).toInt();
    gresbBridgeIP3 = (unsigned char) settings.value("gresbBridgeIP3", 244).toInt();
    this->gresbBridgeIPDialogBox->setIP(gresbBridgeIP0, gresbBridgeIP1, gresbBridgeIP2, gresbBridgeIP3);
}

void gresb::writeSettings()
{
    QSettings settings("lpp", "rmapplugin");
    settings.setValue("gresbBridgeIP0", this->gresbBridgeIPDialogBox->get_addressPart1());
    settings.setValue("gresbBridgeIP1", this->gresbBridgeIPDialogBox->get_addressPart2());
    settings.setValue("gresbBridgeIP2", this->gresbBridgeIPDialogBox->get_addressPart3());
    settings.setValue("gresbBridgeIP3", this->gresbBridgeIPDialogBox->get_addressPart4());
}