#include <fcntl.h>
#include <linux/obc/Ptme.h>
#include <sys/mman.h>
#include <unistd.h>

#include "PtmeConfig.h"
#include "fsfw/serviceinterface/ServiceInterface.h"

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

Ptme::~Ptme() {}

ReturnValue_t Ptme::initialize() {
  VcInterfaceMapIter iter;
  for (iter = vcInterfaceMap.begin(); iter != vcInterfaceMap.end(); iter++) {
    iter->second->initialize();
  }
  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;
  }
}