#include <sys/mman.h>
#include <fcntl.h>

#include <mission/obc/CCSDSIPCoreBridge.h>

CCSDSIPCoreBridge::CCSDSIPCoreBridge(object_id_t objectId, object_id_t tcDestination,
        object_id_t tmStoreId, object_id_t tcStoreId, LinuxLibgpioIF* gpioComIF,
        std::string uioPtme, gpioId_t papbBusyId, gpioId_t papbEmptyId) :
        TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId), gpioComIF(gpioComIF), uioPtme(
                uioPtme), papbBusyId(papbBusyId), papbEmptyId(papbEmptyId) {
}

CCSDSIPCoreBridge::~CCSDSIPCoreBridge() {
}

ReturnValue_t CCSDSIPCoreBridge::initialize() {
    ReturnValue_t result = TmTcBridge::initialize();

    fd = open("/dev/uio0", O_RDWR);
    if (fd < 1) {
        sif::debug << "CCSDSIPCoreBridge::initialize: Invalid UIO device file" << std::endl;
        return RETURN_FAILED;
    }

    /**
     * Map uio device in virtual address space
     * PROT_WRITE: Map uio device in writable only mode
     */
    ptmeBaseAddress = static_cast<uint32_t*>(mmap(NULL, MAP_SIZE, PROT_WRITE, MAP_SHARED, fd, 0));

    if (ptmeBaseAddress == MAP_FAILED) {
        sif::error << "CCSDSIPCoreBridge::initialize: Failed to map uio address" << std::endl;
        return RETURN_FAILED;
    }

    return result;
}

ReturnValue_t CCSDSIPCoreBridge::handleTm() {

    /** Size of one complete transfer frame data field amounts to 1105 bytes */
    uint8_t testPacket[1105];

    /** Fill one test packet */
    for(int idx = 0; idx < 1105; idx++) {
            testPacket[idx] = static_cast<uint8_t>(idx & 0xFF);
    }

    ReturnValue_t result = sendTm(testPacket, 1105);
    if(result != RETURN_OK) {
        return result;
    }
    return RETURN_OK;
}

ReturnValue_t CCSDSIPCoreBridge::sendTm(const uint8_t * data, size_t dataLen) {

    if(pollPapbSignal() == RETURN_OK) {
        startPacketTransfer();
    }

    for(size_t idx = 0; idx < dataLen; idx++) {
        if(pollPapbSignal() == RETURN_OK) {
            *(ptmeBaseAddress + PTME_DATA_REG_OFFSET) = static_cast<uint32_t>(*(data + idx));
        }
        else {
            sif::debug << "CCSDSIPCoreBridge::sendTm: Only written " << idx - 1 << " of " << dataLen
                    << " data" << std::endl;
            return RETURN_FAILED;
        }
    }

    if(pollPapbSignal() == RETURN_OK) {
        endPacketTransfer();
    }
    return RETURN_OK;
}

void CCSDSIPCoreBridge::startPacketTransfer() {
    *(ptmeBaseAddress) = PTME_CONFIG_START;
}

void CCSDSIPCoreBridge::endPacketTransfer() {
    *(ptmeBaseAddress) = PTME_CONFIG_END;
}

ReturnValue_t CCSDSIPCoreBridge::pollPapbSignal() {
    int papbBusyState = 0;
    ReturnValue_t result = RETURN_OK;

    /** Check if PAPB interface is ready to receive data */
    result = gpioComIF->readGpio(papbBusyId, &papbBusyState);
    if (result != RETURN_OK) {
        sif::debug << "CCSDSIPCoreBridge::pollPapbSignal: Failed to read papb busy signal"
                << std::endl;
        return RETURN_FAILED;
    }
    if (!papbBusyState) {
        sif::debug << "CCSDSIPCoreBridge::pollPapbSignal: PAPB busy" << std::endl;
        return PAPB_BUSY;
    }

    return RETURN_OK;
}