#include "PusTmFunnel.h"

#include <limits>

#include "eive/definitions.h"
#include "eive/objects.h"
#include "fsfw/ipc/CommandMessage.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager.h"
#include "fsfw/pus/Service5EventReporting.h"
#include "fsfw/tmstorage/TmStoreMessage.h"
#include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h"
#include "tmtc/pusIds.h"

PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore,
                         TimeReaderIF &timeReader)
    : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader) {}

PusTmFunnel::~PusTmFunnel() = default;

ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
  CommandMessage cmdMessage;
  ReturnValue_t result;
  TmTcMessage currentMessage;
  unsigned int count = 0;
  if (saveSequenceCount) {
    result = saveSequenceCountToFile();
    if (result != returnvalue::OK) {
      sif::error << "PusTmFunnel: Storing sequence count to file has failed" << std::endl;
    }
    saveSequenceCount = false;
  }
  result = tmQueue->receiveMessage(&currentMessage);
  while (result == returnvalue::OK) {
    result = handleTmPacket(currentMessage);
    if (result != returnvalue::OK) {
      sif::warning << "TmFunnel packet handling failed" << std::endl;
      break;
    }
    count++;
    if (count == 1000) {
      sif::error << "PusTmFunnel: Possible message storm detected" << std::endl;
      break;
    }
    result = tmQueue->receiveMessage(&currentMessage);
  }

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

ReturnValue_t PusTmFunnel::handleTmPacket(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++);
  // NOTE: This only works because the limit value bit width is below 16 bits!
  sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;

  // Message type counter handling.
  uint8_t service = packet.getService();
  bool insertionFailed = false;
  auto mapIter = msgCounterMap.find(service);
  if (mapIter == msgCounterMap.end()) {
    auto iterPair = msgCounterMap.emplace(service, 0);
    if (iterPair.second) {
      mapIter = iterPair.first;
    } else {
      // Should really never never happen but you never know..
      insertionFailed = true;
    }
  }
  if (not insertionFailed) {
    packet.setMessageCount(mapIter->second);
    // Sane overflow handling.
    if (mapIter->second == std::numeric_limits<uint16_t>::max()) {
      mapIter->second = 0;
    } else {
      mapIter->second++;
    }
  }

  // Re-calculate CRC after changing the fields. This operation HAS to come last!
  packet.updateErrorControl();

  // Send to persistent TM store if the packet matches some filter.
  MessageQueueId_t destination;
  if (persistentTmMap.packetMatches(packet, destination)) {
    store_address_t storageId;
    result = ramToFileStore.addData(&storageId, packetData, size);
    TmTcMessage msg(storageId);
    if (result != returnvalue::OK) {
      sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
    } else {
      tmQueue->sendMessage(destination, &msg);
    }
  }

  // Send to registered live targets.
  return demultiplexLivePackets(origStoreId, packetData, size);
}

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

void PusTmFunnel::addPersistentTmStoreRouting(PusPacketFilter filter, MessageQueueId_t dest) {
  persistentTmMap.addRouting(filter, dest);
}