#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <mission/tmtc/PersistentSingleTmStoreTask.h>
#include <unistd.h>

PersistentSingleTmStoreTask::PersistentSingleTmStoreTask(
    object_id_t objectId, StorageManagerIF& ipcStore, PersistentTmStoreWithTmQueue& tmStore,
    VirtualChannel& channel, Event eventIfDumpDone, Event eventIfCancelled, SdCardMountedIF& sdcMan)
    : TmStoreTaskBase(objectId, ipcStore, channel, sdcMan),
      storeWithQueue(tmStore),
      dumpContext(eventIfDumpDone, eventIfCancelled) {}

ReturnValue_t PersistentSingleTmStoreTask::performOperation(uint8_t opCode) {
  ReturnValue_t result = returnvalue::OK;
  uint32_t delaysVcBusyDuringDump = 0;
  uint32_t delaysFlowControl = 0;
  uint32_t delayNotBusy = 0;
  uint32_t delayHotLoop = 0;
  while (true) {
    // Delay done by the check
    if (not cyclicStoreCheck()) {
      continue;
    }
    bool busy = handleOneStore(storeWithQueue, dumpContext);
    if (not busy) {
      result = TaskFactory::delayTask(100);
      delayNotBusy++;
    } else if (dumpContext.vcBusyDuringDump) {
      delaysVcBusyDuringDump++;
      result = TaskFactory::delayTask(10);
    } else if (dumpContext.packetWasDumped and
               dumpContext.dumpedBytes - dumpContext.bytesDumpedAtLastDelay >= 2048) {
      dumpContext.bytesDumpedAtLastDelay = dumpContext.dumpedBytes;
      result = TaskFactory::delayTask(10);
      delaysFlowControl++;
    } else {
      // TODO: Lower this as much as possible...
      result = TaskFactory::delayTask(5);
      delayHotLoop++;
    }
    if ((delaysVcBusyDuringDump + delaysFlowControl + delayNotBusy + delayHotLoop) % 2000000 == 0) {
      sif::debug << "DLY NBUSY: " << delayNotBusy << std::endl;
      sif::debug << "DLY FLCTRL: " << delaysFlowControl << std::endl;
      sif::debug << "DLY BUSYDUMP: " << delaysVcBusyDuringDump << std::endl;
      sif::debug << "DLY HOTLOOP: " << delayHotLoop << std::endl;
    }
    if (result != returnvalue::OK) {
      sif::warning << "TmStoreTask: Delay failed" << std::endl;
    }
  }
}

bool PersistentSingleTmStoreTask::initStoresIfPossible() {
  if (sdcMan.isSdCardUsable(std::nullopt)) {
    storeWithQueue.initializeTmStore();
    return true;
  }
  return false;
}