#include "stardundee.h"
#include <QTime>
#include <math.h>

StarDundee::StarDundee(QWidget *parent) :
    QWidget(parent)
{
    // Packet receiver
    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 cmmand code for the RMAP transfers

    rmapSourceLogicalAddress = 0x20;
    connection_LAYOUT = new QGridLayout;

    usbDeviceNumber_LABEL = new QLabel(tr("USB device number: "));
    linkNumber_LABEL = new QLabel(tr("SpaceWire link number: "));
    sourceLogicalAddress_LABEL = new QLabel(tr("Source logical address: "));

    usbDeviceNumber_SPINBOX = new QSpinBox;
    usbDeviceNumber_SPINBOX->setRange(0,32);
    usbDeviceNumber_SPINBOX->setValue(0);
    linkNumber_SPINBOX = new QSpinBox;
    linkNumber_SPINBOX->setRange(1,2);
    linkNumber_SPINBOX->setValue(1);
    sourceLogicalAddress_SPINBOX = new QSpinBox;
    sourceLogicalAddress_SPINBOX->setRange(0,254);
    sourceLogicalAddress_SPINBOX->setValue(32);

    connection_LAYOUT->addWidget(usbDeviceNumber_LABEL, 0, 0, 1, 1);
    connection_LAYOUT->addWidget(usbDeviceNumber_SPINBOX, 0, 1, 1, 1);
    connection_LAYOUT->addWidget(linkNumber_LABEL, 1, 0, 1, 1);
    connection_LAYOUT->addWidget(linkNumber_SPINBOX, 1, 1, 1, 1);
    connection_LAYOUT->addWidget(sourceLogicalAddress_LABEL, 2, 0, 1, 1);
    connection_LAYOUT->addWidget(sourceLogicalAddress_SPINBOX, 2, 1, 1, 1);

    connection_LAYOUT->setRowStretch(3, 1);
    connection_LAYOUT->setColumnStretch(2, 1);

    this->setLayout(connection_LAYOUT);

    connect(this->sourceLogicalAddress_SPINBOX, SIGNAL(valueChanged(int)), this, SLOT(sourceHasChanged(int)));
}

StarDundee::~StarDundee()
{
    free(rmapPacket);
    free(ccsdsPacket);
    free(spwPacket);
    USBSpaceWire_Close(hDevice); // Close the device
}

unsigned int StarDundee::Open()
{
    int status;
    U32 statusControl;
    U32 routingTableEntry;
    int tableEntry;

    if (!USBSpaceWire_Open(&hDevice, usbDeviceNumber_SPINBOX->value())) // Open the USB device
    {
    emit sendMessage("stardundee *** Open *** ERROR: USBSpaceWire_Open(&hDevice, 0))");
    return -1;
    }
    emit sendMessage("stardundee *** Open *** USBSpaceWire_Open successful, device number: "
                     + QString::number(usbDeviceNumber_SPINBOX->value()));

    USBSpaceWire_EnableNetworkMode(hDevice, 0); // deactivate the network mode
    CFGSpaceWire_EnableRMAP(1);                 // Enable the use of RMAP
    CFGSpaceWire_SetRMAPDestinationKey(0x20);   // Set the destination key expected by STAR-Dundee devices

    // Set the path and return path to the device
    CFGSpaceWire_StackClear();
    CFGSpaceWire_AddrStackPush(0);
    CFGSpaceWire_AddrStackPush(254);
    CFGSpaceWire_RetAddrStackPush(254);

    // set the base transmit rate to 100 MHz
    status = CFGSpaceWire_SetBrickBaseTransmitRate( hDevice, CFG_BRK_CLK_100_MHZ, CFG_BRK_DVDR_1, 0xff);
    if (status != CFG_TRANSFER_SUCCESS)
    {
        emit sendMessage("ERROR CFGSpaceWire_SetBrickBaseTransmitRate");
    }
    else emit sendMessage("OK CFGSpaceWire_SetBrickBaseTransmitRate, base rate = 100 MHz");
    // read the link status
    if (CFGSpaceWire_GetLinkStatusControl(hDevice, linkNumber_SPINBOX->value(), &statusControl) != CFG_TRANSFER_SUCCESS)
    {
        emit sendMessage("Could not read link status control for link " + QString::number(linkNumber_SPINBOX->value()));
    }
    else
    {
        emit sendMessage("OK CFGSpaceWire_GetLinkStatusControl of link " + QString::number(linkNumber_SPINBOX->value()));

        // Set the link status control register properties
        CFGSpaceWire_LSEnableAutoStart(&statusControl, 1);
        CFGSpaceWire_LSEnableStart(&statusControl, 1);
        CFGSpaceWire_LSEnableDisabled(&statusControl, 0);
        CFGSpaceWire_LSEnableTristate(&statusControl, 0);
        CFGSpaceWire_LSSetOperatingSpeed(&statusControl, 9);    // sets the link speed to ( 100 MHz / (9+1) ) = 10 MHz

        // Set the link status control register
        if (CFGSpaceWire_SetLinkStatusControl(hDevice, linkNumber_SPINBOX->value(), statusControl) != CFG_TRANSFER_SUCCESS)
        {
            emit sendMessage("Could not set the link status control for link " + QString::number(linkNumber_SPINBOX->value()));
        }
        else emit sendMessage("Set the link status control for link " + QString::number(linkNumber_SPINBOX->value()));
    }

    // SET THE ROUTING TABLE ENTRY FOR LOGICAL ADDRESSING, TARGET 254 <=> 0xfe
    tableEntry = rmapTargetLogicalAddress;
    if (CFGSpaceWire_ClearRoutingTableEntry(hDevice, tableEntry) != CFG_TRANSFER_SUCCESS)
    {
        emit sendMessage("Could not clear routing table entry " + QString::number(tableEntry));
    }
    else emit sendMessage("Routing table entry [" + QString::number(tableEntry) + "] cleared");
    // Build the routing table entry to route out of port 1 with header deletion disabled and priority normal
    CFGSpaceWire_RTBuildRoutingTableEntry(&routingTableEntry,
                                          (1<<linkNumber_SPINBOX->value()), // 0x02 = 0000 0010
                                          0,
                                          0);
    // Set the routing table entry for logical address tableEntry
    if (CFGSpaceWire_SetRoutingTableEntry(hDevice, tableEntry, routingTableEntry) != CFG_TRANSFER_SUCCESS)
    {
        emit sendMessage("Could not set routing table entry [" + QString::number(tableEntry) + "]");
    }
    else emit sendMessage("Routing table entry [" + QString::number(tableEntry) + "] set" );

    // SET THE ROUTING TABLE ENTRY FOR LOGICAL ADDRESSING, TARGET 32 <=> 0x20
    tableEntry = sourceLogicalAddress_SPINBOX->value();
    if (CFGSpaceWire_ClearRoutingTableEntry(hDevice, tableEntry) != CFG_TRANSFER_SUCCESS)
    {
        emit sendMessage("Could not clear routing table entry " + QString::number(tableEntry));
    }
    else emit sendMessage("Routing table entry [" + QString::number(tableEntry) + "] cleared");

    // Build the routing table entry to route out of port 3 with header deletion disabled and priority normal
    CFGSpaceWire_RTBuildRoutingTableEntry(&routingTableEntry,
                                          (1<<3), // 0x02 = 0000 0010
                                          0,
                                          0);
    // Set the routing table entry for logical address tableEntry
    if (CFGSpaceWire_SetRoutingTableEntry(hDevice, tableEntry, routingTableEntry) != CFG_TRANSFER_SUCCESS)
    {
        emit sendMessage("Could not set routing table entry [" + QString::number(tableEntry) + "]");
    }
    else emit sendMessage("Routing table entry [" + QString::number(tableEntry) + "] set" );

    emit sendMessage("The driver's current send buffer size is " + QString::number(USBSpaceWire_GetDriverSendBufferSize(hDevice)) + " bytes");

    USBSpaceWire_RegisterReceiveOnAllPorts(hDevice);    // Register to receive on all ports
    USBSpaceWire_ClearEndpoints(hDevice);               // clear the USB endpoints

    // initialize SPW packet semaphores
    while (rmapPacketSEMAPHORE->available()!=0) rmapPacketSEMAPHORE->acquire();
    while (ccsdsPacketSEMAPHORE->available()!=0) ccsdsPacketSEMAPHORE->acquire();

    emit isOpen(true);

    return 1;
}

unsigned int StarDundee::Close()
{
    USBSpaceWire_Close(hDevice); // Close the device
    emit sendMessage("stardundee *** Close *** USBSpaceWire_Close, device: " + QString::number(usbDeviceNumber_SPINBOX->value()));

    USBSpaceWire_UnregisterReceiveOnAllPorts(hDevice);  // Stop receiving on all ports

    emit isOpen(false);

    return 1;
}

unsigned int StarDundee::GetRoutingTableEntry()
{
    U32 routingTableEntry, outputPorts;
    char enabled, delHead, priority;
    int portNum;
    int tableEntry;

    tableEntry = rmapTargetLogicalAddress;

    // Set the path and return path to the device
    CFGSpaceWire_StackClear();
    CFGSpaceWire_AddrStackPush(0);
    CFGSpaceWire_AddrStackPush(254);
    CFGSpaceWire_RetAddrStackPush(254);

    // Read the routing table entry
    if (CFGSpaceWire_GetRoutingTableEntry(hDevice, tableEntry, &routingTableEntry) != CFG_TRANSFER_SUCCESS)
    {
        emit sendMessage("Could not read routing table entry [" + QString::number(tableEntry) + "]");
    }
    else
    {
        // Display the routing table entry properties
        CFGSpaceWire_RTIsEnabled(routingTableEntry, &enabled);
        CFGSpaceWire_RTIsDelHead(routingTableEntry, &delHead);
        CFGSpaceWire_RTIsPriority(routingTableEntry, &priority);
        CFGSpaceWire_RTGetOutputPorts(routingTableEntry, &outputPorts);
        emit sendMessage("CFGSpaceWire_RTIsEnabled      : " + QString::number(enabled));
        emit sendMessage("CFGSpaceWire_RTIsDelHead      : " + QString::number(delHead));
        emit sendMessage("CFGSpaceWire_RTIsPriority     : " + QString::number(priority));
        emit sendMessage("CFGSpaceWire_RTGetOutputPorts : ");
        for (portNum = 0; portNum < 32; portNum++)
        {
            if (outputPorts & (1 << portNum))
            {
                emit sendMessage(QString::number(portNum));
            }
        }
    }

    return 1;
}

unsigned int StarDundee::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;
    }

    emit this->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 StarDundee::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 (this->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;
        this->acquireRMAPSemaphore();
    }

    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 (this->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;
            }
        }
        this->acquireRMAPSemaphore();
    }

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

unsigned int StarDundee::WriteBLOCK(char *data, unsigned int nbBytes, unsigned int address)
{
    RMAP *RMAPCommand;
    char * packet;
    unsigned char offset, headerSize, CRCSize;
    char* aux;
    unsigned int totalSize;
    int errorCode;
    QString console_message;

    if (getLinkStatus(this->linkNumber_SPINBOX->value()) == 0)
    {
        this->Close();
        return 1;
    }

    RMAPCommand = new RMAP(this->commandCode,
                           rmapTargetLogicalAddress,
                           rmapSourceLogicalAddress,
                           address,
                           nbBytes,
                           data);
    headerSize = sizeof(RMAPCommand->RMAPHeader);
    CRCSize = 1;

    //*************************
    // BUILD THE PACKET TO SEND
    totalSize = headerSize + nbBytes + CRCSize;
    packet = (char*) malloc( totalSize );
    aux = (char*) ((void*) &RMAPCommand->RMAPHeader);
    for(unsigned int i = 0; i<headerSize; i++) packet[i] = aux[i];
    offset = headerSize;
    for(unsigned int i = 0; i<nbBytes; i++)
    {
        packet[i+offset] = data[i];
    }
    packet[nbBytes+offset] = RMAPCommand->dataCRC;

    //****************
    // SEND THE PACKET
    result = USBSpaceWire_SendPacket(hDevice,
                                     packet,
                                     totalSize,
                                     BWAIT_1, &pIdentifier);
    if (result != TRANSFER_SUCCESS)
    {
        emit sendMessage("ERROR *** WriteStarDundee when sending packet of size "
                         + QString::number(totalSize) +", with code: " + QString::number(result));
        USBSpaceWire_FreeSend(hDevice, pIdentifier);
        free(data);
        return 1;
    }
    else emit sendMessage("OK *** WriteStarDundee when sending packet of size " + QString::number(totalSize));

    //**************
    // Free the send
    USBSpaceWire_FreeSend(hDevice, pIdentifier);
    free(packet);

    if ( (commandCode == writeSingle_noVer_Rep) | (commandCode == writeInc_noVer_Rep) |
         (commandCode == writeSingle_ver_rep) | (commandCode == writeInc_ver_rep) )
    {
        // WAIT FOR THE RMAP REPLY PACKET
        errorCode = this->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 (this->rmapPacket[3]) // byte 4 is the status byte in the reply
        {
        case 0:
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit 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");
            emit RMAP_write_reply_setText("reply to the write command required\nlast reply status: 12 Invalid target logical address");
            break;
        }
        this->acquireRMAPSemaphore();
    }
    else
        emit RMAP_write_reply_setText("reply to the write command required\nlast reply status: unavailable");

    return nbBytes;
}

unsigned int StarDundee::ReadBLOCK(unsigned int nbBytes, unsigned int address)
{
    int errorCode;
    RMAP *RMAPCommand;
    unsigned int dataLength;

    //**********************
    // Check the link status
    if ( getLinkStatus( this->linkNumber_SPINBOX->value() ) == 0 )
    {
        this->Close();
        emit appendToLog("WARNING === in function WriteSPW of StarDundee *** SPW link not running\n");
        return 0;
    }

    //**********************
    // SEND THE READ COMMAND
    if (nbBytes > 4)
    {
        RMAPCommand = new RMAP(read_Inc,
                               rmapTargetLogicalAddress,
                               rmapSourceLogicalAddress,
                               address,
                               nbBytes,
                               NULL);
    }
    else
    {
        RMAPCommand = new RMAP(read_Single,
                               rmapTargetLogicalAddress,
                               rmapSourceLogicalAddress,
                               address,
                               nbBytes,
                               NULL);
    }
    result = USBSpaceWire_SendPacket(hDevice,
                                     (unsigned char*) ((void*) &RMAPCommand->RMAPHeader),
                                     sizeof(RMAPCommand->RMAPHeader),
                                     BWAIT_1, &pIdentifier);
    if (result != TRANSFER_SUCCESS) emit sendMessage("ERR *** ReadStarDundee *** ReadBLOCK *** sending the READ command ");
    else emit sendMessage("OK *** ReadStarDundee *** ReadBLOCK *** sending the READ command");

    //*********************************
    // RECEIVE THE INCOMING RMAP PACKET
    errorCode = this->receiveSPWPacket(1); // request ID 1 is for RMAP packet
    if (errorCode<=0)
    {
        emit appendToLog("WARNING === in function ReadBLOCK of StarDundee *** 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 StarDundee *** number of data received ("
                    +QString::number(dataLength)
                    +") not equal to number of data requested ("
                    +QString::number(nbBytes)
                    +")");
        return 0;
     }
    return dataLength;
}

unsigned int StarDundee::WriteSPW(char *Value, unsigned int count, char targetLogicalAddress, char userApplication)
{
    char protocoleIdentifier = 0x02;
    char reserved = 0x00;
    char *SPWData;
    unsigned int totalSize;

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

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

    if ( getLinkStatus( this->linkNumber_SPINBOX->value() ) == 0 )
    {
        this->Close();
        emit appendToLog("WARNING === in function WriteSPW of StarDundee *** SPW link not running\n");
        return 1;
    }

    totalSize = count + 4;
    SPWData = (char*) malloc(totalSize);
    // SPW HEADER
    SPWData[0] = targetLogicalAddress;
    SPWData[1] = protocoleIdentifier;
    SPWData[2] = reserved;
    SPWData[3] = userApplication;
    // CCSDS PACKET
    for (unsigned int i = 0; i<count; i++)
    {
        SPWData[i+4] = Value[i];
    }

    //************
    // SEND THE PACKET
    result = USBSpaceWire_SendPacket(hDevice,
                                     SPWData,
                                     totalSize,
                                     BWAIT_1, &pIdentifier);
    if (result != TRANSFER_SUCCESS)
    {
        emit sendMessage("ERROR *** WriteSPW when sending packet of size "
                         + QString::number(totalSize) +", with code: " + QString::number(result));
        USBSpaceWire_FreeSend(hDevice, pIdentifier);
        free(SPWData);
        return 0;
    }
    else emit sendMessage("OK *** WriteSPW when sending packet of size " + QString::number(totalSize));

    //**************
    // Free the send
    USBSpaceWire_FreeSend(hDevice, pIdentifier);
    free(SPWData);

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

    return 1;
}

unsigned int StarDundee::getLinkStatus(unsigned char link)
{
    U32 statusControl, errorStatus, portType;
    U32 linkStatus, operatingSpeed, outputPortConnection;
    char isLinkRunning, isAutoStart, isStart, isDisabled, isTristate;

    // Read the link status control register
    if (CFGSpaceWire_GetLinkStatusControl(hDevice, 1, &statusControl) != CFG_TRANSFER_SUCCESS)
    {
        emit appendToLog("Could not read link status control for link" + QString::number(link));
    }
    else
    {
        // Display the link status control register properties
        CFGSpaceWire_LSPortType(statusControl, &portType);
        if (portType == CFG_CONFIGURATION_PORT)
        {
            CFGSpaceWire_LSConfigErrorStatus(statusControl, &errorStatus);
            //emit appendToLog("Configuration port error status = " + QString::number(errorStatus));
        }
        else if (portType == CFG_SPACEWIRE_EXTERNAL_PORT)
        {
            CFGSpaceWire_LSExternalErrorStatus(statusControl, &errorStatus);
            //emit appendToLog("External port error status = " + QString::number(errorStatus));
        }
        else
        {
            CFGSpaceWire_LSErrorStatus(statusControl, &errorStatus);
            //emit appendToLog("SpaceWire link error status = " + QString::number(errorStatus));
        }
        CFGSpaceWire_LSLinkState(statusControl, &linkStatus);
        CFGSpaceWire_LSIsLinkRunning(statusControl, &isLinkRunning);
        CFGSpaceWire_LSIsAutoStart(statusControl, &isAutoStart);
        CFGSpaceWire_LSIsStart(statusControl, &isStart);
        CFGSpaceWire_LSIsDisabled(statusControl, &isDisabled);
        CFGSpaceWire_LSIsTristate(statusControl, &isTristate);
        CFGSpaceWire_LSOperatingSpeed(statusControl, &operatingSpeed);
        CFGSpaceWire_LSOutputPortConnection(statusControl, &outputPortConnection);
        //emit appendToLog("The link state is = " + QString::number(linkStatus));
        //emit appendToLog("The link is running = " + QString::number(isLinkRunning));
        //emit appendToLog("The autostart bit is enabled = " + QString::number(isAutoStart));
        //emit appendToLog("The start bit is enabled = " + QString::number(isStart));
        //emit appendToLog("The link is disabled = " + QString::number(isDisabled));
        //emit appendToLog("The tri-state bit is enabled = " + QString::number(isAutoStart));
        //emit appendToLog("The operating speed is = " + QString::number(operatingSpeed));
        //emit appendToLog("This port is currently connected to output port = " + QString::number(outputPortConnection));
    }
    if (linkStatus == 5) return 1;
    else return 0;
}

int StarDundee::receiveSPWPacket(unsigned char requestID) // SLOT
{
    unsigned int packetLength;
    unsigned int nbBytes;
    unsigned nPacketNum;
    USB_SPACEWIRE_PACKET_PROPERTIES properties; // The properties of the read

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

    //********************
    // TRY TO RECEIVE DATA
    nbBytes = qMax(RMAP_MAX_PACKET_LENGTH, CCSDS_MAX_PACKET_LENGTH); // maximum size of the packet to receive
    nPacketNum = 1;
    result = USBSpaceWire_ReadPackets(hDevice, spwPacket, nbBytes, nPacketNum, BWAIT_1, &properties, &pIdentifier);
    if (result != TRANSFER_SUCCESS) emit sendMessage("Error: Could not receive the packet");
    else
    {
        emit sendMessage("A packet of length " + QString::number(properties.len) + " has been successfully received");
        USBSpaceWire_FreeRead(hDevice, pIdentifier);         // Free the receive
    }
    packetLength = properties.len;

    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) return -4; // ERROR === previous CCSDS packet not processed yet
        for(unsigned int i=0; i<packetLength; i++) ccsdsPacket[i] = spwPacket[i];
        ccsdsPacketSize = packetLength;
        ccsdsPacketSEMAPHORE->release();
        emit(ccsdsPacketAvailable(ccsdsPacket, packetLength));
        return packetLength;
    }
    return 0;
}