#include "spwpacketreceiver.h"
#include <QTime>
#include "rmapoperations.h"

spwpacketreceiver::spwpacketreceiver(QObject *parent) :
    QObject(parent)
{
    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) );
    connect(this, SIGNAL(ccsdsPacketAvailable(unsigned char*,uint)), this, SLOT(processCCSDSPacket(unsigned char*,uint)));
}

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

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

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

    // READ THE SPW PACKET
    while(gresbReceptionSocket->bytesAvailable() < packetLength)
    {
        gresbReceptionSocket->waitForReadyRead(100);
        if(spwPacketReceiverTimeout.elapsed()>1000) return -2; // ERROR === read SPW packet TIMEOUT
    }
    gresbReceptionSocket->read( spwPacket, packetLength );
    gresbReceptionSocket->blockSignals(0);

    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;
}

bool spwpacketreceiver::isRMAPPacketSemaphoreAvailable()
{
    if (rmapPacketSEMAPHORE->available()!=0) return true;
    return false;
}

void spwpacketreceiver::acquireRMAPSemaphore()
{
    rmapPacketSEMAPHORE->acquire();
}

void spwpacketreceiver::acquireCCSDSSemaphore()
{
    ccsdsPacketSEMAPHORE->acquire();
}

void spwpacketreceiver::processCCSDSPacket(unsigned char *ccsdsPacket, unsigned int size)
{
    QString message;
    unsigned int fine_time_value = 0;
    fine_time_value = ((unsigned int) ccsdsPacket[7]<<24)
            + ((unsigned int) ccsdsPacket[6]<<16)
            + ((unsigned int) ccsdsPacket[5]<<8)
            + ((unsigned int) ccsdsPacket[4]);
    message.append(QTime::currentTime().toString() +":" + QString::number(QTime::currentTime().msec()) + ": ");
    message.append("size "
                   + QString::number(size)
                   +" *** header "
                   + QString::number(ccsdsPacket[0], 16)
                   + " "
                   + QString::number(ccsdsPacket[1], 16)
                   + " "
                   + QString::number(ccsdsPacket[2], 16)
                   + " "
                   + QString::number(ccsdsPacket[3], 16)
                   + " *** coarse time "
                   + QString::number(fine_time_value));
                   //+ QString::number(ccsdsPacket[4], 16)
                   //+" "
                   //+ QString::number(ccsdsPacket[5], 16)
                   //+" "
                   //+ QString::number(ccsdsPacket[6], 16)
                   //+" "
                   //+ QString::number(ccsdsPacket[7], 16));
    ccsdsPacketSEMAPHORE->acquire();
    emit sendMessage(message);
}