#include "VirtualChannel.h"

#include "CCSDSHandler.h"
#include "OBSWConfig.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serviceinterface/ServiceInterfaceStream.h"
#include "fsfw/tmtcservices/TmTcMessage.h"

VirtualChannel::VirtualChannel(uint8_t vcId, uint32_t tmQueueDepth, object_id_t ownerId)
    : vcId(vcId) {
  auto mqArgs = MqArgs(ownerId, reinterpret_cast<void*>(vcId));
  tmQueue = QueueFactory::instance()->createMessageQueue(
      tmQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}

ReturnValue_t VirtualChannel::initialize() {
  tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
  if (tmStore == nullptr) {
    sif::error << "VirtualChannel::initialize: Failed to get tm store" << std::endl;
    return RETURN_FAILED;
  }
  return RETURN_OK;
}

ReturnValue_t VirtualChannel::performOperation() {
  ReturnValue_t result = RETURN_OK;
  TmTcMessage message;

  uint8_t data[50] = {0xa};
  result = ptme->writeToVc(vcId, data, sizeof(data));
  if (result != RETURN_OK) {
	  sif::warning << "VirtualChannel::performOperation: Failed to send test data" << std::endl;
	  return result;
  }

  while (tmQueue->receiveMessage(&message) == RETURN_OK) {
    store_address_t storeId = message.getStorageId();
    const uint8_t* data = nullptr;
    size_t size = 0;
    result = tmStore->getData(storeId, &data, &size);
    if (result != RETURN_OK) {
      sif::warning << "VirtualChannel::performOperation: Failed to read data from IPC store"
                   << std::endl;
      tmStore->deleteData(storeId);
      return result;
    }

    if (linkIsUp) {
      result = ptme->writeToVc(vcId, data, size);
    }

    tmStore->deleteData(storeId);

    if (result != RETURN_OK) {
      return result;
    }
  }
  return result;
}

MessageQueueId_t VirtualChannel::getReportReceptionQueue(uint8_t virtualChannel) {
  return tmQueue->getId();
}

void VirtualChannel::setPtmeObject(PtmeIF* ptme_) {
  if (ptme_ == nullptr) {
    sif::warning << "VirtualChannel::setPtmeObject: Invalid ptme object" << std::endl;
    return;
  }
  ptme = ptme_;
}

void VirtualChannel::setLinkState(bool linkIsUp_) { linkIsUp = linkIsUp_; }