#include "TmFunnelBase.h"

#include <fsfw/tmtcservices/TmTcMessage.h>

#include "fsfw/ipc/QueueFactory.h"

TmFunnelBase::TmFunnelBase(object_id_t objectId, StorageManagerIF &tmStore, uint32_t tmMsgDepth,
                           uint32_t tcMsgDepth, StorageManagerIF &ipcStore)
    : SystemObject(objectId), tmStore(tmStore), ipcStore(ipcStore) {
  tmQueue = QueueFactory::instance()->createMessageQueue(tmMsgDepth);
  tcQueue = QueueFactory::instance()->createMessageQueue(tcMsgDepth);
}

MessageQueueId_t TmFunnelBase::getCommandQueue() const { return tcQueue->getId(); }

TmFunnelBase::~TmFunnelBase() { QueueFactory::instance()->deleteMessageQueue(tmQueue); }

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

void TmFunnelBase::addDestination(const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) {
  auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
  destinations.emplace_back(queueId, vcid);
}

ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId,
                                                     TmTcMessage &message,
                                                     const uint8_t *packetData, size_t size) {
  ReturnValue_t result;
  for (unsigned int idx = 0; idx < destinations.size(); idx++) {
    const auto &destVcidPair = destinations[idx];
    if (destinations.size() > 1) {
      if (idx < destinations.size() - 1) {
        // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
        // to bother with send order and where the data is deleted.
        store_address_t storeId;
        result = tmStore.addData(&storeId, packetData, size);
        if (result == returnvalue::OK) {
          message.setStorageId(storeId);
        } else {
#if FSFW_CPP_OSTREAM_ENABLED == 1
          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
                     << std::endl;
#endif
        }
      } else {
        message.setStorageId(origStoreId);
      }
    }
    result = tmQueue->sendMessage(destVcidPair.first, &message);
    if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
#endif
      tmStore.deleteData(message.getStorageId());
    }
  }
  return result;
}