#include "PusTmFunnel.h"

#include "eive/definitions.h"
#include "eive/objects.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager.h"
#include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h"

PusTmFunnel::PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore,
                         SdCardMountedIF &sdcMan, uint32_t messageDepth)
    : TmFunnelBase(objectId, tmStore, messageDepth),
      timeReader(timeReader),
      miscStore(objects::MISC_STORE, "misc", RolloverInterval::HOURLY, 8, sdcMan) {
	miscStore.addApid(config::EIVE_PUS_APID);
	miscStore.addService(17);
}

PusTmFunnel::~PusTmFunnel() = default;

ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
  TmTcMessage currentMessage;
  ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
  while (status == returnvalue::OK) {
    status = handlePacket(currentMessage);
    if (status != returnvalue::OK) {
      sif::warning << "TmFunnel packet handling failed" << std::endl;
      break;
    }
    status = tmQueue->receiveMessage(&currentMessage);
  }

  if (status == MessageQueueIF::EMPTY) {
    return returnvalue::OK;
  }
  return status;
}

ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) {
  uint8_t *packetData = nullptr;
  size_t size = 0;
  store_address_t origStoreId = message.getStorageId();
  ReturnValue_t result = tmStore.modifyData(origStoreId, &packetData, &size);
  if (result != returnvalue::OK) {
    return result;
  }
  PusTmZeroCopyWriter packet(timeReader, packetData, size);
  result = packet.parseDataWithoutCrcCheck();
  if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "PusTmFunnel::handlePacket: Error parsing received PUS packet" << std::endl;
#endif
    return result;
  }
  packet.setSequenceCount(sourceSequenceCount++);
  sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
  packet.updateErrorControl();

  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;
}

const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; }