#include "LiveTmTask.h"

#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>

LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel,
                       VirtualChannelWithQueue& channel)
    : SystemObject(objectId), pusFunnel(pusFunnel), cfdpFunnel(cfdpFunnel), channel(channel) {}

ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
  while (true) {
    // The funnel tasks are scheduled here directly as well.
    ReturnValue_t result = channel.sendNextTm();
    if (result == DirectTmSinkIF::IS_BUSY) {
      sif::error << "Lost live TM, PAPB busy" << std::endl;
    }
    if (result == MessageQueueIF::EMPTY) {
      if (tmFunnelCd.hasTimedOut()) {
        pusFunnel.performOperation(0);
        cfdpFunnel.performOperation(0);
        tmFunnelCd.resetTimer();
      }
      // 40 ms IDLE delay. Might tweak this in the future.
      TaskFactory::delayTask(40);
    }
  }
}