#include "LiveTmTask.h"

#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/subsystem/helper.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>

LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel,
                       VirtualChannelWithQueue& channel, const std::atomic_bool& ptmeLocked)
    : SystemObject(objectId),
      modeHelper(this),
      pusFunnel(pusFunnel),
      cfdpFunnel(cfdpFunnel),
      channel(channel),
      ptmeLocked(ptmeLocked) {
  requestQueue = QueueFactory::instance()->createMessageQueue();
}

ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
  readCommandQueue();
  while (true) {
    // The funnel tasks are scheduled here directly as well.
    ReturnValue_t result = channel.handleNextTm(!ptmeLocked);
    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();
      }
      // Read command queue during idle times.
      readCommandQueue();
      // 40 ms IDLE delay. Might tweak this in the future.
      TaskFactory::delayTask(40);
    } else {
      packetCounter++;
    }
  }
}

MessageQueueId_t LiveTmTask::getCommandQueue() const { return requestQueue->getId(); }

void LiveTmTask::getMode(Mode_t* mode, Submode_t* submode) {
  if (mode != nullptr) {
    *mode = this->mode;
  }
  if (submode != nullptr) {
    *submode = SUBMODE_NONE;
  }
}

ReturnValue_t LiveTmTask::checkModeCommand(Mode_t mode, Submode_t submode,
                                           uint32_t* msToReachTheMode) {
  if (mode == MODE_ON or mode == MODE_OFF) {
    return returnvalue::OK;
  }
  return returnvalue::FAILED;
}

void LiveTmTask::startTransition(Mode_t mode, Submode_t submode) {
  this->mode = mode;
  modeHelper.modeChanged(mode, submode);
  announceMode(false);
}

void LiveTmTask::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, SUBMODE_NONE); }

object_id_t LiveTmTask::getObjectId() const { return SystemObject::getObjectId(); }

const HasHealthIF* LiveTmTask::getOptHealthIF() const { return nullptr; }

const HasModesIF& LiveTmTask::getModeIF() const { return *this; }

ReturnValue_t LiveTmTask::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
  return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
}

void LiveTmTask::readCommandQueue(void) {
  CommandMessage commandMessage;
  ReturnValue_t result = returnvalue::FAILED;

  result = requestQueue->receiveMessage(&commandMessage);
  if (result == returnvalue::OK) {
    result = modeHelper.handleModeCommand(&commandMessage);
    if (result == returnvalue::OK) {
      return;
    }
    CommandMessage reply;
    reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, commandMessage.getCommand());
    requestQueue->reply(&reply);
    return;
  }
}

ModeTreeChildIF& LiveTmTask::getModeTreeChildIF() { return *this; }

ReturnValue_t LiveTmTask::initialize() {
  modeHelper.initialize();
  return returnvalue::OK;
}