#include "bridge.h"

bridge::bridge(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) );

    bridge_LAYOUT = new QGridLayout;
}

bridge::~bridge()
{
    free(rmapPacket);
    free(ccsdsPacket);
    free(spwPacket);
}

unsigned int bridge::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 bridge::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;
        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 (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;
            }
        }
        rmapPacketSEMAPHORE->acquire();
    }

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