#include "PusTmFunnel.h"

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

PusTmFunnel::PusTmFunnel(object_id_t objectId, const AcceptsTelemetryIF &downlinkDestination,
                         TimeReaderIF &timeReader, uint32_t messageDepth)
    : SystemObject(objectId), timeReader(timeReader) {
  tmQueue = QueueFactory::instance()->createMessageQueue(messageDepth,
                                                         MessageQueueMessage::MAX_MESSAGE_SIZE);
  tmQueue->setDefaultDestination(downlinkDestination.getReportReceptionQueue());
}

PusTmFunnel::~PusTmFunnel() = default;

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

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;
  } else {
    return status;
  }
}

ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage *message) {
  uint8_t *packetData = nullptr;
  size_t size = 0;
  ReturnValue_t result = tmPool->modifyData(message->getStorageId(), &packetData, &size);
  if (result != returnvalue::OK) {
    return result;
  }
  PusTmZeroCopyWriter packet(timeReader, packetData, size);
  result = packet.parseDataWithoutCrcCheck();
  if (result != returnvalue::OK) {
    return result;
  }
  packet.setSequenceCount(sourceSequenceCount++);
  sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
  packet.updateErrorControl();

  result = tmQueue->sendToDefault(message);
  if (result != returnvalue::OK) {
    tmPool->deleteData(message->getStorageId());
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "TmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
#endif
    return result;
  }

  return result;
}

ReturnValue_t PusTmFunnel::initialize() {
  tmPool = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
  if (tmPool == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "TmFunnel::initialize: TM store not set." << std::endl;
    sif::error << "Make sure the tm store is set up properly and implements "
                  "StorageManagerIF"
               << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  return SystemObject::initialize();
}

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