#include "PlocSupervisorHandler.h"

#include <fsfw/filesystem/HasFileSystemIF.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/tasks/TaskFactory.h>

#include <filesystem>
#include <fstream>
#include <sstream>
#include <string>

#include "OBSWConfig.h"
#include "eive/definitions.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/globalfunctions/CRC.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/timemanager/Clock.h"

using namespace supv;
using namespace returnvalue;

PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, CookieIF* comCookie,
                                             Gpio uartIsolatorSwitch, power::Switch_t powerSwitch,
                                             PlocSupvUartManager& supvHelper)
    : DeviceHandlerBase(objectId, supvHelper.getObjectId(), comCookie),
      uartIsolatorSwitch(uartIsolatorSwitch),
      hkset(this),
      bootStatusReport(this),
      latchupStatusReport(this),
      loggingReport(this),
      adcReport(this),
      powerSwitch(powerSwitch),
      uartManager(supvHelper) {
  if (comCookie == nullptr) {
    sif::error << "PlocSupervisorHandler: Invalid com cookie" << std::endl;
  }
  spParams.buf = commandBuffer;
  spParams.maxSize = sizeof(commandBuffer);
  eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
}

PlocSupervisorHandler::~PlocSupervisorHandler() {}

ReturnValue_t PlocSupervisorHandler::initialize() {
  ReturnValue_t result = returnvalue::OK;
  result = DeviceHandlerBase::initialize();
  if (result != returnvalue::OK) {
    return result;
  }
#ifdef XIPHOS_Q7S
  sdcMan = SdCardManager::instance();
#endif /* TE0720_1CFA */

  result = eventSubscription();
  if (result != returnvalue::OK) {
    return result;
  }
  return result;
}

void PlocSupervisorHandler::performOperationHook() {
  EventMessage event;
  for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
       result = eventQueue->receiveMessage(&event)) {
    switch (event.getMessageId()) {
      case EventMessage::EVENT_MESSAGE:
        handleEvent(&event);
        break;
      default:
        sif::debug << "PlocSupervisorHandler::performOperationHook: Did not subscribe to this event"
                   << " message" << std::endl;
        break;
    }
  }
}

ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId,
                                                   MessageQueueId_t commandedBy,
                                                   const uint8_t* data, size_t size) {
  using namespace supv;
  ReturnValue_t result = returnvalue::OK;

  switch (actionId) {
    default:
      break;
  }

  if (uartManager.longerRequestActive()) {
    return result::SUPV_HELPER_EXECUTING;
  }

  result = acceptExternalDeviceCommands();
  if (result != returnvalue::OK) {
    return result;
  }

  switch (actionId) {
    case PERFORM_UPDATE: {
      if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
        return result::FILENAME_TOO_LONG;
      }
      shutdownCmdSent = false;
      UpdateParams params;
      result = extractUpdateCommand(data, size, params);
      if (result != returnvalue::OK) {
        return result;
      }
      result = uartManager.performUpdate(params);
      if (result != returnvalue::OK) {
        return result;
      }
      return EXECUTION_FINISHED;
    }
    case CONTINUE_UPDATE: {
      shutdownCmdSent = false;
      uartManager.initiateUpdateContinuation();
      return EXECUTION_FINISHED;
    }
    case MEMORY_CHECK_WITH_FILE: {
      shutdownCmdSent = false;
      UpdateParams params;
      result = extractBaseParams(&data, size, params);
      if (result != returnvalue::OK) {
        return result;
      }
      if (not std::filesystem::exists(params.file)) {
        return HasFileSystemIF::FILE_DOES_NOT_EXIST;
      }
      uartManager.performMemCheck(params.file, params.memId, params.startAddr);
      return EXECUTION_FINISHED;
    }
    default:
      break;
  }
  return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
}

void PlocSupervisorHandler::doStartUp() {
  if (startupState == StartupState::OFF) {
    bootTimeout.resetTimer();
    startupState = StartupState::BOOTING;
  }
  if (startupState == StartupState::BOOTING) {
    if (bootTimeout.hasTimedOut()) {
      uartIsolatorSwitch.pullHigh();
      uartManager.start();
      if (SET_TIME_DURING_BOOT) {
        startupState = StartupState::SET_TIME;
      } else {
        startupState = StartupState::ON;
      }
    }
  }
  if (startupState == StartupState::TIME_WAS_SET) {
    startupState = StartupState::ON;
  }
  if (startupState == StartupState::ON) {
    hkset.setReportingEnabled(true);
    setMode(_MODE_TO_ON);
  }
}

void PlocSupervisorHandler::doShutDown() {
  setMode(_MODE_POWER_DOWN);
  hkset.setReportingEnabled(false);
  hkset.setValidity(false, true);
  shutdownCmdSent = false;
  packetInBuffer = false;
  nextReplyId = supv::NONE;
  uartManager.stop();
  uartIsolatorSwitch.pullLow();
  startupState = StartupState::OFF;
}

ReturnValue_t PlocSupervisorHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  if (not commandIsExecuting(GET_HK_REPORT)) {
    *id = GET_HK_REPORT;
    return buildCommandFromCommand(*id, nullptr, 0);
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t PlocSupervisorHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  if (startupState == StartupState::SET_TIME) {
    *id = supv::SET_TIME_REF;
    startupState = StartupState::WAIT_FOR_TIME_REPLY;
    return buildCommandFromCommand(*id, nullptr, 0);
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                             const uint8_t* commandData,
                                                             size_t commandDataLen) {
  using namespace supv;
  ReturnValue_t result = returnvalue::FAILED;
  spParams.buf = commandBuffer;
  switch (deviceCommand) {
    case GET_HK_REPORT: {
      prepareEmptyCmd(Apid::HK, static_cast<uint8_t>(tc::HkId::GET_REPORT));
      result = returnvalue::OK;
      break;
    }
    case START_MPSOC: {
      prepareEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::START_MPSOC));
      result = returnvalue::OK;
      break;
    }
    case SHUTDOWN_MPSOC: {
      prepareEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::SHUTDOWN_MPSOC));
      result = returnvalue::OK;
      break;
    }
    case SEL_MPSOC_BOOT_IMAGE: {
      prepareSelBootImageCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case RESET_MPSOC: {
      prepareEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::RESET_MPSOC));
      result = returnvalue::OK;
      break;
    }
    case SET_TIME_REF: {
      result = prepareSetTimeRefCmd();
      break;
    }
    case SET_BOOT_TIMEOUT: {
      prepareSetBootTimeoutCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case SET_MAX_RESTART_TRIES: {
      prepareRestartTriesCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case DISABLE_PERIOIC_HK_TRANSMISSION: {
      prepareDisableHk();
      result = returnvalue::OK;
      break;
    }
    case GET_BOOT_STATUS_REPORT: {
      prepareEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::GET_BOOT_STATUS_REPORT));
      result = returnvalue::OK;
      break;
    }
    case ENABLE_LATCHUP_ALERT: {
      result = prepareLatchupConfigCmd(commandData, deviceCommand);
      break;
    }
    case DISABLE_LATCHUP_ALERT: {
      result = prepareLatchupConfigCmd(commandData, deviceCommand);
      break;
    }
    case SET_ALERT_LIMIT: {
      result = prepareSetAlertLimitCmd(commandData);
      break;
    }
    case GET_LATCHUP_STATUS_REPORT: {
      prepareEmptyCmd(Apid::LATCHUP_MON, static_cast<uint8_t>(tc::LatchupMonId::GET_STATUS_REPORT));
      result = returnvalue::OK;
      break;
    }
    case RUN_AUTO_EM_TESTS: {
      result = prepareRunAutoEmTest(commandData);
      break;
    }
    case SET_GPIO: {
      prepareSetGpioCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case FACTORY_RESET: {
      result = prepareFactoryResetCmd(commandData, commandDataLen);
      break;
    }
    case READ_GPIO: {
      prepareReadGpioCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case SET_SHUTDOWN_TIMEOUT: {
      prepareSetShutdownTimeoutCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case FACTORY_FLASH: {
      prepareEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::FACTORY_FLASH));
      result = returnvalue::OK;
      break;
    }
    case RESET_PL: {
      prepareEmptyCmd(Apid::BOOT_MAN, static_cast<uint8_t>(tc::BootManId::RESET_PL));
      result = returnvalue::OK;
      break;
    }
    case SET_ADC_ENABLED_CHANNELS: {
      prepareSetAdcEnabledChannelsCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case SET_ADC_WINDOW_AND_STRIDE: {
      prepareSetAdcWindowAndStrideCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case SET_ADC_THRESHOLD: {
      prepareSetAdcThresholdCmd(commandData);
      result = returnvalue::OK;
      break;
    }
    case WIPE_MRAM: {
      result = prepareWipeMramCmd(commandData);
      break;
    }
      //    case ENABLE_NVMS: {
      //      result = prepareEnableNvmsCommand(commandData);
      //      break;
      //    }
      //    case RESTART_SUPERVISOR: {
      //      prepareEmptyCmd(APID_RESTART_SUPERVISOR);
      //      result = returnvalue::OK;
      //      break;
      //    }
      // Removed command
      //    case START_MPSOC_QUIET: {
      //      prepareEmptyCmd(APID_START_MPSOC_QUIET);
      //      result = returnvalue::OK;
      //      break;
      //    }
      //    case ENABLE_AUTO_TM: {
      //      EnableAutoTm packet(spParams);
      //      result = packet.buildPacket();
      //      if (result != returnvalue::OK) {
      //        break;
      //      }
      //      finishTcPrep(packet.getFullPacketLen());
      //      break;
      //    }
      //    case DISABLE_AUTO_TM: {
      //      DisableAutoTm packet(spParams);
      //      result = packet.buildPacket();
      //      if (result != returnvalue::OK) {
      //        break;
      //      }
      //      finishTcPrep(packet.getFullPacketLen());
      //      break;
      //    }
      //    case LOGGING_REQUEST_COUNTERS: {
      //      RequestLoggingData packet(spParams);
      //      result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_COUNTERS);
      //      if (result != returnvalue::OK) {
      //        break;
      //      }
      //      finishTcPrep(packet.getFullPacketLen());
      //      break;
      //    }
      //    case LOGGING_CLEAR_COUNTERS: {
      //      RequestLoggingData packet(spParams);
      //      result = packet.buildPacket(RequestLoggingData::Sa::CLEAR_COUNTERS);
      //      if (result != returnvalue::OK) {
      //        break;
      //      }
      //      finishTcPrep(packet.getFullPacketLen());
      //      break;
      //    }
      //    case LOGGING_SET_TOPIC: {
      //      if (commandData == nullptr or commandDataLen == 0) {
      //        return HasActionsIF::INVALID_PARAMETERS;
      //      }
      //      uint8_t tpc = *(commandData);
      //      RequestLoggingData packet(spParams);
      //      result = packet.buildPacket(RequestLoggingData::Sa::SET_LOGGING_TOPIC, tpc);
      //      if (result != returnvalue::OK) {
      //        break;
      //      }
      //      finishTcPrep(packet.getFullPacketLen());
      //      break;
      //    }
      // I think this is disabled right now according to the TC excel table
      //    case COPY_ADC_DATA_TO_MRAM: {
      //      prepareEmptyCmd(APID_COPY_ADC_DATA_TO_MRAM);
      //      result = returnvalue::OK;
      //      break;
      //    }
      //    case REQUEST_ADC_REPORT: {
      //      prepareEmptyCmd(APID_REQUEST_ADC_REPORT);
      //      result = returnvalue::OK;
      //      break;
      //    }
      //    case FIRST_MRAM_DUMP:
      //    case CONSECUTIVE_MRAM_DUMP:
      //      result = prepareDumpMramCmd(commandData);
      //      break;
    default:
      sif::debug << "PlocSupervisorHandler::buildCommandFromCommand: Command not implemented"
                 << std::endl;
      result = DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
      break;
  }
  return result;
}

void PlocSupervisorHandler::fillCommandAndReplyMap() {
  // Command only
  insertInCommandMap(GET_HK_REPORT);
  insertInCommandMap(START_MPSOC);
  insertInCommandMap(SHUTDOWN_MPSOC);
  insertInCommandMap(SEL_MPSOC_BOOT_IMAGE);
  insertInCommandMap(SET_BOOT_TIMEOUT);
  insertInCommandMap(SET_MAX_RESTART_TRIES);
  insertInCommandMap(RESET_MPSOC);
  insertInCommandMap(WIPE_MRAM);
  insertInCommandMap(SET_TIME_REF);
  insertInCommandMap(DISABLE_PERIOIC_HK_TRANSMISSION);
  insertInCommandMap(GET_BOOT_STATUS_REPORT);
  insertInCommandMap(ENABLE_LATCHUP_ALERT);
  insertInCommandMap(DISABLE_LATCHUP_ALERT);
  insertInCommandMap(SET_ALERT_LIMIT);
  insertInCommandMap(GET_LATCHUP_STATUS_REPORT);
  insertInCommandMap(RUN_AUTO_EM_TESTS);
  insertInCommandMap(SET_GPIO);
  insertInCommandMap(READ_GPIO);
  insertInCommandMap(FACTORY_RESET);
  insertInCommandMap(MEMORY_CHECK);
  insertInCommandMap(SET_SHUTDOWN_TIMEOUT);
  insertInCommandMap(FACTORY_FLASH);
  insertInCommandMap(SET_ADC_ENABLED_CHANNELS);
  insertInCommandMap(SET_ADC_THRESHOLD);
  insertInCommandMap(SET_ADC_WINDOW_AND_STRIDE);
  insertInCommandMap(RESET_PL);

  // ACK replies, use countdown for them
  insertInReplyMap(ACK_REPORT, 0, nullptr, SIZE_ACK_REPORT, false, &acknowledgementReportTimeout);
  insertInReplyMap(EXE_REPORT, 0, nullptr, SIZE_EXE_REPORT, false, &executionReportTimeout);
  insertInReplyMap(MEMORY_CHECK, 5, nullptr, 0, false);

  // TM replies
  insertInReplyMap(HK_REPORT, 3, &hkset, SIZE_HK_REPORT);
  insertInReplyMap(BOOT_STATUS_REPORT, 3, &bootStatusReport, SIZE_BOOT_STATUS_REPORT);
  insertInReplyMap(LATCHUP_REPORT, 3, &latchupStatusReport, SIZE_LATCHUP_STATUS_REPORT);
  insertInReplyMap(LOGGING_REPORT, 3, &loggingReport, SIZE_LOGGING_REPORT);
  insertInReplyMap(ADC_REPORT, 3, &adcReport, SIZE_ADC_REPORT);
}

ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command,
                                                           uint8_t expectedReplies,
                                                           bool useAlternateId,
                                                           DeviceCommandId_t alternateReplyID) {
  ReturnValue_t result = OK;

  uint8_t enabledReplies = 0;

  switch (command->first) {
    case GET_HK_REPORT: {
      enabledReplies = 3;
      result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, HK_REPORT);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << HK_REPORT
                   << " not in replyMap" << std::endl;
      }
      break;
    }
    case GET_BOOT_STATUS_REPORT: {
      enabledReplies = 3;
      result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true,
                                                        BOOT_STATUS_REPORT);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id "
                   << BOOT_STATUS_REPORT << " not in replyMap" << std::endl;
      }
      break;
    }
    case GET_LATCHUP_STATUS_REPORT: {
      enabledReplies = 3;
      result =
          DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, LATCHUP_REPORT);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id "
                   << LATCHUP_REPORT << " not in replyMap" << std::endl;
      }
      break;
    }
    case LOGGING_REQUEST_COUNTERS: {
      enabledReplies = 3;
      result =
          DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, LOGGING_REPORT);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id "
                   << LOGGING_REPORT << " not in replyMap" << std::endl;
      }
      break;
    }
    case REQUEST_ADC_REPORT: {
      enabledReplies = 3;
      result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, ADC_REPORT);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << ADC_REPORT
                   << " not in replyMap" << std::endl;
      }
      break;
    }
    case FIRST_MRAM_DUMP: {
      enabledReplies = 2;  // expected replies will be increased in handleMramDumpPacket
      result =
          DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, FIRST_MRAM_DUMP);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id "
                   << FIRST_MRAM_DUMP << " not in replyMap" << std::endl;
      }
      break;
    }
    case CONSECUTIVE_MRAM_DUMP: {
      enabledReplies = 2;  // expected replies will be increased in handleMramDumpPacket
      result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true,
                                                        CONSECUTIVE_MRAM_DUMP);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id "
                   << CONSECUTIVE_MRAM_DUMP << " not in replyMap" << std::endl;
      }
      break;
    }
    case MEMORY_CHECK: {
      enabledReplies = 3;
      result =
          DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, MEMORY_CHECK);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << MEMORY_CHECK
                   << " not in replyMap" << std::endl;
      }
      break;
    }
    case START_MPSOC:
    case SHUTDOWN_MPSOC:
    case SEL_MPSOC_BOOT_IMAGE:
    case SET_BOOT_TIMEOUT:
    case SET_MAX_RESTART_TRIES:
    case RESET_MPSOC:
    case SET_TIME_REF:
    case ENABLE_LATCHUP_ALERT:
    case DISABLE_LATCHUP_ALERT:
    case SET_ALERT_LIMIT:
    case SET_ADC_ENABLED_CHANNELS:
    case SET_ADC_WINDOW_AND_STRIDE:
    case SET_ADC_THRESHOLD:
    // case COPY_ADC_DATA_TO_MRAM:
    case RUN_AUTO_EM_TESTS:
    case WIPE_MRAM:
    case SET_GPIO:
    case FACTORY_RESET:
    case READ_GPIO:
    // case RESTART_SUPERVISOR:
    case DISABLE_PERIOIC_HK_TRANSMISSION:
    // case START_MPSOC_QUIET:
    case SET_SHUTDOWN_TIMEOUT:
    case FACTORY_FLASH:
    case ENABLE_AUTO_TM:
    case DISABLE_AUTO_TM:
    // case LOGGING_CLEAR_COUNTERS:
    // case LOGGING_SET_TOPIC:
    case RESET_PL:
      // case ENABLE_NVMS:
      enabledReplies = 2;
      break;
    default:
      sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Unknown command id" << std::endl;
      break;
  }

  /**
   * Every command causes at least one acknowledgment and one execution report. Therefore both
   * replies will be enabled here.
   */
  result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, ACK_REPORT);
  if (result != returnvalue::OK) {
    sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << ACK_REPORT
               << " not in replyMap" << std::endl;
  }

  setExecutionTimeout(command->first);

  result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, EXE_REPORT);
  if (result != returnvalue::OK) {
    sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << EXE_REPORT
               << " not in replyMap" << std::endl;
  }

  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                                  DeviceCommandId_t* foundId, size_t* foundLen) {
  using namespace supv;
  // TODO: Is this still required?
  //  if (nextReplyId == FIRST_MRAM_DUMP) {
  //    *foundId = FIRST_MRAM_DUMP;
  //    return parseMramPackets(start, remainingSize, foundLen);
  //  } else if (nextReplyId == CONSECUTIVE_MRAM_DUMP) {
  //    *foundId = CONSECUTIVE_MRAM_DUMP;
  //    return parseMramPackets(start, remainingSize, foundLen);
  //  }

  tmReader.setData(start, remainingSize);
  // sif::debug << "PlocSupervisorHandler::scanForReply: Received Packet" << std::endl;
  // arrayprinter::print(start, remainingSize);
  uint16_t apid = tmReader.getModuleApid();

  switch (apid) {
    case (Apid::TMTC_MAN): {
      switch (tmReader.getServiceId()) {
        case (static_cast<uint8_t>(supv::tm::TmtcId::ACK)):
        case (static_cast<uint8_t>(supv::tm::TmtcId::NAK)): {
          *foundLen = tmReader.getFullPacketLen();
          *foundId = ReplyId::ACK_REPORT;
          return OK;
        }
        case (static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)):
        case (static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)): {
          *foundLen = tmReader.getFullPacketLen();
          *foundId = EXE_REPORT;
          return OK;
        }
      }
      break;
    }
    case (Apid::HK): {
      if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::HkId::REPORT)) {
        *foundLen = tmReader.getFullPacketLen();
        *foundId = ReplyId::HK_REPORT;
        return OK;
      } else if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::HkId::HARDFAULTS)) {
        handleBadApidServiceCombination(SUPV_UNINIMPLEMENTED_TM, apid, tmReader.getServiceId());
        return INVALID_DATA;
      }
      break;
    }
    case (Apid::BOOT_MAN): {
      if (tmReader.getServiceId() ==
          static_cast<uint8_t>(supv::tm::BootManId::BOOT_STATUS_REPORT)) {
        *foundLen = tmReader.getFullPacketLen();
        *foundId = ReplyId::BOOT_STATUS_REPORT;
        return OK;
      }
      break;
    }
    case (Apid::MEM_MAN): {
      if (tmReader.getServiceId() ==
          static_cast<uint8_t>(supv::tm::MemManId::UPDATE_STATUS_REPORT)) {
        *foundLen = tmReader.getFullPacketLen();
        *foundId = ReplyId::UPDATE_STATUS_REPORT;
        return OK;
      }
    }
  }
  handleBadApidServiceCombination(SUPV_UNKNOWN_TM, apid, tmReader.getServiceId());
  *foundLen = remainingSize;
  return INVALID_DATA;
}

ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                          const uint8_t* packet) {
  using namespace supv;
  ReturnValue_t result = returnvalue::OK;

  switch (id) {
    case ACK_REPORT: {
      result = handleAckReport(packet);
      break;
    }
    case (HK_REPORT): {
      result = handleHkReport(packet);
      break;
    }
    case (BOOT_STATUS_REPORT): {
      result = handleBootStatusReport(packet);
      break;
    }
    case (LATCHUP_REPORT): {
      result = handleLatchupStatusReport(packet);
      break;
    }
    case (ADC_REPORT): {
      result = handleAdcReport(packet);
      break;
    }
    case (EXE_REPORT): {
      result = handleExecutionReport(packet);
      break;
    }
    case (UPDATE_STATUS_REPORT): {
      // TODO: handle status report here
      break;
    }
    default: {
      sif::debug << "PlocSupervisorHandler::interpretDeviceReply: Unknown device reply id"
                 << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }

  return result;
}

ReturnValue_t PlocSupervisorHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                             LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(supv::NUM_TMS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::TEMP_PS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::TEMP_PL, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::HK_SOC_STATE, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::NVM0_1_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::NVM3_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::MISSION_IO_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::FMC_STATE, &fmcStateEntry);
  localDataPoolMap.emplace(supv::NUM_TCS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::TEMP_SUP, &tempSupEntry);
  localDataPoolMap.emplace(supv::UPTIME, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(supv::CPULOAD, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::AVAILABLEHEAP, new PoolEntry<uint32_t>({0}));

  localDataPoolMap.emplace(supv::BR_SOC_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::POWER_CYCLES, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::BOOT_AFTER_MS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::BOOT_TIMEOUT_MS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::ACTIVE_NVM, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::BP0_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::BP1_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::BP2_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::BOOT_STATE, &bootStateEntry);
  localDataPoolMap.emplace(supv::BOOT_CYCLES, &bootCyclesEntry);

  localDataPoolMap.emplace(supv::LATCHUP_ID, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::CNT0, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::CNT1, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::CNT2, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::CNT3, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::CNT4, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::CNT5, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::CNT6, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_MSEC, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_SEC, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_MIN, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_HOUR, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_DAY, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_MON, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_YEAR, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_RPT_IS_SET, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_0, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_2, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_3, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_4, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_5, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_6, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::ADC_DEVIATION_TRIGGERS_CNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::TC_RECEIVED_CNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::TM_AVAILABLE_CNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::SUPERVISOR_BOOTS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::MPSOC_BOOTS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::MPSOC_BOOT_FAILED_ATTEMPTS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::MPSOC_POWER_UP, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::MPSOC_UPDATES, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::LAST_RECVD_TC, new PoolEntry<uint32_t>({0}));

  localDataPoolMap.emplace(supv::ADC_RAW_0, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_1, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_2, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_3, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_4, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_5, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_6, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_7, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_8, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_9, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_10, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_11, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_12, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_13, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_14, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_RAW_15, new PoolEntry<uint16_t>({0}));

  localDataPoolMap.emplace(supv::ADC_ENG_0, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_1, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_2, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_3, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_4, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_5, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_6, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_7, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_8, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_9, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_10, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_11, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_12, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_13, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_14, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(supv::ADC_ENG_15, new PoolEntry<uint16_t>({0}));

  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(hkset.getSid(), false, 10.0));
  return returnvalue::OK;
}

void PlocSupervisorHandler::handleEvent(EventMessage* eventMessage) {
  ReturnValue_t result = returnvalue::OK;
  object_id_t objectId = eventMessage->getReporter();
  Event event = eventMessage->getEvent();
  switch (objectId) {
    case objects::PLOC_SUPERVISOR_HELPER: {
      // After execution of update procedure, PLOC is in a state where it draws approx. 700 mA of
      // current. To leave this state the shutdown MPSoC command must be sent here.
      if (event == PlocSupvUartManager::SUPV_UPDATE_FAILED ||
          event == PlocSupvUartManager::SUPV_UPDATE_SUCCESSFUL ||
          event == PlocSupvUartManager::SUPV_CONTINUE_UPDATE_FAILED ||
          event == PlocSupvUartManager::SUPV_CONTINUE_UPDATE_SUCCESSFUL ||
          event == PlocSupvUartManager::SUPV_MEM_CHECK_FAIL ||
          event == PlocSupvUartManager::SUPV_MEM_CHECK_OK) {
        // Wait for a short period for the uart state machine to adjust
        // TaskFactory::delayTask(5);
        if (not shutdownCmdSent) {
          shutdownCmdSent = true;
          result = this->executeAction(supv::SHUTDOWN_MPSOC, NO_COMMANDER, nullptr, 0);
          if (result != returnvalue::OK) {
            triggerEvent(SUPV_MPSOC_SHUTDOWN_BUILD_FAILED);
            sif::warning << "PlocSupervisorHandler::handleEvent: Failed to build MPSoC shutdown "
                            "command"
                         << std::endl;
            return;
          }
        }
      }
      break;
    }
    default:
      sif::debug << "PlocMPSoCHandler::handleEvent: Did not subscribe to this event" << std::endl;
      break;
  }
}

void PlocSupervisorHandler::setExecutionTimeout(DeviceCommandId_t command) {
  using namespace supv;
  switch (command) {
    case FIRST_MRAM_DUMP:
    case CONSECUTIVE_MRAM_DUMP:
      executionReportTimeout.setTimeout(MRAM_DUMP_EXECUTION_TIMEOUT);
      break;
    case COPY_ADC_DATA_TO_MRAM:
      executionReportTimeout.setTimeout(COPY_ADC_TO_MRAM_TIMEOUT);
      break;
    default:
      executionReportTimeout.setTimeout(EXECUTION_DEFAULT_TIMEOUT);
      break;
  }
}

ReturnValue_t PlocSupervisorHandler::verifyPacket(const uint8_t* start, size_t foundLen) {
  if (CRC::crc16ccitt(start, foundLen) != 0) {
    return result::CRC_FAILURE;
  }
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) {
  using namespace supv;
  ReturnValue_t result = returnvalue::OK;

  if (not tmReader.verifyCrc()) {
    sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl;
    nextReplyId = supv::NONE;
    replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT);
    triggerEvent(SUPV_CRC_FAILURE_EVENT);
    sendFailureReport(supv::ACK_REPORT, result::CRC_FAILURE);
    disableAllReplies();
    return returnvalue::OK;
  }
  AcknowledgmentReport ack(tmReader);
  result = ack.parse();
  if (result != returnvalue::OK) {
    nextReplyId = supv::NONE;
    replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT);
    triggerEvent(SUPV_CRC_FAILURE_EVENT);
    sendFailureReport(supv::ACK_REPORT, result);
    disableAllReplies();
    return result;
  }
  if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::NAK)) {
    DeviceCommandId_t commandId = getPendingCommand();
    if (commandId != DeviceHandlerIF::NO_COMMAND_ID) {
      triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast<uint32_t>(ack.getStatusCode()));
    }
    ack.printStatusInformation();
    printAckFailureInfo(ack.getStatusCode(), commandId);
    sendFailureReport(supv::ACK_REPORT, result::RECEIVED_ACK_FAILURE);
    disableAllReplies();
    nextReplyId = supv::NONE;
    result = IGNORE_REPLY_DATA;
  } else if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::ACK)) {
    setNextReplyId();
  }
  return result;
}

ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) {
  using namespace supv;
  ReturnValue_t result = returnvalue::OK;

  if (not tmReader.verifyCrc()) {
    nextReplyId = supv::NONE;
    return result::CRC_FAILURE;
  }
  ExecutionReport report(tmReader);
  result = report.parse();
  if (result != OK) {
    nextReplyId = supv::NONE;
    return result;
  }
  if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)) {
    result = handleExecutionSuccessReport(report);
  } else if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
    handleExecutionFailureReport(report);
  }
  nextReplyId = supv::NONE;
  return result;
}

ReturnValue_t PlocSupervisorHandler::handleHkReport(const uint8_t* data) {
  ReturnValue_t result = returnvalue::OK;

  result = verifyPacket(data, tmReader.getFullPacketLen());

  if (result == result::CRC_FAILURE) {
    sif::error << "PlocSupervisorHandler::handleHkReport: Hk report has invalid crc" << std::endl;
    return result;
  }

  uint16_t offset = supv::PAYLOAD_OFFSET;
  hkset.tempPs = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 |
                 *(data + offset + 3);
  offset += 4;
  hkset.tempPl = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 |
                 *(data + offset + 3);
  offset += 4;
  hkset.tempSup = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 |
                  *(data + offset + 3);
  offset += 4;
  size_t size = sizeof(hkset.uptime.value);
  result = SerializeAdapter::deSerialize(&hkset.uptime, data + offset, &size,
                                         SerializeIF::Endianness::BIG);
  offset += 8;
  hkset.cpuLoad = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 |
                  *(data + offset + 3);
  offset += 4;
  hkset.availableHeap = *(data + offset) << 24 | *(data + offset + 1) << 16 |
                        *(data + offset + 2) << 8 | *(data + offset + 3);
  offset += 4;
  hkset.numTcs = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 |
                 *(data + offset + 3);
  offset += 4;
  hkset.numTms = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 |
                 *(data + offset + 3);
  offset += 4;
  hkset.socState = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 |
                   *(data + offset + 3);
  offset += 4;
  hkset.nvm0_1_state = *(data + offset);
  offset += 1;
  hkset.nvm3_state = *(data + offset);
  offset += 1;
  hkset.missionIoState = *(data + offset);
  offset += 1;
  hkset.fmcState = *(data + offset);
  offset += 1;

  nextReplyId = supv::EXE_REPORT;
  hkset.setValidity(true, true);

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
  sif::info << "PlocSupervisorHandler::handleHkReport: temp_ps: " << hkset.tempPs << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: temp_pl: " << hkset.tempPl << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: temp_sup: " << hkset.tempSup << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: uptime: " << hkset.uptime << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: cpu_load: " << hkset.cpuLoad << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: available_heap: " << hkset.availableHeap
            << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: num_tcs: " << hkset.numTcs << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: num_tms: " << hkset.numTms << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: soc_state: " << hkset.socState << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: nvm0_1_state: "
            << static_cast<unsigned int>(hkset.nvm0_1_state.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: nvm3_state: "
            << static_cast<unsigned int>(hkset.nvm3_state.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: mission_io_state: "
            << static_cast<unsigned int>(hkset.missionIoState.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleHkReport: fmc_state: "
            << static_cast<unsigned int>(hkset.fmcState.value) << std::endl;

#endif

  return result;
}

ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) {
  ReturnValue_t result = returnvalue::OK;

  result = verifyPacket(data, tmReader.getFullPacketLen());

  if (result == result::CRC_FAILURE) {
    sif::error << "PlocSupervisorHandler::handleBootStatusReport: Boot status report has invalid"
                  " crc"
               << std::endl;
    return result;
  }

  const uint8_t* payloadStart = tmReader.getPayloadStart();
  uint16_t offset = 0;
  bootStatusReport.socState = payloadStart[0];
  offset += 1;
  bootStatusReport.powerCycles = payloadStart[1];
  offset += 1;
  bootStatusReport.bootAfterMs = *(payloadStart + offset) << 24 |
                                 *(payloadStart + offset + 1) << 16 |
                                 *(payloadStart + offset + 2) << 8 | *(payloadStart + offset + 3);
  offset += 4;
  bootStatusReport.bootTimeoutMs = *(payloadStart + offset) << 24 |
                                   *(payloadStart + offset + 1) << 16 |
                                   *(payloadStart + offset + 2) << 8 | *(payloadStart + offset + 3);
  offset += 4;
  bootStatusReport.activeNvm = *(payloadStart + offset);
  offset += 1;
  bootStatusReport.bp0State = *(payloadStart + offset);
  offset += 1;
  bootStatusReport.bp1State = *(payloadStart + offset);
  offset += 1;
  bootStatusReport.bp2State = *(payloadStart + offset);
  offset += 1;
  bootStatusReport.bootState = *(payloadStart + offset);
  offset += 1;
  bootStatusReport.bootCycles = *(payloadStart + offset);

  nextReplyId = supv::EXE_REPORT;
  bootStatusReport.setValidity(true, true);

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: SoC State (0 - off, 1 - booting, 2 "
               "- Update, 3 "
               "- operating, 4 - Shutdown, 5 - Reset): "
            << static_cast<unsigned int>(bootStatusReport.socState.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: Power Cycles: "
            << static_cast<unsigned int>(bootStatusReport.powerCycles.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: BootAfterMs: "
            << bootStatusReport.bootAfterMs << " ms" << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: BootTimeoutMs: " << std::dec
            << bootStatusReport.bootTimeoutMs << " ms" << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: Active NVM: "
            << static_cast<unsigned int>(bootStatusReport.activeNvm.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: BP0: "
            << static_cast<unsigned int>(bootStatusReport.bp0State.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: BP1: "
            << static_cast<unsigned int>(bootStatusReport.bp1State.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: BP2: "
            << static_cast<unsigned int>(bootStatusReport.bp2State.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: Boot state: "
            << static_cast<unsigned int>(bootStatusReport.bootState.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleBootStatusReport: Boot cycles: "
            << static_cast<unsigned int>(bootStatusReport.bootCycles.value) << std::endl;
#endif

  return result;
}

ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* data) {
  ReturnValue_t result = returnvalue::OK;

  result = verifyPacket(data, tmReader.getFullPacketLen());

  if (result == result::CRC_FAILURE) {
    sif::error << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup status report has "
               << "invalid crc" << std::endl;
    return result;
  }

  const uint8_t* payloadData = tmReader.getPayloadStart();
  uint16_t offset = 0;
  latchupStatusReport.id = *(payloadData + offset);
  offset += 1;
  latchupStatusReport.cnt0 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
  offset += 2;
  latchupStatusReport.cnt1 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
  offset += 2;
  latchupStatusReport.cnt2 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
  offset += 2;
  latchupStatusReport.cnt3 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
  offset += 2;
  latchupStatusReport.cnt4 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
  offset += 2;
  latchupStatusReport.cnt5 = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
  offset += 2;
  latchupStatusReport.cnt6 = *(payloadData + offset) << 8 | *(data + offset + 1);
  offset += 2;
  uint16_t msec = *(payloadData + offset) << 8 | *(payloadData + offset + 1);
  latchupStatusReport.isSet = msec >> supv::LatchupStatusReport::IS_SET_BIT_POS;
  latchupStatusReport.timeMsec = msec & (~(1 << latchupStatusReport.IS_SET_BIT_POS));
  offset += 2;
  latchupStatusReport.timeSec = *(payloadData + offset);
  offset += 1;
  latchupStatusReport.timeMin = *(payloadData + offset);
  offset += 1;
  latchupStatusReport.timeHour = *(payloadData + offset);
  offset += 1;
  latchupStatusReport.timeDay = *(payloadData + offset);
  offset += 1;
  latchupStatusReport.timeMon = *(payloadData + offset);
  offset += 1;
  latchupStatusReport.timeYear = *(payloadData + offset);

  nextReplyId = supv::EXE_REPORT;

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup ID: "
            << static_cast<unsigned int>(latchupStatusReport.id.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: CNT0: "
            << latchupStatusReport.cnt0 << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: CNT1: "
            << latchupStatusReport.cnt1 << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: CNT2: "
            << latchupStatusReport.cnt2 << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: CNT3: "
            << latchupStatusReport.cnt3 << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: CNT4: "
            << latchupStatusReport.cnt4 << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: CNT5: "
            << latchupStatusReport.cnt5 << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: CNT6: "
            << latchupStatusReport.cnt6 << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Sec: "
            << static_cast<unsigned int>(latchupStatusReport.timeSec.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Min: "
            << static_cast<unsigned int>(latchupStatusReport.timeMin.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Hour: "
            << static_cast<unsigned int>(latchupStatusReport.timeHour.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Day: "
            << static_cast<unsigned int>(latchupStatusReport.timeDay.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Mon: "
            << static_cast<unsigned int>(latchupStatusReport.timeMon.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Year: "
            << static_cast<unsigned int>(latchupStatusReport.timeYear.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: Msec: "
            << static_cast<unsigned int>(latchupStatusReport.timeMsec.value) << std::endl;
  sif::info << "PlocSupervisorHandler::handleLatchupStatusReport: isSet: "
            << static_cast<unsigned int>(latchupStatusReport.isSet.value) << std::endl;
#endif

  return result;
}

ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) {
  ReturnValue_t result = returnvalue::OK;

  result = verifyPacket(data, supv::SIZE_ADC_REPORT);

  if (result == result::CRC_FAILURE) {
    sif::error << "PlocSupervisorHandler::handleAdcReport: ADC report has "
               << "invalid crc" << std::endl;
    return result;
  }

  const uint8_t* dataField = data + supv::PAYLOAD_OFFSET;
  result = adcReport.read();
  if (result != returnvalue::OK) {
    return result;
  }
  adcReport.setValidityBufferGeneration(false);
  size_t size = adcReport.getSerializedSize();
  result = adcReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupervisorHandler::handleAdcReport: Deserialization failed" << std::endl;
  }
  adcReport.setValidityBufferGeneration(true);
  adcReport.setValidity(true, true);
  result = adcReport.commit();
  if (result != returnvalue::OK) {
    return result;
  }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
  adcReport.printSet();
#endif
  nextReplyId = supv::EXE_REPORT;
  return result;
}

void PlocSupervisorHandler::setNextReplyId() {
  switch (getPendingCommand()) {
    case supv::GET_HK_REPORT:
      nextReplyId = supv::HK_REPORT;
      break;
    case supv::GET_BOOT_STATUS_REPORT:
      nextReplyId = supv::BOOT_STATUS_REPORT;
      break;
    case supv::GET_LATCHUP_STATUS_REPORT:
      nextReplyId = supv::LATCHUP_REPORT;
      break;
    case supv::FIRST_MRAM_DUMP:
      nextReplyId = supv::FIRST_MRAM_DUMP;
      break;
    case supv::CONSECUTIVE_MRAM_DUMP:
      nextReplyId = supv::CONSECUTIVE_MRAM_DUMP;
      break;
    case supv::LOGGING_REQUEST_COUNTERS:
      nextReplyId = supv::LOGGING_REPORT;
      break;
    case supv::REQUEST_ADC_REPORT:
      nextReplyId = supv::ADC_REPORT;
      break;
    default:
      /* If no telemetry is expected the next reply is always the execution report */
      nextReplyId = supv::EXE_REPORT;
      break;
  }
}

size_t PlocSupervisorHandler::getNextReplyLength(DeviceCommandId_t commandId) {
  size_t replyLen = 0;

  if (nextReplyId == supv::NONE) {
    return replyLen;
  }

  if (nextReplyId == supv::FIRST_MRAM_DUMP || nextReplyId == supv::CONSECUTIVE_MRAM_DUMP) {
    /**
     * Try to read 20 MRAM packets. If reply is larger, the packets will be read with the
     * next doSendRead call. The command will be as long active as the packet with the sequence
     * count indicating the last packet has not been received.
     */
    replyLen = supv::MAX_PACKET_SIZE * 20;
    return replyLen;
  }

  DeviceReplyIter iter = deviceReplyMap.find(nextReplyId);
  if (iter != deviceReplyMap.end()) {
    if ((iter->second.delayCycles == 0 && iter->second.countdown == nullptr) ||
        (not iter->second.active && iter->second.countdown != nullptr)) {
      /* Reply inactive */
      return replyLen;
    }
    replyLen = iter->second.replyLen;
  } else {
    sif::debug << "PlocSupervisorHandler::getNextReplyLength: No entry for reply with reply id "
               << std::hex << nextReplyId << " in deviceReplyMap" << std::endl;
  }

  return replyLen;
}

void PlocSupervisorHandler::doOffActivity() {}

void PlocSupervisorHandler::handleDeviceTm(const uint8_t* data, size_t dataSize,
                                           DeviceCommandId_t replyId) {
  ReturnValue_t result = returnvalue::OK;

  if (wiretappingMode == RAW) {
    /* Data already sent in doGetRead() */
    return;
  }

  DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
  if (iter == deviceReplyMap.end()) {
    sif::debug << "PlocSupervisorHandler::handleDeviceTM: Unknown reply id" << std::endl;
    return;
  }
  MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;

  if (queueId == NO_COMMANDER) {
    return;
  }

  result = actionHelper.reportData(queueId, replyId, data, dataSize);
  if (result != returnvalue::OK) {
    sif::debug << "PlocSupervisorHandler::handleDeviceTM: Failed to report data" << std::endl;
  }
}

ReturnValue_t PlocSupervisorHandler::prepareEmptyCmd(uint16_t apid, uint8_t serviceId) {
  supv::NoPayloadPacket packet(spParams, apid, serviceId);
  ReturnValue_t result = packet.buildPacket();
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareSelBootImageCmd(const uint8_t* commandData) {
  supv::MPSoCBootSelect packet(spParams);
  ReturnValue_t result =
      packet.buildPacket(commandData[0], commandData[1], commandData[2], commandData[3]);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareSetTimeRefCmd() {
  Clock::TimeOfDay_t time;
  ReturnValue_t result = Clock::getDateAndTime(&time);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupervisorHandler::prepareSetTimeRefCmd: Failed to get current time"
                 << std::endl;
    return result::GET_TIME_FAILURE;
  }
  supv::SetTimeRef packet(spParams);
  result = packet.buildPacket(&time);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareDisableHk() {
  supv::DisablePeriodicHkTransmission packet(spParams);
  ReturnValue_t result = packet.buildPacket();
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareSetBootTimeoutCmd(const uint8_t* commandData) {
  supv::SetBootTimeout packet(spParams);
  uint32_t timeout = *(commandData) << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 |
                     *(commandData + 3);
  ReturnValue_t result = packet.buildPacket(timeout);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareRestartTriesCmd(const uint8_t* commandData) {
  uint8_t restartTries = *(commandData);
  supv::SetRestartTries packet(spParams);
  ReturnValue_t result = packet.buildPacket(restartTries);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareLatchupConfigCmd(const uint8_t* commandData,
                                                             DeviceCommandId_t deviceCommand) {
  ReturnValue_t result = returnvalue::OK;
  uint8_t latchupId = *commandData;
  if (latchupId > 6) {
    return result::INVALID_LATCHUP_ID;
  }
  switch (deviceCommand) {
    case (supv::ENABLE_LATCHUP_ALERT): {
      supv::LatchupAlert packet(spParams);
      result = packet.buildPacket(true, latchupId);
      if (result != returnvalue::OK) {
        return result;
      }
      finishTcPrep(packet.getFullPacketLen());
      break;
    }
    case (supv::DISABLE_LATCHUP_ALERT): {
      supv::LatchupAlert packet(spParams);
      result = packet.buildPacket(false, latchupId);
      if (result != returnvalue::OK) {
        return result;
      }
      finishTcPrep(packet.getFullPacketLen());
      break;
    }
    default: {
      sif::debug << "PlocSupervisorHandler::prepareLatchupConfigCmd: Invalid command id"
                 << std::endl;
      result = returnvalue::FAILED;
      break;
    }
  }
  return result;
}

ReturnValue_t PlocSupervisorHandler::prepareSetAlertLimitCmd(const uint8_t* commandData) {
  uint8_t offset = 0;
  uint8_t latchupId = *commandData;
  offset += 1;
  uint32_t dutycycle = *(commandData + offset) << 24 | *(commandData + offset + 1) << 16 |
                       *(commandData + offset + 2) << 8 | *(commandData + offset + 3);
  if (latchupId > 6) {
    return result::INVALID_LATCHUP_ID;
  }
  supv::SetAlertlimit packet(spParams);
  ReturnValue_t result = packet.buildPacket(latchupId, dutycycle);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData) {
  uint16_t ch = *(commandData) << 8 | *(commandData + 1);
  supv::SetAdcEnabledChannels packet(spParams);
  ReturnValue_t result = packet.buildPacket(ch);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData) {
  uint8_t offset = 0;
  uint16_t windowSize = *(commandData + offset) << 8 | *(commandData + offset + 1);
  offset += 2;
  uint16_t stridingStepSize = *(commandData + offset) << 8 | *(commandData + offset + 1);
  supv::SetAdcWindowAndStride packet(spParams);
  ReturnValue_t result = packet.buildPacket(windowSize, stridingStepSize);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareSetAdcThresholdCmd(const uint8_t* commandData) {
  uint32_t threshold = *(commandData) << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 |
                       *(commandData + 3);
  supv::SetAdcThreshold packet(spParams);
  ReturnValue_t result = packet.buildPacket(threshold);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareRunAutoEmTest(const uint8_t* commandData) {
  uint8_t test = *commandData;
  if (test != 1 && test != 2) {
    return result::INVALID_TEST_PARAM;
  }
  supv::RunAutoEmTests packet(spParams);
  ReturnValue_t result = packet.buildPacket(test);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandData) {
  uint8_t port = *commandData;
  uint8_t pin = *(commandData + 1);
  uint8_t val = *(commandData + 2);
  supv::SetGpio packet(spParams);
  ReturnValue_t result = packet.buildPacket(port, pin, val);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareReadGpioCmd(const uint8_t* commandData) {
  uint8_t port = *commandData;
  uint8_t pin = *(commandData + 1);
  supv::ReadGpio packet(spParams);
  ReturnValue_t result = packet.buildPacket(port, pin);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareFactoryResetCmd(const uint8_t* commandData,
                                                            size_t len) {
  FactoryReset resetCmd(spParams);
  if (len < 1) {
    return HasActionsIF::INVALID_PARAMETERS;
  }
  ReturnValue_t result = resetCmd.buildPacket(commandData[0]);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(resetCmd.getFullPacketLen());
  return returnvalue::OK;
}

void PlocSupervisorHandler::finishTcPrep(size_t packetLen) {
  nextReplyId = supv::ACK_REPORT;
  rawPacket = commandBuffer;
  rawPacketLen = packetLen;
}

ReturnValue_t PlocSupervisorHandler::prepareSetShutdownTimeoutCmd(const uint8_t* commandData) {
  uint32_t timeout = 0;
  ReturnValue_t result = returnvalue::OK;
  size_t size = sizeof(timeout);
  result =
      SerializeAdapter::deSerialize(&timeout, &commandData, &size, SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::warning
        << "PlocSupervisorHandler::prepareSetShutdownTimeoutCmd: Failed to deserialize timeout"
        << std::endl;
  }
  supv::SetShutdownTimeout packet(spParams);
  result = packet.buildPacket(timeout);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

void PlocSupervisorHandler::disableAllReplies() {
  using namespace supv;
  DeviceReplyMap::iterator iter;

  /* Disable ack reply */
  iter = deviceReplyMap.find(ACK_REPORT);
  DeviceReplyInfo* info = &(iter->second);
  info->delayCycles = 0;
  info->command = deviceCommandMap.end();

  DeviceCommandId_t commandId = getPendingCommand();

  /* If the command expects a telemetry packet the appropriate tm reply will be disabled here */
  switch (commandId) {
    case GET_HK_REPORT: {
      disableReply(GET_HK_REPORT);
      break;
    }
    case FIRST_MRAM_DUMP:
    case CONSECUTIVE_MRAM_DUMP: {
      disableReply(commandId);
      break;
    }
    case REQUEST_ADC_REPORT: {
      disableReply(ADC_REPORT);
      break;
    }
    case GET_BOOT_STATUS_REPORT: {
      disableReply(BOOT_STATUS_REPORT);
      break;
    }
    case GET_LATCHUP_STATUS_REPORT: {
      disableReply(LATCHUP_REPORT);
      break;
    }
    case LOGGING_REQUEST_COUNTERS: {
      disableReply(LOGGING_REPORT);
      break;
    }
    default: {
      break;
    }
  }

  /* We must always disable the execution report reply here */
  disableExeReportReply();
}

void PlocSupervisorHandler::disableReply(DeviceCommandId_t replyId) {
  DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
  DeviceReplyInfo* info = &(iter->second);
  info->delayCycles = 0;
  info->active = false;
  info->command = deviceCommandMap.end();
}

void PlocSupervisorHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnValue_t status) {
  DeviceReplyIter iter = deviceReplyMap.find(replyId);

  if (iter == deviceReplyMap.end()) {
    sif::debug << "PlocSupervisorHandler::sendFailureReport: Reply not in reply map" << std::endl;
    return;
  }

  DeviceCommandInfo* info = &(iter->second.command->second);

  if (info == nullptr) {
    sif::debug << "PlocSupervisorHandler::sendFailureReport: Reply has no active command"
               << std::endl;
    return;
  }

  if (info->sendReplyTo != NO_COMMANDER) {
    actionHelper.finish(false, info->sendReplyTo, iter->first, status);
  }
  info->isExecuting = false;
}

void PlocSupervisorHandler::disableExeReportReply() {
  DeviceReplyIter iter = deviceReplyMap.find(supv::EXE_REPORT);
  DeviceReplyInfo* info = &(iter->second);
  info->delayCycles = 0;
  info->command = deviceCommandMap.end();
  info->active = false;
  /* Expected replies is set to one here. The value will set to 0 in replyToReply() */
  info->command->second.expectedReplies = 1;
}

ReturnValue_t PlocSupervisorHandler::handleMramDumpPacket(DeviceCommandId_t id) {
  ReturnValue_t result = returnvalue::FAILED;

  // Prepare packet for downlink
  if (packetInBuffer) {
    uint16_t packetLen = readSpacePacketLength(spacePacketBuffer);
    result = verifyPacket(spacePacketBuffer, ccsds::HEADER_LEN + packetLen + 1);
    if (result != returnvalue::OK) {
      sif::warning << "PlocSupervisorHandler::handleMramDumpPacket: CRC failure" << std::endl;
      return result;
    }
    result = handleMramDumpFile(id);
    if (result != returnvalue::OK) {
      DeviceCommandMap::iterator iter = deviceCommandMap.find(id);
      actionHelper.finish(false, iter->second.sendReplyTo, id, result);
      disableAllReplies();
      nextReplyId = supv::NONE;
      return result;
    }
    packetInBuffer = false;
    receivedMramDumpPackets++;
    if (expectedMramDumpPackets == receivedMramDumpPackets) {
      nextReplyId = supv::EXE_REPORT;
    }
    increaseExpectedMramReplies(id);
    return returnvalue::OK;
  }
  return result;
}

void PlocSupervisorHandler::increaseExpectedMramReplies(DeviceCommandId_t id) {
  DeviceReplyMap::iterator mramDumpIter = deviceReplyMap.find(id);
  DeviceReplyMap::iterator exeReportIter = deviceReplyMap.find(supv::EXE_REPORT);
  if (mramDumpIter == deviceReplyMap.end()) {
    sif::debug << "PlocSupervisorHandler::increaseExpectedMramReplies: Dump MRAM reply not "
               << "in reply map" << std::endl;
    return;
  }
  if (exeReportIter == deviceReplyMap.end()) {
    sif::debug << "PlocSupervisorHandler::increaseExpectedMramReplies: Execution report not "
               << "in reply map" << std::endl;
    return;
  }
  DeviceReplyInfo* mramReplyInfo = &(mramDumpIter->second);
  if (mramReplyInfo == nullptr) {
    sif::debug << "PlocSupervisorHandler::increaseExpectedReplies: MRAM reply info nullptr"
               << std::endl;
    return;
  }
  DeviceReplyInfo* exeReplyInfo = &(exeReportIter->second);
  if (exeReplyInfo == nullptr) {
    sif::debug << "PlocSupervisorHandler::increaseExpectedReplies: Execution reply info"
               << " nullptr" << std::endl;
    return;
  }
  DeviceCommandInfo* info = &(mramReplyInfo->command->second);
  if (info == nullptr) {
    sif::debug << "PlocSupervisorHandler::increaseExpectedReplies: Command info nullptr"
               << std::endl;
    return;
  }
  uint8_t sequenceFlags = spacePacketBuffer[2] >> 6;
  if (sequenceFlags != static_cast<uint8_t>(ccsds::SequenceFlags::LAST_SEGMENT) &&
      (sequenceFlags != static_cast<uint8_t>(ccsds::SequenceFlags::UNSEGMENTED))) {
    // Command expects at least one MRAM packet more and the execution report
    info->expectedReplies = 2;
    mramReplyInfo->countdown->resetTimer();
  } else {
    // Command expects the execution report
    info->expectedReplies = 1;
    mramReplyInfo->active = false;
  }
  exeReplyInfo->countdown->resetTimer();
  return;
}

ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) {
#ifdef XIPHOS_Q7S
  if (not sdcMan->getActiveSdCard()) {
    return HasFileSystemIF::FILESYSTEM_INACTIVE;
  }
#endif
  ReturnValue_t result = returnvalue::OK;
  uint16_t packetLen = readSpacePacketLength(spacePacketBuffer);
  uint8_t sequenceFlags = readSequenceFlags(spacePacketBuffer);
  if (id == supv::FIRST_MRAM_DUMP) {
    if (sequenceFlags == static_cast<uint8_t>(ccsds::SequenceFlags::FIRST_SEGMENT) ||
        (sequenceFlags == static_cast<uint8_t>(ccsds::SequenceFlags::UNSEGMENTED))) {
      result = createMramDumpFile();
      if (result != returnvalue::OK) {
        return result;
      }
    }
  }
  if (not std::filesystem::exists(activeMramFile)) {
    sif::warning << "PlocSupervisorHandler::handleMramDumpFile: MRAM file does not exist"
                 << std::endl;
    return result::MRAM_FILE_NOT_EXISTS;
  }
  std::ofstream file(activeMramFile, std::ios_base::app | std::ios_base::out);
  file.write(reinterpret_cast<const char*>(spacePacketBuffer + ccsds::HEADER_LEN), packetLen - 1);
  file.close();
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) {
  uint32_t start = 0;
  uint32_t stop = 0;
  size_t size = sizeof(start) + sizeof(stop);
  SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG);
  SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG);
  if ((stop - start) <= 0) {
    return result::INVALID_MRAM_ADDRESSES;
  }
  supv::MramCmd packet(spParams);
  ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE);
  if (result != returnvalue::OK) {
    return result;
  }
  finishTcPrep(packet.getFullPacketLen());
  return returnvalue::OK;
}

uint16_t PlocSupervisorHandler::readSpacePacketLength(uint8_t* spacePacket) {
  return spacePacket[4] << 8 | spacePacket[5];
}

uint8_t PlocSupervisorHandler::readSequenceFlags(uint8_t* spacePacket) {
  return spacePacketBuffer[2] >> 6;
}

ReturnValue_t PlocSupervisorHandler::createMramDumpFile() {
  ReturnValue_t result = returnvalue::OK;
  std::string timeStamp;
  result = getTimeStampString(timeStamp);
  if (result != returnvalue::OK) {
    return result;
  }

  std::string filename = "mram-dump--" + timeStamp + ".bin";

#ifdef XIPHOS_Q7S
  const char* currentMountPrefix = sdcMan->getCurrentMountPrefix();
#else
  const char* currentMountPrefix = "/mnt/sd0";
#endif /* BOARD_TE0720 == 0 */
  if (currentMountPrefix == nullptr) {
    return returnvalue::FAILED;
  }

  // Check if path to PLOC directory exists
  if (not std::filesystem::exists(std::string(currentMountPrefix) + "/" + supervisorFilePath)) {
    sif::warning << "PlocSupervisorHandler::createMramDumpFile: Supervisor path does not exist"
                 << std::endl;
    return result::PATH_DOES_NOT_EXIST;
  }
  activeMramFile = std::string(currentMountPrefix) + "/" + supervisorFilePath + "/" + filename;
  // Create new file
  std::ofstream file(activeMramFile, std::ios_base::out);
  file.close();

  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::getTimeStampString(std::string& timeStamp) {
  Clock::TimeOfDay_t time;
  ReturnValue_t result = Clock::getDateAndTime(&time);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupervisorHandler::getTimeStampString: Failed to get current time"
                 << std::endl;
    return result::GET_TIME_FAILURE;
  }
  timeStamp = std::to_string(time.year) + "-" + std::to_string(time.month) + "-" +
              std::to_string(time.day) + "--" + std::to_string(time.hour) + "-" +
              std::to_string(time.minute) + "-" + std::to_string(time.second);
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::extractUpdateCommand(const uint8_t* commandData, size_t size,
                                                          supv::UpdateParams& params) {
  size_t remSize = size;
  if (size > (config::MAX_FILENAME_SIZE + config::MAX_PATH_SIZE + sizeof(params.memId)) +
                 sizeof(params.startAddr) + sizeof(params.bytesWritten) + sizeof(params.seqCount) +
                 sizeof(uint8_t)) {
    sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Data size too big" << std::endl;
    return result::INVALID_LENGTH;
  }
  ReturnValue_t result = returnvalue::OK;
  result = extractBaseParams(&commandData, size, params);
  result = SerializeAdapter::deSerialize(&params.bytesWritten, &commandData, &remSize,
                                         SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Failed to deserialize bytes "
                    "already written"
                 << std::endl;
    return result;
  }
  result = SerializeAdapter::deSerialize(&params.seqCount, &commandData, &remSize,
                                         SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::warning
        << "PlocSupervisorHandler::extractUpdateCommand: Failed to deserialize start sequence count"
        << std::endl;
    return result;
  }
  uint8_t delMemRaw = 0;
  result = SerializeAdapter::deSerialize(&delMemRaw, &commandData, &remSize,
                                         SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::warning
        << "PlocSupervisorHandler::extractUpdateCommand: Failed to deserialize whether to delete "
           "memory"
        << std::endl;
    return result;
  }
  params.deleteMemory = delMemRaw;
  return returnvalue::OK;
}

ReturnValue_t PlocSupervisorHandler::extractBaseParams(const uint8_t** commandData, size_t& remSize,
                                                       supv::UpdateParams& params) {
  bool nullTermFound = false;
  for (size_t idx = 0; idx < remSize; idx++) {
    if ((*commandData)[idx] == '\0') {
      nullTermFound = true;
      break;
    }
  }
  if (not nullTermFound) {
    return returnvalue::FAILED;
  }
  params.file = std::string(reinterpret_cast<const char*>(*commandData));
  if (params.file.size() > (config::MAX_FILENAME_SIZE + config::MAX_PATH_SIZE)) {
    sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Filename too long" << std::endl;
    return result::FILENAME_TOO_LONG;
  }
  *commandData += params.file.size() + SIZE_NULL_TERMINATOR;
  remSize -= (params.file.size() + SIZE_NULL_TERMINATOR);
  params.memId = **commandData;
  *commandData += 1;
  remSize -= 1;
  ReturnValue_t result = SerializeAdapter::deSerialize(&params.startAddr, commandData, &remSize,
                                                       SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::warning << "PlocSupervisorHandler::extractBaseParams: Failed to deserialize start address"
                 << std::endl;
    return result;
  }
  return result;
}

ReturnValue_t PlocSupervisorHandler::eventSubscription() {
  ReturnValue_t result = returnvalue::OK;
  EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "PlocSupervisorHandler::eventSubscritpion: Invalid event manager" << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
    ;
  }
  result = manager->registerListener(eventQueue->getId());
  if (result != returnvalue::OK) {
    return result;
  }
  result = manager->subscribeToEventRange(
      eventQueue->getId(), event::getEventId(PlocSupvUartManager::SUPV_UPDATE_FAILED),
      event::getEventId(PlocSupvUartManager::SUPV_MEM_CHECK_FAIL));
  if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "PlocSupervisorHandler::eventSubscritpion: Failed to subscribe to events from "
                    " ploc supervisor helper"
                 << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  return result;
}

ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionReport& report) {
  DeviceCommandId_t commandId = getPendingCommand();
  ReturnValue_t result = OK;
  switch (commandId) {
    case supv::READ_GPIO: {
      // TODO: Fix
      uint16_t gpioState = report.getStatusCode();
#if OBSW_DEBUG_PLOC_SUPERVISOR == 1
      sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl;
#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
      DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId);
      if (iter->second.sendReplyTo == NO_COMMAND_ID) {
        return returnvalue::OK;
      }
      uint8_t data[sizeof(gpioState)];
      size_t size = 0;
      result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState),
                                           SerializeIF::Endianness::BIG);
      if (result != returnvalue::OK) {
        sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << std::endl;
      }
      result = actionHelper.reportData(iter->second.sendReplyTo, commandId, data, sizeof(data));
      if (result != returnvalue::OK) {
        sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << std::endl;
      }
      break;
    }
    case supv::SET_TIME_REF: {
      // We could only allow proper bootup when the time was set successfully, but
      // this makes debugging difficult.

      if (startupState == StartupState::WAIT_FOR_TIME_REPLY) {
        startupState = StartupState::TIME_WAS_SET;
      }
      break;
    }
    default:
      break;
  }
  return returnvalue::OK;
}

void PlocSupervisorHandler::handleExecutionFailureReport(ExecutionReport& report) {
  using namespace supv;
  DeviceCommandId_t commandId = getPendingCommand();
  report.printStatusInformation();
  if (commandId != DeviceHandlerIF::NO_COMMAND_ID) {
    triggerEvent(SUPV_EXE_FAILURE, commandId, static_cast<uint32_t>(report.getStatusCode()));
  }
  sendFailureReport(EXE_REPORT, result::RECEIVED_EXE_FAILURE);
  disableExeReportReply();
}

void PlocSupervisorHandler::handleBadApidServiceCombination(Event event, unsigned int apid,
                                                            unsigned int serviceId) {
  const char* printString = "";
  if (event == SUPV_UNKNOWN_TM) {
    printString = "PlocSupervisorHandler: Unknown";
  } else if (event == SUPV_UNINIMPLEMENTED_TM) {
    printString = "PlocSupervisorHandler: Unimplemented";
  }
  triggerEvent(event, apid, tmReader.getServiceId());
  sif::warning << printString << " APID service combination 0x" << std::setw(2) << std::setfill('0')
               << std::hex << apid << ", 0x" << std::setw(2) << serviceId << std::endl;
}

void PlocSupervisorHandler::printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId) {
  switch (commandId) {
    case (supv::SET_TIME_REF): {
      sif::warning
          << "PlocSupervisoHandler: Setting time failed. Make sure the OBC has a valid time"
          << std::endl;
      break;
    }
    default:
      break;
  }
}

ReturnValue_t PlocSupervisorHandler::getSwitches(const uint8_t** switches,
                                                 uint8_t* numberOfSwitches) {
  if (powerSwitch == power::NO_SWITCH) {
    return DeviceHandlerBase::NO_SWITCH;
  }
  *numberOfSwitches = 1;
  *switches = &powerSwitch;
  return returnvalue::OK;
}

uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
  return 7000;
}

// ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() {
//   uint16_t apid = (spacePacketBuffer[0] << 8 | spacePacketBuffer[1]) & supv::APID_MASK;
//  TODO: Fix
//  if (apid != supv::APID_MRAM_DUMP_TM) {
//    return result::NO_MRAM_PACKET;
//  }
// return APERIODIC_REPLY;
//}

// ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, size_t
// remainingSize,
//                                                       size_t* foundLen) {
//   ReturnValue_t result = IGNORE_FULL_PACKET;
//   uint16_t packetLen = 0;
//   *foundLen = 0;
//
//   for (size_t idx = 0; idx < remainingSize; idx++) {
//     std::memcpy(spacePacketBuffer + bufferTop, packet + idx, 1);
//     bufferTop += 1;
//     *foundLen += 1;
//     if (bufferTop >= ccsds::HEADER_LEN) {
//       packetLen = readSpacePacketLength(spacePacketBuffer);
//     }
//
//     if (bufferTop == ccsds::HEADER_LEN + packetLen + 1) {
//       packetInBuffer = true;
//       bufferTop = 0;
//       return checkMramPacketApid();
//     }
//
//     if (bufferTop == supv::MAX_PACKET_SIZE) {
//       *foundLen = remainingSize;
//       disableAllReplies();
//       bufferTop = 0;
//       sif::info << "PlocSupervisorHandler::parseMramPackets: Can not find MRAM packet in space "
//                    "packet buffer"
//                 << std::endl;
//       return result::MRAM_PACKET_PARSING_FAILURE;
//     }
//   }
//
//   return result;
// }

// ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandData) {
//   uint32_t start = 0;
//   uint32_t stop = 0;
//   size_t size = sizeof(start) + sizeof(stop);
//   SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG);
//   SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG);
//   if ((stop - start) <= 0) {
//     return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES;
//   }
//   supv::MramCmd packet(spParams);
//   ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::DUMP);
//   if (result != returnvalue::OK) {
//     return result;
//   }
//   expectedMramDumpPackets = (stop - start) / supv::MAX_DATA_CAPACITY;
//   if ((stop - start) % supv::MAX_DATA_CAPACITY) {
//     expectedMramDumpPackets++;
//   }
//   receivedMramDumpPackets = 0;
//
//   finishTcPrep(packet.getFullPacketLen());
//   return returnvalue::OK;
// }

// ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* commandData,
//                                                            size_t commandDataLen) {
//   using namespace supv;
//   RequestLoggingData::Sa sa = static_cast<RequestLoggingData::Sa>(*commandData);
//   uint8_t tpc = *(commandData + 1);
//   RequestLoggingData packet(spParams);
//   ReturnValue_t result = packet.buildPacket(sa, tpc);
//   if (result != returnvalue::OK) {
//     return result;
//   }
//   finishTcPrep(packet.getFullPacketLen());
//   return returnvalue::OK;
// }

// ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* commandData) {
//   using namespace supv;
//   uint8_t nvm01 = *(commandData);
//   uint8_t nvm3 = *(commandData + 1);
//   EnableNvms packet(spParams);
//   ReturnValue_t result = packet.buildPacket(nvm01, nvm3);
//   if (result != returnvalue::OK) {
//     return result;
//   }
//   finishTcPrep(packet.getFullPacketLen());
//   return returnvalue::OK;
// }

// ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) {
//   ReturnValue_t result = returnvalue::OK;
//
//   result = verifyPacket(data, supv::SIZE_LOGGING_REPORT);
//
//   if (result == SupvReturnValuesIF::CRC_FAILURE) {
//     sif::warning << "PlocSupervisorHandler::handleLoggingReport: Logging report has "
//                  << "invalid crc" << std::endl;
//     return result;
//   }
//
//   const uint8_t* dataField = data + supv::PAYLOAD_OFFSET + sizeof(supv::RequestLoggingData::Sa);
//   result = loggingReport.read();
//   if (result != returnvalue::OK) {
//     return result;
//   }
//   loggingReport.setValidityBufferGeneration(false);
//   size_t size = loggingReport.getSerializedSize();
//   result = loggingReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG);
//   if (result != returnvalue::OK) {
//     sif::warning << "PlocSupervisorHandler::handleLoggingReport: Deserialization failed"
//                  << std::endl;
//   }
//   loggingReport.setValidityBufferGeneration(true);
//   loggingReport.setValidity(true, true);
//   result = loggingReport.commit();
//   if (result != returnvalue::OK) {
//     return result;
//   }
//#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
//   loggingReport.printSet();
//#endif
//   nextReplyId = supv::EXE_REPORT;
//   return result;
// }