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

#include <linux/obc/Ptme.h>
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "PtmeConfig.h"

Ptme::Ptme(object_id_t objectId): SystemObject(objectId) {
}

Ptme::~Ptme() {
}

ReturnValue_t Ptme::initialize() {

    int fd = open(PtmeConfig::UIO_DEVICE_FILE, O_RDWR);
    if (fd < 1) {
        sif::warning << "Ptme::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 << "Ptme::initialize: Failed to map uio address" << std::endl;
        return RETURN_FAILED;
    }

    VcInterfaceMapIter iter;
    for (iter = vcInterfaceMap.begin(); iter != vcInterfaceMap.end(); iter++) {
        iter->second->setRegisterAddress(ptmeBaseAddress);
    }

    return RETURN_OK;
}

ReturnValue_t Ptme::writeToVc(uint8_t vcId, const uint8_t * data, size_t size) {
    ReturnValue_t result = RETURN_OK;
    VcInterfaceMapIter vcInterfaceMapIter = vcInterfaceMap.find(vcId);
    if (vcInterfaceMapIter == vcInterfaceMap.end()) {
        sif::warning << "Ptme::writeToVc: No virtual channel interface found for the virtual "
                "channel with id " << static_cast<unsigned int>(vcId) << std::endl;
        return UNKNOWN_VC_ID;
    }
    result = vcInterfaceMapIter->second->write(data, size);
    return result;
}

void Ptme::addVcInterface(VcId_t vcId, VcInterfaceIF* vc) {

    if (vcId > common::NUMBER_OF_VIRTUAL_CHANNELS) {
        sif::warning << "Ptme::addVcInterface: Invalid virtual channel ID" << std::endl;
        return;
    }

    if (vc == nullptr) {
        sif::warning << "Ptme::addVcInterface: Invalid virtual channel interface" << std::endl;
        return;
    }

    auto status = vcInterfaceMap.emplace(vcId, vc);
    if (status.second == false) {
        sif::warning << "Ptme::addVcInterface: Failed to add virtual channel interface to "
                "virtual channel map" << std::endl;
        return;
    }
}