#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <mission/acs/str/StarTrackerHandler.h>
#include <mission/acs/str/strHelpers.h>
#include <mission/acs/str/strJsonCommands.h>

#include <string>

#include "fsfw/filesystem/HasFileSystemIF.h"
#include "fsfw/ipc/MessageQueueIF.h"
#include "fsfw/returnvalues/returnvalue.h"
#include "mission/memory/SdCardMountedIF.h"

extern "C" {
#include <sagitta/client/actionreq.h>
#include <sagitta/client/client_tm_structs.h>
#include <sagitta/client/parameter.h>
#include <sagitta/client/telemetry.h>
#include <wire/common/genericstructs.h>
}

#include <atomic>
#include <thread>

#include "OBSWConfig.h"
#include "eive/definitions.h"
#include "fsfw/thermal/tcsDefinitions.h"

std::atomic_bool JCFG_DONE(false);

StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                                       StrComHandler* strHelper, power::Switch_t powerSwitch,
                                       startracker::SdCardConfigPathGetter& cfgPathGetter,
                                       SdCardMountedIF& sdCardIF)
    : DeviceHandlerBase(objectId, comIF, comCookie),
      temperatureSet(this),
      versionSet(this),
      powerSet(this),
      interfaceSet(this),
      timeSet(this),
      solutionSet(this),
      histogramSet(this),
      checksumSet(this),
      cameraSet(this),
      limitsSet(this),
      loglevelSet(this),
      mountingSet(this),
      imageProcessorSet(this),
      centroidingSet(this),
      lisaSet(this),
      matchingSet(this),
      trackingSet(this),
      validationSet(this),
      algoSet(this),
      subscriptionSet(this),
      logSubscriptionSet(this),
      debugCameraSet(this),
      autoBlobSet(this),
      matchedCentroidsSet(this),
      blobSet(this),
      blobsSet(this),
      centroidSet(this),
      centroidsSet(this),
      contrastSet(this),
      blobStatsSet(this),
      strHelper(strHelper),
      powerSwitch(powerSwitch),
      sdCardIF(sdCardIF),
      cfgPathGetter(cfgPathGetter) {
  if (comCookie == nullptr) {
    sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
  }
  if (strHelper == nullptr) {
    sif::error << "StarTrackerHandler: Invalid str image loader" << std::endl;
  }
  eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
  additionalRequestedTm.emplace(startracker::REQ_TEMPERATURE);
  currentSecondaryTmIter = additionalRequestedTm.begin();
}

StarTrackerHandler::~StarTrackerHandler() {}

void StarTrackerHandler::doStartUp() {
  switch (startupState) {
    case StartupState::IDLE:
      startupState = StartupState::CHECK_PROGRAM;
      return;
    case StartupState::BOOT_BOOTLOADER:
      // This step is required in case the star tracker is already in firmware mode to harmonize
      // the device handler's submode to the star tracker's mode
      return;
    case StartupState::DONE:
      if (!JCFG_DONE) {
        startupState = StartupState::WAIT_JCFG;
        return;
      }
      break;
    case StartupState::WAIT_JCFG: {
      return;
    }
    default:
      return;
  }
  startupState = StartupState::DONE;
  internalState = InternalState::IDLE;
  setMode(_MODE_TO_ON);
}

void StarTrackerHandler::doShutDown() {
  // If the star tracker is shutdown also stop all running processes in the image loader task
  strHelper->stopProcess();
  internalState = InternalState::IDLE;
  startupState = StartupState::IDLE;
  bootState = FwBootState::NONE;
  solutionSet.setReportingEnabled(false);
  {
    PoolReadGuard pg(&solutionSet);
    solutionSet.caliQw.value = 0.0;
    solutionSet.caliQx.value = 0.0;
    solutionSet.caliQy.value = 0.0;
    solutionSet.caliQz.value = 0.0;
    solutionSet.isTrustWorthy.value = 0;
    solutionSet.setValidity(false, true);
  }
  {
    PoolReadGuard pg(&temperatureSet);
    temperatureSet.fpgaTemperature = thermal::INVALID_TEMPERATURE;
    temperatureSet.cmosTemperature = thermal::INVALID_TEMPERATURE;
    temperatureSet.mcuTemperature = thermal::INVALID_TEMPERATURE;
    temperatureSet.setValidity(false, true);
  }
  reinitNextSetParam = false;
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t StarTrackerHandler::initialize() {
  ReturnValue_t result = returnvalue::OK;
  result = DeviceHandlerBase::initialize();
  if (result != returnvalue::OK) {
    return result;
  }

  // Spin up a thread to do the JSON initialization, takes 200-250 ms which would
  // delay whole satellite boot process.
  reloadJsonCfgFile();

  // Default firmware target is always initialized from persistent file.
  loadTargetFirmwareFromPersistentCfg();

  EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "StarTrackerHandler::initialize: 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(StrComHandler::IMAGE_UPLOAD_FAILED),
                                          event::getEventId(StrComHandler::FIRMWARE_UPDATE_FAILED));
  if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "StarTrackerHandler::initialize: Failed to subscribe to events from "
                    " str helper"
                 << std::endl;
#endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }

  return returnvalue::OK;
}

void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() {
  const char* prefix = sdCardIF.getCurrentMountPrefix();
  std::filesystem::path path = std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
  std::ifstream ifile(path);
  if (ifile.is_open() and !ifile.bad()) {
    std::string targetStr;
    std::getline(ifile, targetStr);
    if (targetStr == "backup") {
      firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP);
    }
  }
}

bool StarTrackerHandler::reloadJsonCfgFile() {
  jcfgCountdown.resetTimer();
  auto strCfgPath = cfgPathGetter.getCfgPath();
  if (strCfgPath.has_value()) {
    jcfgPending = true;
    JCFG_DONE = false;
    jsonCfgTask = std::thread{setUpJsonCfgs, std::ref(jcfgs), strCfgPath.value()};
    return true;
  }
  // Simplified FDIR: Just continue as usual..
  JCFG_DONE = true;
  return false;
}

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

  switch (actionId) {
    case (startracker::ADD_SECONDARY_TM_TO_NORMAL_MODE): {
      if (size < 4) {
        return HasActionsIF::INVALID_PARAMETERS;
      }
      DeviceCommandId_t idToAdd;
      result =
          SerializeAdapter::deSerialize(&idToAdd, data, &size, SerializeIF::Endianness::NETWORK);
      if (result != returnvalue::OK) {
        return result;
      }
      addSecondaryTmForNormalMode(idToAdd);
      return EXECUTION_FINISHED;
    }
    case (startracker::RESET_SECONDARY_TM_SET): {
      resetSecondaryTmSet();
      return EXECUTION_FINISHED;
    }
    case (startracker::READ_SECONDARY_TM_SET): {
      std::vector<uint8_t> dataVec(additionalRequestedTm.size() * 4);
      unsigned idx = 0;
      size_t serLen = 0;
      for (const auto& cmd : additionalRequestedTm) {
        SerializeAdapter::serialize(&cmd, dataVec.data() + idx * 4, &serLen, dataVec.size(),
                                    SerializeIF::Endianness::NETWORK);
        idx++;
      }
      actionHelper.reportData(commandedBy, actionId, dataVec.data(), dataVec.size());
      return EXECUTION_FINISHED;
    }
    case (startracker::STOP_IMAGE_LOADER): {
      strHelper->stopProcess();
      return EXECUTION_FINISHED;
    }
    case (startracker::SET_JSON_FILE_NAME): {
      if (size > config::MAX_PATH_SIZE) {
        return FILE_PATH_TOO_LONG;
      }
      paramJsonFile = std::string(reinterpret_cast<const char*>(data), size);
      return EXECUTION_FINISHED;
    }
    case (startracker::DISABLE_TIMESTAMP_GENERATION):
      strHelper->disableTimestamping();
      return EXECUTION_FINISHED;
    case (startracker::ENABLE_TIMESTAMP_GENERATION):
      strHelper->enableTimestamping();
      return EXECUTION_FINISHED;
    default:
      break;
  }

  if (strHelperHandlingSpecialRequest == true) {
    return STR_HELPER_EXECUTING;
  }

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

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

  // Intercept image loader commands which do not follow the common DHB communication flow
  switch (actionId) {
    case (startracker::UPLOAD_IMAGE): {
      result = DeviceHandlerBase::acceptExternalDeviceCommands();
      if (result != returnvalue::OK) {
        return result;
      }
      if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
        return FILE_PATH_TOO_LONG;
      }
      result = strHelper->startImageUpload(std::string(reinterpret_cast<const char*>(data), size));
      if (result != returnvalue::OK) {
        return result;
      }
      strHelperHandlingSpecialRequest = true;
      return EXECUTION_FINISHED;
    }
    case (startracker::DOWNLOAD_IMAGE): {
      result = DeviceHandlerBase::acceptExternalDeviceCommands();
      if (result != returnvalue::OK) {
        return result;
      }
      if (size > config::MAX_PATH_SIZE) {
        return FILE_PATH_TOO_LONG;
      }
      result =
          strHelper->startImageDownload(std::string(reinterpret_cast<const char*>(data), size));
      if (result != returnvalue::OK) {
        return result;
      }
      strHelperHandlingSpecialRequest = true;
      return EXECUTION_FINISHED;
    }
    case (startracker::FLASH_READ): {
      result = DeviceHandlerBase::acceptExternalDeviceCommands();
      if (result != returnvalue::OK) {
        return result;
      }
      result = executeFlashReadCommand(data, size);
      if (result != returnvalue::OK) {
        return result;
      }
      strHelperHandlingSpecialRequest = true;
      return EXECUTION_FINISHED;
    }
    case (startracker::CHANGE_IMAGE_DOWNLOAD_FILE): {
      if (size > config::MAX_FILENAME_SIZE) {
        return FILENAME_TOO_LONG;
      }
      strHelper->setDownloadImageName(std::string(reinterpret_cast<const char*>(data), size));
      return EXECUTION_FINISHED;
    }
    case (startracker::SET_FLASH_READ_FILENAME): {
      if (size > config::MAX_FILENAME_SIZE) {
        return FILENAME_TOO_LONG;
      }
      strHelper->setFlashReadFilename(std::string(reinterpret_cast<const char*>(data), size));
      return EXECUTION_FINISHED;
    }
    case (startracker::FIRMWARE_UPDATE_MAIN): {
      return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::MAIN);
    }
    case (startracker::FIRMWARE_UPDATE_BACKUP): {
      return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::BACKUP);
    }
    default:
      break;
  }
  // In case the JSON has changed, reinitiate the next parameter set to update.
  reinitNextSetParam = true;
  return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
}
ReturnValue_t StarTrackerHandler::handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
                                                              startracker::FirmwareTarget target) {
  ReturnValue_t result = DeviceHandlerBase::acceptExternalDeviceCommands();
  if (result != returnvalue::OK) {
    return result;
  }
  if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
    return FILE_PATH_TOO_LONG;
  }
  result = strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size),
                                          target);
  if (result != returnvalue::OK) {
    return result;
  }
  strHelperHandlingSpecialRequest = true;
  return EXECUTION_FINISHED;
}

void StarTrackerHandler::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 << "CCSDSHandler::checkEvents: Did not subscribe to this event message"
                   << std::endl;
        break;
    }
  }
  if (jcfgPending) {
    if (JCFG_DONE) {
      if (startupState == StartupState::WAIT_JCFG) {
        startupState = StartupState::DONE;
      }
      jsonCfgTask.join();
      jcfgPending = false;
      auto iter = deviceCommandMap.find(startracker::RELOAD_JSON_CFG_FILE);
      if (iter != deviceCommandMap.end() and iter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) {
        actionHelper.finish(true, iter->second.sendReplyTo, startracker::RELOAD_JSON_CFG_FILE);
      }
    } else if (jcfgCountdown.hasTimedOut()) {
      auto iter = deviceCommandMap.find(startracker::RELOAD_JSON_CFG_FILE);
      if (iter != deviceCommandMap.end() and iter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) {
        actionHelper.finish(false, iter->second.sendReplyTo, startracker::RELOAD_JSON_CFG_FILE);
      }
    }
  }
}

Submode_t StarTrackerHandler::getInitialSubmode() { return startracker::SUBMODE_BOOTLOADER; }

ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  if (strHelperHandlingSpecialRequest) {
    return NOTHING_TO_SEND;
  }
  switch (normalState) {
    case NormalState::SECONDARY_REQUEST:
      if (additionalRequestedTm.size() == 0) {
        break;
      }
      *id = *currentSecondaryTmIter;
      currentSecondaryTmIter++;
      if (currentSecondaryTmIter == additionalRequestedTm.end()) {
        currentSecondaryTmIter = additionalRequestedTm.begin();
      }
      normalState = NormalState::SOLUTION_REQUEST;
      break;
    case NormalState::SOLUTION_REQUEST:
      *id = startracker::REQ_SOLUTION;
      normalState = NormalState::SECONDARY_REQUEST;
      break;
    default:
      sif::debug << "StarTrackerHandler::buildNormalDeviceCommand: Invalid normal step"
                 << std::endl;
      return NOTHING_TO_SEND;
  }
  return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  switch (internalState) {
    case InternalState::BOOT_FIRMWARE: {
      if (bootState == FwBootState::VERIFY_BOOT or isAwaitingReply()) {
        return NOTHING_TO_SEND;
      }
      if (bootState == FwBootState::NONE) {
        *id = startracker::BOOT;
        bootCountdown.setTimeout(BOOT_TIMEOUT);
        bootState = FwBootState::BOOT_DELAY;
        return buildCommandFromCommand(*id, nullptr, 0);
      }
      if (bootState == FwBootState::BOOT_DELAY) {
        if (bootCountdown.isBusy()) {
          return NOTHING_TO_SEND;
        }
        // Was already done.
        reinitNextSetParam = false;
        bootState = FwBootState::REQ_VERSION;
      }
      switch (bootState) {
        case (FwBootState::REQ_VERSION): {
          bootState = FwBootState::VERIFY_BOOT;
          // Again read program to check if firmware boot was successful
          *id = startracker::REQ_VERSION;
          return buildCommandFromCommand(*id, nullptr, 0);
        }
        case (FwBootState::SET_TIME): {
          *id = startracker::SET_TIME_FROM_SYS_TIME;
          return buildCommandFromCommand(*id, nullptr, 0);
        }
        case (FwBootState::LOGLEVEL): {
          *id = startracker::LOGLEVEL;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        }
        case (FwBootState::LIMITS): {
          *id = startracker::LIMITS;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        }
        case (FwBootState::TRACKING): {
          *id = startracker::TRACKING;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        }
        case FwBootState::MOUNTING:
          *id = startracker::MOUNTING;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::IMAGE_PROCESSOR:
          *id = startracker::IMAGE_PROCESSOR;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::CAMERA:
          *id = startracker::CAMERA;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::CENTROIDING:
          *id = startracker::CENTROIDING;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::LISA:
          *id = startracker::LISA;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::MATCHING:
          *id = startracker::MATCHING;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::VALIDATION:
          *id = startracker::VALIDATION;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::ALGO:
          *id = startracker::ALGO;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::LOG_SUBSCRIPTION:
          *id = startracker::LOGSUBSCRIPTION;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::DEBUG_CAMERA:
          *id = startracker::DEBUG_CAMERA;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        case FwBootState::AUTO_THRESHOLD:
          *id = startracker::AUTO_THRESHOLD;
          return buildCommandFromCommand(
              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
        default: {
          sif::error << "STR: Unexpected boot state" << (int)bootState << std::endl;
          return NOTHING_TO_SEND;
        }
      }
    }
    case InternalState::BOOT_BOOTLOADER:
      internalState = InternalState::BOOTLOADER_CHECK;
      *id = startracker::SWITCH_TO_BOOTLOADER_PROGRAM;
      return buildCommandFromCommand(*id, nullptr, 0);
    case InternalState::BOOTLOADER_CHECK:
      *id = startracker::REQ_VERSION;
      return buildCommandFromCommand(*id, nullptr, 0);
    default:
      break;
  }
  switch (startupState) {
    case StartupState::CHECK_PROGRAM:
      startupState = StartupState::WAIT_CHECK_PROGRAM;
      *id = startracker::REQ_VERSION;
      return buildCommandFromCommand(*id, nullptr, 0);
      break;
    case StartupState::BOOT_BOOTLOADER:
      startupState = StartupState::CHECK_PROGRAM;
      *id = startracker::SWITCH_TO_BOOTLOADER_PROGRAM;
      return buildCommandFromCommand(*id, nullptr, 0);
      break;
    default:
      break;
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                          const uint8_t* commandData,
                                                          size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
  switch (deviceCommand) {
    case (startracker::PING_REQUEST): {
      preparePingRequest();
      return returnvalue::OK;
    }
    case (startracker::RELOAD_JSON_CFG_FILE): {
      if (jcfgPending) {
        return HasActionsIF::IS_BUSY;
      }
      // It should be noted that this just reloads the JSON config structure in memory from the
      // JSON file. The configuration still needs to be sent to the STR. The easiest way to achieve
      // this is to simply reboot the device after a reload.
      reloadJsonCfgFile();
      return returnvalue::OK;
    }
    case (startracker::SET_TIME_FROM_SYS_TIME): {
      SetTimeActionRequest setTimeRequest{};
      timeval tv;
      Clock::getClock(&tv);
      setTimeRequest.unixTime =
          (static_cast<uint64_t>(tv.tv_sec) * 1000 * 1000) + (static_cast<uint64_t>(tv.tv_usec));
      prv_arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen);
      size_t serLen = 0;
      // Time in milliseconds. Manual serialization because arcsec API ignores endianness.
      SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen,
                                  sizeof(commandBuffer) - 2, SerializeIF::Endianness::LITTLE);
      rawPacket = commandBuffer;
      return returnvalue::OK;
    }

    case (startracker::REQ_TIME): {
      prepareTimeRequest();
      return returnvalue::OK;
    }
    case (startracker::REQ_CENTROID): {
      prepareRequestCentroidTm();
      return returnvalue::OK;
    }
    case (startracker::REQ_CENTROIDS): {
      prepareRequestCentroidsTm();
      return returnvalue::OK;
    }
    case (startracker::REQ_CONTRAST): {
      prepareRequestContrastTm();
      return returnvalue::OK;
    }
    case (startracker::REQ_BLOB_STATS): {
      prepareRequestBlobStatsTm();
      return returnvalue::OK;
    }
    case (startracker::BOOT): {
      prepareBootCommand(static_cast<startracker::FirmwareTarget>(firmwareTargetRaw));
      return returnvalue::OK;
    }
    case (startracker::REQ_VERSION): {
      prepareVersionRequest();
      return returnvalue::OK;
    }
    case (startracker::REQ_INTERFACE): {
      prepareInterfaceRequest();
      return returnvalue::OK;
    }
    case (startracker::REQ_POWER): {
      preparePowerRequest();
      return returnvalue::OK;
    }
    case (startracker::SWITCH_TO_BOOTLOADER_PROGRAM): {
      prepareSwitchToBootloaderCmd();
      return returnvalue::OK;
    }
    case (startracker::TAKE_IMAGE): {
      prepareTakeImageCommand(commandData);
      return returnvalue::OK;
    }
    case (startracker::SUBSCRIPTION): {
      result =
          prepareParamCommand(commandData, commandDataLen, jcfgs.subscription, reinitNextSetParam);
      return returnvalue::OK;
    }
    case (startracker::REQ_SOLUTION): {
      prepareSolutionRequest();
      return returnvalue::OK;
    }
    case (startracker::REQ_TEMPERATURE): {
      prepareTemperatureRequest();
      return returnvalue::OK;
    }
    case (startracker::REQ_HISTOGRAM): {
      prepareHistogramRequest();
      return returnvalue::OK;
    }
    case (startracker::LIMITS): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.limits, reinitNextSetParam);
      return result;
    }
    case (startracker::MOUNTING): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.mounting, reinitNextSetParam);
      return result;
    }
    case (startracker::IMAGE_PROCESSOR): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.imageProcessor,
                                   reinitNextSetParam);
      return result;
    }
    case (startracker::CAMERA): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.camera, reinitNextSetParam);
      return result;
    }
    case (startracker::CENTROIDING): {
      result =
          prepareParamCommand(commandData, commandDataLen, jcfgs.centroiding, reinitNextSetParam);
      return result;
    }
    case (startracker::LISA): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.lisa, reinitNextSetParam);
      return result;
    }
    case (startracker::MATCHING): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.matching, reinitNextSetParam);
      return result;
    }
    case (startracker::VALIDATION): {
      result =
          prepareParamCommand(commandData, commandDataLen, jcfgs.validation, reinitNextSetParam);
      return result;
    }
    case (startracker::ALGO): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.algo, reinitNextSetParam);
      return result;
    }
    case (startracker::TRACKING): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.tracking, reinitNextSetParam);
      return result;
    }
    case (startracker::LOGLEVEL): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.logLevel, reinitNextSetParam);
      return result;
    }
    case (startracker::AUTO_THRESHOLD): {
      result =
          prepareParamCommand(commandData, commandDataLen, jcfgs.autoThreshold, reinitNextSetParam);
      return result;
    }
    case (startracker::LOGSUBSCRIPTION): {
      result = prepareParamCommand(commandData, commandDataLen, jcfgs.logSubscription,
                                   reinitNextSetParam);
      return result;
    }
    case (startracker::DEBUG_CAMERA): {
      result =
          prepareParamCommand(commandData, commandDataLen, jcfgs.debugCamera, reinitNextSetParam);
      return result;
    }
    case (startracker::CHECKSUM): {
      result = prepareChecksumCommand(commandData, commandDataLen);
      return result;
    }
    case (startracker::REQ_CAMERA): {
      result = prepareRequestCameraParams();
      return result;
    }
    case (startracker::REQ_LIMITS): {
      result = prepareRequestLimitsParams();
      return result;
    }
    case (startracker::REQ_LOG_LEVEL): {
      result = prepareRequestLogLevelParams();
      return result;
    }
    case (startracker::REQ_MOUNTING): {
      result = prepareRequestMountingParams();
      return result;
    }
    case (startracker::REQ_IMAGE_PROCESSOR): {
      result = prepareRequestImageProcessorParams();
      return result;
    }
    case (startracker::REQ_CENTROIDING): {
      result = prepareRequestCentroidingParams();
      return result;
    }
    case (startracker::REQ_LISA): {
      result = prepareRequestLisaParams();
      return result;
    }
    case (startracker::REQ_MATCHED_CENTROIDS): {
      result = prepareRequestMatchedCentroidsTm();
      return result;
    }
    case (startracker::REQ_BLOB): {
      result = prepareRequestBlobTm();
      return result;
    }
    case (startracker::REQ_BLOBS): {
      result = prepareRequestBlobsTm();
      return result;
    }
    case (startracker::REQ_AUTO_BLOB): {
      result = prepareRequestAutoBlobTm();
      return returnvalue::OK;
    }
    case (startracker::REQ_MATCHING): {
      result = prepareRequestMatchingParams();
      return result;
    }
    case (startracker::REQ_TRACKING): {
      result = prepareRequestTrackingParams();
      return result;
    }
    case (startracker::REQ_VALIDATION): {
      result = prepareRequestValidationParams();
      return result;
    }
    case (startracker::REQ_ALGO): {
      result = prepareRequestAlgoParams();
      return result;
    }
    case (startracker::REQ_SUBSCRIPTION): {
      result = prepareRequestSubscriptionParams();
      return result;
    }
    case (startracker::REQ_LOG_SUBSCRIPTION): {
      result = prepareRequestLogSubscriptionParams();
      return result;
    }
    case (startracker::REQ_DEBUG_CAMERA): {
      result = prepareRequestDebugCameraParams();
      return result;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::FAILED;
}

void StarTrackerHandler::fillCommandAndReplyMap() {
  /** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size
   * is specified */
  this->insertInCommandAndReplyMap(startracker::PING_REQUEST, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandMap(startracker::BOOT);
  this->insertInCommandAndReplyMap(startracker::REQ_VERSION, 3, &versionSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_TIME, 3, &timeSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandMap(startracker::UPLOAD_IMAGE);
  this->insertInCommandMap(startracker::DOWNLOAD_IMAGE);
  this->insertInCommandMap(startracker::RELOAD_JSON_CFG_FILE);
  this->insertInCommandAndReplyMap(startracker::REQ_POWER, 3, &powerSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_INTERFACE, 3, &interfaceSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  // Reboot has no reply. Star tracker reboots immediately
  this->insertInCommandMap(startracker::SWITCH_TO_BOOTLOADER_PROGRAM);
  this->insertInCommandAndReplyMap(startracker::SUBSCRIPTION, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_SOLUTION, 3, &solutionSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_TEMPERATURE, 3, &temperatureSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_HISTOGRAM, 3, &histogramSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::LOGLEVEL, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::SET_TIME_FROM_SYS_TIME, 2, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::LOGSUBSCRIPTION, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::DEBUG_CAMERA, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::LIMITS, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::AUTO_THRESHOLD, 2, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::MOUNTING, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::IMAGE_PROCESSOR, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::CAMERA, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::CENTROIDING, 2, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::LISA, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::MATCHING, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::TRACKING, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::VALIDATION, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::ALGO, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::TAKE_IMAGE, 3, nullptr,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::CHECKSUM, 3, &checksumSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_CAMERA, 3, &cameraSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_LIMITS, 3, &limitsSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_LOG_LEVEL, 3, &loglevelSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_MOUNTING, 3, &mountingSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_IMAGE_PROCESSOR, 3, &imageProcessorSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_CENTROIDING, 3, &centroidingSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_LISA, 3, &lisaSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_MATCHING, 3, &matchingSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_TRACKING, 3, &trackingSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_VALIDATION, 3, &validationSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_ALGO, 3, &algoSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_SUBSCRIPTION, 3, &subscriptionSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_LOG_SUBSCRIPTION, 3, &logSubscriptionSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_DEBUG_CAMERA, 3, &debugCameraSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_AUTO_BLOB, 3, &autoBlobSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroidsSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_BLOB, 3, &blobSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_BLOBS, 3, &blobsSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_CENTROID, 3, &centroidSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_CENTROIDS, 3, &centroidsSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
  this->insertInCommandAndReplyMap(startracker::REQ_BLOB_STATS, 3, &blobStatsSet,
                                   startracker::MAX_FRAME_SIZE * 2 + 2);
}

ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
  if (getMode() == MODE_NORMAL && mode == MODE_ON) {
    return TRANS_NOT_ALLOWED;
  }
  switch (mode) {
    case MODE_OFF:
    case MODE_NORMAL:
    case MODE_RAW:
      if (submode == SUBMODE_NONE) {
        return returnvalue::OK;
      } else {
        return INVALID_SUBMODE;
      }
    case MODE_ON:
      if (submode == startracker::SUBMODE_BOOTLOADER || submode == startracker::SUBMODE_FIRMWARE) {
        return returnvalue::OK;
      } else {
        return INVALID_SUBMODE;
      }
    default:
      return HasModesIF::INVALID_MODE;
  }
}

void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  switch (getMode()) {
    case _MODE_TO_ON:
      doOnTransition(subModeFrom);
      break;
    case _MODE_TO_RAW:
      setMode(MODE_RAW);
      break;
    case _MODE_TO_NORMAL:
      doNormalTransition(modeFrom, subModeFrom);
      break;
    case _MODE_POWER_DOWN:
      internalState = InternalState::IDLE;
      startupState = StartupState::IDLE;
      break;
    default:
      break;
  }
}

void StarTrackerHandler::doOnTransition(Submode_t subModeFrom) {
  using namespace startracker;
  uint8_t dhbSubmode = getSubmode();
  // We hide that the transition to submode firmware actually goes through the submode bootloader.
  // This is because the startracker always starts in bootloader mode but we want to allow direct
  // transitions to firmware mode.
  if (startupState == StartupState::DONE) {
    subModeFrom = SUBMODE_BOOTLOADER;
  }
  if (dhbSubmode == SUBMODE_NONE) {
    bootFirmware(MODE_ON);
  }
  if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_FIRMWARE) {
    bootBootloader();
  } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_FIRMWARE) {
    setMode(MODE_ON);
  } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_BOOTLOADER) {
    bootFirmware(MODE_ON);
  } else if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_BOOTLOADER) {
    setMode(MODE_ON);
  } else if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_NONE) {
    setMode(MODE_ON);
  } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_NONE) {
    setMode(MODE_ON);
  }
}

void StarTrackerHandler::doNormalTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  using namespace startracker;
  if (subModeFrom == SUBMODE_FIRMWARE) {
    setMode(MODE_NORMAL);
  } else if (subModeFrom == SUBMODE_BOOTLOADER) {
    bootFirmware(MODE_NORMAL);
  } else if (modeFrom == MODE_NORMAL && subModeFrom == SUBMODE_NONE) {
    // Device handler already in mode normal
    setMode(MODE_NORMAL);
  }
}

void StarTrackerHandler::bootFirmware(Mode_t toMode) {
  switch (internalState) {
    case InternalState::IDLE:
      sif::info << "STR: Booting to firmware mode" << std::endl;
      internalState = InternalState::BOOT_FIRMWARE;
      break;
    case InternalState::BOOT_FIRMWARE:
      break;
    case InternalState::FAILED_FIRMWARE_BOOT:
      internalState = InternalState::IDLE;
      break;
    case InternalState::DONE:
      if (toMode == MODE_NORMAL) {
        setMode(toMode, 0);
      } else {
        setMode(toMode, startracker::SUBMODE_FIRMWARE);
      }
      sif::info << "STR: Firmware boot success" << std::endl;
      solutionSet.setReportingEnabled(true);
      internalState = InternalState::IDLE;
      startupState = StartupState::IDLE;
      break;
    default:
      return;
  }
}

void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, std::string paramJsonFile) {
  cfgs.tracking.init(paramJsonFile);
  cfgs.logLevel.init(paramJsonFile);
  cfgs.logSubscription.init(paramJsonFile);
  cfgs.debugCamera.init(paramJsonFile);
  cfgs.algo.init(paramJsonFile);
  cfgs.validation.init(paramJsonFile);
  cfgs.matching.init(paramJsonFile);
  cfgs.lisa.init(paramJsonFile);
  cfgs.centroiding.init(paramJsonFile);
  cfgs.camera.init(paramJsonFile);
  cfgs.imageProcessor.init(paramJsonFile);
  cfgs.mounting.init(paramJsonFile);
  cfgs.limits.init(paramJsonFile);
  cfgs.subscription.init(paramJsonFile);
  cfgs.autoThreshold.init(paramJsonFile);
  JCFG_DONE = true;
}

ReturnValue_t StarTrackerHandler::statusFieldCheck(const uint8_t* rawFrame) {
  uint8_t status = startracker::getStatusField(rawFrame);
  if (status != startracker::STATUS_OK) {
    sif::warning << "StarTrackerHandler::handleTm: Reply error: "
                 << static_cast<unsigned int>(status) << std::endl;
    return REPLY_ERROR;
  }
  return returnvalue::OK;
}

void StarTrackerHandler::addSecondaryTmForNormalMode(DeviceCommandId_t cmd) {
  additionalRequestedTm.emplace(cmd);
}

void StarTrackerHandler::resetSecondaryTmSet() {
  additionalRequestedTm.clear();
  additionalRequestedTm.emplace(startracker::REQ_TEMPERATURE);
  currentSecondaryTmIter = additionalRequestedTm.begin();
  {
    PoolReadGuard pg(&autoBlobSet);
    if (pg.getReadResult() == returnvalue::OK) {
      autoBlobSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&matchedCentroidsSet);
    if (pg.getReadResult() == returnvalue::OK) {
      matchedCentroidsSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&blobSet);
    if (pg.getReadResult() == returnvalue::OK) {
      blobSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&blobsSet);
    if (pg.getReadResult() == returnvalue::OK) {
      blobsSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&centroidSet);
    if (pg.getReadResult() == returnvalue::OK) {
      centroidSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&contrastSet);
    if (pg.getReadResult() == returnvalue::OK) {
      contrastSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&centroidsSet);
    if (pg.getReadResult() == returnvalue::OK) {
      centroidsSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&histogramSet);
    if (pg.getReadResult() == returnvalue::OK) {
      histogramSet.setValidity(false, true);
    }
  }
  {
    PoolReadGuard pg(&blobStatsSet);
    if (pg.getReadResult() == returnvalue::OK) {
      histogramSet.setValidity(false, true);
    }
  }
}

void StarTrackerHandler::bootBootloader() {
  if (internalState == InternalState::IDLE) {
    internalState = InternalState::BOOT_BOOTLOADER;
  } else if (internalState == InternalState::FAILED_BOOTLOADER_BOOT) {
    internalState = InternalState::IDLE;
  } else if (internalState == InternalState::DONE) {
    internalState = InternalState::IDLE;
    startupState = StartupState::IDLE;
    setMode(MODE_ON);
  }
}

ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                               DeviceCommandId_t* foundId, size_t* foundLen) {
  ReturnValue_t result = returnvalue::OK;

  if (remainingSize == 0) {
    *foundLen = remainingSize;
    return returnvalue::OK;
  }
  if (remainingSize < 2) {
    sif::error << "StarTrackerHandler: Reply packet with length " << remainingSize
               << "  less than "
                  "2 is invalid"
               << std::endl;
    return returnvalue::FAILED;
  }

  switch (startracker::getReplyFrameType(start)) {
    case TMTC_COMM_ERROR: {
      *foundLen = remainingSize;
      triggerEvent(COM_ERROR_REPLY_RECEIVED, start[1]);
      break;
    }
    case TMTC_ACTIONREPLY: {
      *foundLen = remainingSize;
      fullPacketLen = remainingSize;
      return scanForActionReply(startracker::getId(start), foundId);
    }
    case TMTC_SETPARAMREPLY: {
      *foundLen = remainingSize;
      fullPacketLen = remainingSize;
      return scanForSetParameterReply(startracker::getId(start), foundId);
    }
    case TMTC_PARAMREPLY: {
      *foundLen = remainingSize;
      fullPacketLen = remainingSize;
      return scanForGetParameterReply(startracker::getId(start), foundId);
    }
    case TMTC_TELEMETRYREPLYA:
    case TMTC_TELEMETRYREPLY: {
      *foundLen = remainingSize;
      fullPacketLen = remainingSize;
      return scanForTmReply(startracker::getId(start), foundId);
    }
    default: {
      sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
      result = returnvalue::FAILED;
    }
  }
  return result;
}

ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                       const uint8_t* packet) {
  ReturnValue_t result = returnvalue::OK;

  switch (id) {
    case (startracker::SET_TIME_FROM_SYS_TIME): {
      result = handleActionReply(packet);
      break;
    }
    case (startracker::REQ_TIME): {
      result = handleTm(packet, timeSet, "REQ_TIME");
      break;
    }
    case (startracker::PING_REQUEST): {
      result = handlePingReply(packet);
      break;
    }
    case (startracker::BOOT):
    case (startracker::TAKE_IMAGE):
      break;
    case (startracker::CHECKSUM): {
      result = handleChecksumReply(packet);
      break;
    }
    case (startracker::REQ_VERSION): {
      result = handleTm(packet, versionSet, "REQ_VERSION");
      if (result != returnvalue::OK) {
        return result;
      }
      result = checkProgram();
      if (result != returnvalue::OK) {
        return result;
      }
      break;
    }
    case (startracker::REQ_INTERFACE): {
      result = handleTm(packet, interfaceSet, "REQ_INTERFACE");
      break;
    }
    case (startracker::REQ_POWER): {
      result = handleTm(packet, powerSet, "REQ_POWER");
      break;
    }
    case (startracker::REQ_SOLUTION): {
      result = handleSolution(packet);
      break;
    }
    case (startracker::REQ_CONTRAST): {
      result = handleTm(packet, contrastSet, "REQ_CONTRAST");
      break;
    }
    case (startracker::REQ_AUTO_BLOB): {
      result = handleAutoBlobTm(packet);
      break;
    }
    case (startracker::REQ_MATCHED_CENTROIDS): {
      result = handleMatchedCentroidTm(packet);
      break;
    }
    case (startracker::REQ_BLOB): {
      result = handleBlobTm(packet);
      break;
    }
    case (startracker::REQ_BLOBS): {
      result = handleBlobsTm(packet);
      break;
    }
    case (startracker::REQ_CENTROID): {
      result = handleCentroidTm(packet);
      break;
    }
    case (startracker::REQ_CENTROIDS): {
      result = handleCentroidsTm(packet);
      break;
    }
    case (startracker::REQ_TEMPERATURE): {
      result = handleTm(packet, temperatureSet, "REQ_TEMP");
      break;
    }
    case (startracker::REQ_HISTOGRAM): {
      result = handleTm(packet, histogramSet, "REQ_HISTO");
      break;
    }
    case (startracker::REQ_BLOB_STATS): {
      result = handleTm(packet, blobStatsSet, "REQ_BLOB_STATS");
      break;
    }
    case (startracker::SUBSCRIPTION):
    case (startracker::LOGLEVEL):
    case (startracker::LOGSUBSCRIPTION):
    case (startracker::DEBUG_CAMERA):
    case (startracker::LIMITS):
    case (startracker::MOUNTING):
    case (startracker::CAMERA):
    case (startracker::CENTROIDING):
    case (startracker::LISA):
    case (startracker::MATCHING):
    case (startracker::TRACKING):
    case (startracker::VALIDATION):
    case (startracker::IMAGE_PROCESSOR):
    case (startracker::ALGO):
    case (startracker::AUTO_THRESHOLD): {
      result = handleSetParamReply(packet);
      break;
    }
    case (startracker::REQ_CAMERA): {
      handleParamRequest(packet, cameraSet, startracker::CameraSet::SIZE);
      break;
    }
    case (startracker::REQ_LIMITS): {
      handleParamRequest(packet, limitsSet, startracker::LimitsSet::SIZE);
      break;
    }
    case (startracker::REQ_LOG_LEVEL): {
      handleParamRequest(packet, loglevelSet, startracker::LogLevelSet::SIZE);
      break;
    }
    case (startracker::REQ_MOUNTING): {
      handleParamRequest(packet, mountingSet, startracker::MountingSet::SIZE);
      break;
    }
    case (startracker::REQ_IMAGE_PROCESSOR): {
      handleParamRequest(packet, imageProcessorSet, startracker::ImageProcessorSet::SIZE);
      break;
    }
    case (startracker::REQ_CENTROIDING): {
      handleParamRequest(packet, centroidingSet, startracker::CentroidingSet::SIZE);
      break;
    }
    case (startracker::REQ_LISA): {
      handleParamRequest(packet, lisaSet, startracker::LisaSet::SIZE);
      break;
    }
    case (startracker::REQ_MATCHING): {
      handleParamRequest(packet, matchingSet, startracker::MatchingSet::SIZE);
      break;
    }
    case (startracker::REQ_TRACKING): {
      handleParamRequest(packet, trackingSet, startracker::TrackingSet::SIZE);
      break;
    }
    case (startracker::REQ_VALIDATION): {
      handleParamRequest(packet, validationSet, startracker::ValidationSet::SIZE);
      break;
    }
    case (startracker::REQ_ALGO): {
      handleParamRequest(packet, algoSet, startracker::AlgoSet::SIZE);
      break;
    }
    case (startracker::REQ_SUBSCRIPTION): {
      handleParamRequest(packet, subscriptionSet, startracker::SubscriptionSet::SIZE);
      break;
    }
    case (startracker::REQ_LOG_SUBSCRIPTION): {
      handleParamRequest(packet, logSubscriptionSet, startracker::LogSubscriptionSet::SIZE);
      break;
    }
    case (startracker::REQ_DEBUG_CAMERA): {
      handleParamRequest(packet, debugCameraSet, startracker::DebugCameraSet::SIZE);
      break;
    }
    default: {
      sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id
                 << std::endl;
      result = DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return result;
}

void StarTrackerHandler::setNormalDatapoolEntriesInvalid() {}

uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
  return DEFAULT_TRANSITION_DELAY;
}

ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                          LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(startracker::TICKS_TIME_SET, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TIME_TIME_SET, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(startracker::RUN_TIME, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::UNIX_TIME, new PoolEntry<uint64_t>({0}));

  localDataPoolMap.emplace(startracker::TICKS_VERSION_SET, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TIME_VERSION_SET, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(startracker::PROGRAM, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::MAJOR, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::MINOR, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::TICKS_INTERFACE_SET, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TIME_INTERFACE_SET, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(startracker::FRAME_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::CHECKSUM_ERROR_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::SET_PARAM_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::SET_PARAM_REPLY_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::PARAM_REQUEST_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::PARAM_REPLY_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::REQ_TM_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TM_REPLY_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::ACTION_REQ_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::ACTION_REPLY_COUNT, new PoolEntry<uint32_t>({0}));

  localDataPoolMap.emplace(startracker::TICKS_POWER_SET, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TIME_POWER_SET, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(startracker::MCU_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::MCU_VOLTAGE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::FPGA_CORE_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::FPGA_CORE_VOLTAGE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::FPGA_18_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::FPGA_18_VOLTAGE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::FPGA_25_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::FPGA_25_VOLTAGE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_21_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_21_VOLTAGE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_PIX_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_PIX_VOLTAGE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_33_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_33_VOLTAGE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_RES_CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMV_RES_VOLTAGE, new PoolEntry<float>({0}));

  localDataPoolMap.emplace(startracker::TICKS_TEMPERATURE_SET, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TIME_TEMPERATURE_SET, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(startracker::MCU_TEMPERATURE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CMOS_TEMPERATURE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::FPGA_TEMPERATURE, new PoolEntry<float>({0}));

  localDataPoolMap.emplace(startracker::TICKS_SOLUTION_SET, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TIME_SOLUTION_SET, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(startracker::CALI_QW, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CALI_QX, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CALI_QY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CALI_QZ, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACK_CONFIDENCE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACK_QW, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACK_QX, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACK_QY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACK_QZ, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACK_REMOVED, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::STARS_CENTROIDED, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::STARS_MATCHED_DATABASE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LISA_QW, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_QX, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_QY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_QZ, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_PERC_CLOSE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_NR_CLOSE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::STR_MODE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::TRUST_WORTHY, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::STABLE_COUNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::SOLUTION_STRATEGY, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::TICKS_HISTOGRAM_SET, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::TIME_HISTOGRAM_SET, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA0, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA2, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA3, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA4, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA5, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA6, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA7, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINA8, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB0, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB2, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB3, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB4, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB5, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB6, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB7, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINB8, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC0, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC2, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC3, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC4, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC5, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC6, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC7, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BINC8, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND0, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND2, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND3, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND4, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND5, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND6, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND7, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::HISTOGRAM_BIND8, new PoolEntry<uint32_t>({0}));

  localDataPoolMap.emplace(startracker::CAMERA_MODE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::FOCALLENGTH, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::EXPOSURE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::INTERVAL, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CAMERA_OFFSET, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(startracker::PGAGAIN, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::ADCGAIN, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG2, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL2, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG3, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL3, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG4, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL4, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG5, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL5, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG6, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL6, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG7, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL7, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_REG8, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_VAL8, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CAM_FREQ_1, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::LIMITS_ACTION, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_FPGA18CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_FPGA25CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_FPGA10CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_MCUCURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_CMOS21CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_CMOSPIXCURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_CMOS33CURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_CMOSVRESCURRENT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LIMITS_CMOSTEMPERATURE, new PoolEntry<float>({0}));

  localDataPoolMap.emplace(startracker::LOGLEVEL1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL2, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL3, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL4, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL5, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL6, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL7, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL8, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL9, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL10, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL11, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL12, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL13, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL14, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL15, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOGLEVEL16, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::MOUNTING_QW, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::MOUNTING_QX, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::MOUNTING_QY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::MOUNTING_QZ, new PoolEntry<float>({0}));

  localDataPoolMap.emplace(startracker::IMAGE_PROCESSOR_MODE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::IMAGE_PROCESSOR_STORE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::IMAGE_PROCESSOR_SIGNALTHRESHOLD,
                           new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(startracker::IMAGE_PROCESSOR_DARKTHRESHOLD,
                           new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(startracker::IMAGE_PROCESSOR_BACKGROUNDCOMPENSATION,
                           new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::CENTROIDING_ENABLE_FILTER, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_MAX_QUALITY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_DARK_THRESHOLD, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_MIN_QUALITY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_MAX_INTENSITY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_MIN_INTENSITY, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_MAX_MAGNITUDE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_GAUSSIAN_CMAX, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_GAUSSIAN_CMIN, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_TRANSMATRIX00, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_TRANSMATRIX01, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_TRANSMATRIX10, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::CENTROIDING_TRANSMATRIX11, new PoolEntry<float>({0}));

  localDataPoolMap.emplace(startracker::LISA_MODE, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::LISA_PREFILTER_DIST_THRESHOLD, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_PREFILTER_ANGLE_THRESHOLD, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_FOV_WIDTH, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_FOV_HEIGHT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_FLOAT_STAR_LIMIT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_CLOSE_STAR_LIMIT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_RATING_WEIGHT_CLOSE_STAR_COUNT,
                           new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_RATING_WEIGHT_FRACTION_CLOSE,
                           new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_RATING_WEIGHT_MEAN_SUM, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_RATING_WEIGHT_DB_STAR_COUNT,
                           new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::LISA_MAX_COMBINATIONS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LISA_NR_STARS_STOP, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LISA_FRACTION_CLOSE_STOP, new PoolEntry<float>({0}));

  localDataPoolMap.emplace(startracker::MATCHING_SQUARED_DISTANCE_LIMIT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::MATCHING_SQUARED_SHIFT_LIMIT, new PoolEntry<float>({0}));

  localDataPoolMap.emplace(startracker::TRACKING_THIN_LIMIT, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACKING_OUTLIER_THRESHOLD, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACKING_OUTLIER_THRESHOLD_QUEST,
                           new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::TRACKING_TRACKER_CHOICE, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::VALIDATION_STABLE_COUNT, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::VALIDATION_MAX_DIFFERENCE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::VALIDATION_MIN_TRACKER_CONFIDENCE,
                           new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::VALIDATION_MIN_MATCHED_STARS, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::ALGO_MODE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::ALGO_I2T_MIN_CONFIDENCE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::ALGO_I2T_MIN_MATCHED, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::ALGO_I2L_MIN_CONFIDENCE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(startracker::ALGO_I2L_MIN_MATCHED, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM2, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM3, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM4, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM5, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM6, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM7, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM8, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM9, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM10, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM11, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM12, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM13, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM14, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM15, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::SUBSCRIPTION_TM16, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::LOG_SUBSCRIPTION_LEVEL1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOG_SUBSCRIPTION_MODULE1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOG_SUBSCRIPTION_LEVEL2, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(startracker::LOG_SUBSCRIPTION_MODULE2, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(startracker::DEBUG_CAMERA_TIMING, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(startracker::DEBUG_CAMERA_TEST, new PoolEntry<uint32_t>({0}));

  localDataPoolMap.emplace(startracker::CHKSUM, new PoolEntry<uint32_t>({0}));

  localDataPoolMap.emplace(startracker::TICKS_AUTO_BLOB, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::TIME_AUTO_BLOB, new PoolEntry<uint64_t>());
  localDataPoolMap.emplace(startracker::AUTO_BLOB_THRESHOLD, new PoolEntry<float>());

  localDataPoolMap.emplace(startracker::PoolIds::NUM_MATCHED_CENTROIDS, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_STAR_IDS,
                           new PoolEntry<uint32_t>(16));
  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_COORDS,
                           new PoolEntry<float>(16));
  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_COORDS,
                           new PoolEntry<float>(16));
  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_ERRORS,
                           new PoolEntry<float>(16));
  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_ERRORS,
                           new PoolEntry<float>(16));
  localDataPoolMap.emplace(startracker::PoolIds::TICKS_MATCHED_CENTROIDS,
                           new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::PoolIds::TIME_MATCHED_CENTROIDS, new PoolEntry<uint64_t>());

  localDataPoolMap.emplace(startracker::PoolIds::BLOB_TICKS, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOB_TIME, new PoolEntry<uint64_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOB_COUNT, new PoolEntry<uint32_t>());

  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_TICKS, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_TIME, new PoolEntry<uint64_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_COUNT, new PoolEntry<uint16_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_COUNT_USED, new PoolEntry<uint16_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_NR_4LINES_SKIPPED,
                           new PoolEntry<uint16_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_X_COORDS, new PoolEntry<uint16_t>(8));
  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_Y_COORDS, new PoolEntry<uint16_t>(8));

  localDataPoolMap.emplace(startracker::PoolIds::CENTROID_TICKS, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::PoolIds::CENTROID_TIME, new PoolEntry<uint64_t>());
  localDataPoolMap.emplace(startracker::PoolIds::CENTROID_COUNT, new PoolEntry<uint32_t>());

  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_TICKS, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_TIME, new PoolEntry<uint64_t>());
  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_COUNT, new PoolEntry<uint16_t>());
  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_X_COORDS, new PoolEntry<float>(16));
  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_Y_COORDS, new PoolEntry<float>(16));
  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_MAGNITUDES, new PoolEntry<uint8_t>(16));

  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_TICKS, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_TIME, new PoolEntry<uint64_t>());
  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_A, new PoolEntry<uint32_t>(9));
  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_B, new PoolEntry<uint32_t>(9));
  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry<uint32_t>(9));
  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry<uint32_t>(9));

  localDataPoolMap.emplace(startracker::TICKS_BLOB_STATS, new PoolEntry<uint32_t>());
  localDataPoolMap.emplace(startracker::TIME_BLOB_STATS, new PoolEntry<uint64_t>());
  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry<uint8_t>(16));
  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry<uint8_t>(16));
  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry<uint8_t>(16));
  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_OFLOW, new PoolEntry<uint8_t>(16));

  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(powerSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(interfaceSet.getSid(), false, 10.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(solutionSet.getSid(), false, 30.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(cameraSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(histogramSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(lisaSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(autoBlobSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(matchedCentroidsSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(blobSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(blobsSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(centroidSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(blobStatsSet.getSid(), false, 10.0));
  return returnvalue::OK;
}

size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId) {
  return startracker::MAX_FRAME_SIZE;
}

ReturnValue_t StarTrackerHandler::doSendReadHook() {
  // Prevent DHB from polling UART during commands executed by the image loader task
  if (strHelperHandlingSpecialRequest) {
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

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

ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) {
  switch (actionId) {
    case startracker::UPLOAD_IMAGE:
    case startracker::DOWNLOAD_IMAGE:
    case startracker::FLASH_READ:
    case startracker::FIRMWARE_UPDATE_BACKUP:
    case startracker::FIRMWARE_UPDATE_MAIN: {
      return DeviceHandlerBase::acceptExternalDeviceCommands();
      default:
        break;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::scanForActionReply(uint8_t replyId, DeviceCommandId_t* foundId) {
  switch (replyId) {
    case (startracker::ID::PING): {
      *foundId = startracker::PING_REQUEST;
      break;
    }
    case (startracker::ID::BOOT): {
      *foundId = startracker::BOOT;
      break;
    }
    case (startracker::ID::TAKE_IMAGE): {
      *foundId = startracker::TAKE_IMAGE;
      break;
    }
    case (startracker::ID::UPLOAD_IMAGE): {
      *foundId = startracker::UPLOAD_IMAGE;
      break;
    }
    case (ARC_ACTION_REQ_SETTIME_ID): {
      *foundId = startracker::SET_TIME_FROM_SYS_TIME;
      break;
    }
    case (startracker::ID::CHECKSUM): {
      *foundId = startracker::CHECKSUM;
      break;
    }
    default:
      sif::warning << "StarTrackerHandler::scanForActionReply: Unknown parameter reply id"
                   << std::endl;
      return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::scanForSetParameterReply(uint8_t replyId,
                                                           DeviceCommandId_t* foundId) {
  switch (replyId) {
    case (startracker::ID::SUBSCRIPTION): {
      *foundId = startracker::SUBSCRIPTION;
      break;
    }
    case (startracker::ID::LIMITS): {
      *foundId = startracker::LIMITS;
      break;
    }
    case (startracker::ID::MOUNTING): {
      *foundId = startracker::MOUNTING;
      break;
    }
    case (startracker::ID::IMAGE_PROCESSOR): {
      *foundId = startracker::IMAGE_PROCESSOR;
      break;
    }
    case (startracker::ID::CAMERA): {
      *foundId = startracker::CAMERA;
      break;
    }
    case (startracker::ID::CENTROIDING): {
      *foundId = startracker::CENTROIDING;
      break;
    }
    case (startracker::ID::LISA): {
      *foundId = startracker::LISA;
      break;
    }
    case (startracker::ID::MATCHING): {
      *foundId = startracker::MATCHING;
      break;
    }
    case (startracker::ID::TRACKING): {
      *foundId = startracker::TRACKING;
      break;
    }
    case (startracker::ID::VALIDATION): {
      *foundId = startracker::VALIDATION;
      break;
    }
    case (startracker::ID::ALGO): {
      *foundId = startracker::ALGO;
      break;
    }
    case (startracker::ID::LOG_LEVEL): {
      *foundId = startracker::LOGLEVEL;
      break;
    }
    case (startracker::ID::DEBUG_CAMERA): {
      *foundId = startracker::DEBUG_CAMERA;
      break;
    }
    case (startracker::ID::AUTO_THRESHOLD): {
      *foundId = startracker::AUTO_THRESHOLD;
      break;
    }
    case (startracker::ID::LOG_SUBSCRIPTION): {
      *foundId = startracker::LOGSUBSCRIPTION;
      break;
    }
    default:
      sif::debug << "StarTrackerHandler::scanForParameterReply: Unknown parameter reply id"
                 << std::endl;
      return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::scanForGetParameterReply(uint8_t replyId,
                                                           DeviceCommandId_t* foundId) {
  switch (replyId) {
    case (startracker::ID::CAMERA): {
      *foundId = startracker::REQ_CAMERA;
      break;
    }
    case (startracker::ID::LIMITS): {
      *foundId = startracker::REQ_LIMITS;
      break;
    }
    case (startracker::ID::LOG_LEVEL): {
      *foundId = startracker::REQ_LOG_LEVEL;
      break;
    }
    case (startracker::ID::MOUNTING): {
      *foundId = startracker::REQ_MOUNTING;
      break;
    }
    case (startracker::ID::IMAGE_PROCESSOR): {
      *foundId = startracker::REQ_IMAGE_PROCESSOR;
      break;
    }
    case (startracker::ID::CENTROIDING): {
      *foundId = startracker::REQ_CENTROIDING;
      break;
    }
    case (startracker::ID::LISA): {
      *foundId = startracker::REQ_LISA;
      break;
    }
    case (startracker::ID::MATCHING): {
      *foundId = startracker::REQ_MATCHING;
      break;
    }
    case (startracker::ID::TRACKING): {
      *foundId = startracker::REQ_TRACKING;
      break;
    }
    case (startracker::ID::VALIDATION): {
      *foundId = startracker::REQ_VALIDATION;
      break;
    }
    case (startracker::ID::ALGO): {
      *foundId = startracker::REQ_ALGO;
      break;
    }
    case (startracker::ID::SUBSCRIPTION): {
      *foundId = startracker::REQ_SUBSCRIPTION;
      break;
    }
    case (startracker::ID::LOG_SUBSCRIPTION): {
      *foundId = startracker::REQ_LOG_SUBSCRIPTION;
      break;
    }
    case (startracker::ID::DEBUG_CAMERA): {
      *foundId = startracker::REQ_DEBUG_CAMERA;
      break;
    }
    default: {
      sif::warning << "tarTrackerHandler::scanForGetParameterReply: UnkNown ID" << std::endl;
      return returnvalue::FAILED;
      break;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandId_t* foundId) {
  switch (replyId) {
    case (startracker::ID::VERSION): {
      *foundId = startracker::REQ_VERSION;
      break;
    }
    case (startracker::ID::INTERFACE): {
      *foundId = startracker::REQ_INTERFACE;
      break;
    }
    case (startracker::ID::POWER): {
      *foundId = startracker::REQ_POWER;
      break;
    }
    case (startracker::ID::TEMPERATURE): {
      *foundId = startracker::REQ_TEMPERATURE;
      break;
    }
    case (startracker::ID::HISTOGRAM): {
      *foundId = startracker::REQ_HISTOGRAM;
      break;
    }
    case (startracker::ID::TIME): {
      *foundId = startracker::REQ_TIME;
      break;
    }
    case (startracker::ID::SOLUTION): {
      *foundId = startracker::REQ_SOLUTION;
      break;
    }
    case (startracker::ID::BLOB): {
      *foundId = startracker::REQ_BLOB;
      break;
    }
    case (startracker::ID::BLOB_STATS): {
      *foundId = startracker::REQ_BLOB_STATS;
      break;
    }
    case (startracker::ID::BLOBS): {
      *foundId = startracker::REQ_BLOBS;
      break;
    }
    case (startracker::ID::CENTROID): {
      *foundId = startracker::REQ_CENTROID;
      break;
    }
    case (startracker::ID::CENTROIDS): {
      *foundId = startracker::REQ_CENTROIDS;
      break;
    }
    case (startracker::ID::AUTO_BLOB): {
      *foundId = startracker::REQ_AUTO_BLOB;
      break;
    }
    case (startracker::ID::MATCHED_CENTROIDS): {
      *foundId = startracker::REQ_MATCHED_CENTROIDS;
      break;
    }
    case (startracker::ID::CONTRAST): {
      *foundId = startracker::REQ_CONTRAST;
      break;
    }
    default: {
      sif::debug << "StarTrackerHandler::scanForTmReply: Reply contains invalid reply ID: "
                 << static_cast<unsigned int>(replyId) << std::endl;
      return returnvalue::FAILED;
      break;
    }
  }
  return returnvalue::OK;
}

void StarTrackerHandler::handleEvent(EventMessage* eventMessage) {
  object_id_t objectId = eventMessage->getReporter();
  switch (objectId) {
    case objects::STR_COM_IF: {
      // All events from image loader signal either that the operation was successful or that it
      // failed
      strHelperHandlingSpecialRequest = false;
      break;
    }
    default:
      sif::debug << "StarTrackerHandler::handleEvent: Did not subscribe to this event" << std::endl;
      break;
  }
}

ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* commandData,
                                                          size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
  if (commandDataLen < FlashReadCmd::MIN_LENGTH) {
    sif::warning << "StarTrackerHandler::executeFlashReadCommand: Command too short" << std::endl;
    return COMMAND_TOO_SHORT;
  }
  uint8_t startRegion = *(commandData);
  uint32_t length;
  size_t size = sizeof(length);
  const uint8_t* lengthPtr = commandData + sizeof(startRegion);
  result = SerializeAdapter::deSerialize(&length, lengthPtr, &size, SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::debug << "StarTrackerHandler::executeFlashReadCommand: Deserialization of length failed"
               << std::endl;
    return result;
  }
  if (commandDataLen - sizeof(startRegion) - sizeof(length) > config::MAX_PATH_SIZE) {
    sif::warning << "StarTrackerHandler::executeFlashReadCommand: Received command with invalid"
                 << " path and filename" << std::endl;
    return FILE_PATH_TOO_LONG;
  }
  const uint8_t* filePtr = commandData + sizeof(startRegion) + sizeof(length);
  std::string fullname = std::string(reinterpret_cast<const char*>(filePtr),
                                     commandDataLen - sizeof(startRegion) - sizeof(length));
  result = strHelper->startFlashRead(fullname, startRegion, length);
  if (result != returnvalue::OK) {
    return result;
  }
  return result;
}

void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) {
  uint32_t length = 0;
  struct BootActionRequest bootRequest = {static_cast<uint8_t>(target)};
  prv_arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandData,
                                                         size_t commandDataLen) {
  struct ChecksumActionRequest req;
  ReturnValue_t result = returnvalue::OK;
  if (commandDataLen != ChecksumCmd::LENGTH) {
    sif::warning << "StarTrackerHandler::prepareChecksumCommand: Invalid length" << std::endl;
    return INVALID_LENGTH;
  }
  req.region = *(commandData);
  size_t size = sizeof(req.address);
  const uint8_t* addressPtr = commandData + ChecksumCmd::ADDRESS_OFFSET;
  result =
      SerializeAdapter::deSerialize(&req.address, addressPtr, &size, SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::debug << "StarTrackerHandler::prepareChecksumCommand: Deserialization of address "
               << "failed" << std::endl;
    return result;
  }
  size = sizeof(req.length);
  const uint8_t* lengthPtr = commandData + ChecksumCmd::LENGTH_OFFSET;
  result =
      SerializeAdapter::deSerialize(&req.length, lengthPtr, &size, SerializeIF::Endianness::BIG);
  if (result != returnvalue::OK) {
    sif::debug << "StarTrackerHandler::prepareChecksumCommand: Deserialization of length failed"
               << std::endl;
    return result;
  }
  uint32_t rawCmdLength = 0;
  prv_arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
  rawPacket = commandBuffer;
  rawPacketLen = rawCmdLength;
  checksumCmd.rememberRegion = req.region;
  checksumCmd.rememberAddress = req.address;
  checksumCmd.rememberLength = req.length;
  return result;
}

void StarTrackerHandler::prepareTimeRequest() {
  uint32_t length = 0;
  arc_tm_pack_time_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::preparePingRequest() {
  uint32_t length = 0;
  struct PingActionRequest pingRequest = {PING_ID};
  prv_arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::prepareVersionRequest() {
  uint32_t length = 0;
  arc_tm_pack_version_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::prepareInterfaceRequest() {
  uint32_t length = 0;
  arc_tm_pack_interface_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::preparePowerRequest() {
  uint32_t length = 0;
  arc_tm_pack_power_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::prepareSwitchToBootloaderCmd() {
  uint32_t length = 0;
  struct RebootActionRequest rebootReq {};
  prv_arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) {
  uint32_t length = 0;
  struct CameraActionRequest camReq;
  camReq.actionid = *commandData;
  prv_arc_pack_camera_action_req(&camReq, commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::prepareSolutionRequest() {
  uint32_t length = 0;
  arc_tm_pack_solution_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::prepareTemperatureRequest() {
  uint32_t length = 0;
  arc_tm_pack_temperature_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

void StarTrackerHandler::prepareHistogramRequest() {
  uint32_t length = 0;
  arc_tm_pack_histogram_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
}

ReturnValue_t StarTrackerHandler::prepareRequestAutoBlobTm() {
  uint32_t length = 0;
  arc_tm_pack_autoblob_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestMatchedCentroidsTm() {
  uint32_t length = 0;
  arc_tm_pack_matchedcentroids_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestBlobTm() {
  uint32_t length = 0;
  arc_tm_pack_blob_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestBlobsTm() {
  uint32_t length = 0;
  arc_tm_pack_blobs_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() {
  uint32_t length = 0;
  arc_tm_pack_centroid_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestBlobStatsTm() {
  uint32_t length = 0;
  arc_tm_pack_blobstats_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() {
  uint32_t length = 0;
  arc_tm_pack_centroids_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestContrastTm() {
  uint32_t length = 0;
  arc_tm_pack_contrast_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
                                                      size_t commandDataLen,
                                                      ArcsecJsonParamBase& paramSet,
                                                      bool reinitSet) {
  // Stopwatch watch;
  ReturnValue_t result = returnvalue::OK;
  if (commandDataLen > config::MAX_PATH_SIZE) {
    return FILE_PATH_TOO_LONG;
  }
  if (reinitSet) {
    result = paramSet.init(paramJsonFile);
    if (result != returnvalue::OK) {
      return result;
    }
  }

  result = paramSet.create(commandBuffer);
  if (result != returnvalue::OK) {
    return result;
  }
  rawPacket = commandBuffer;
  rawPacketLen = paramSet.getSize();
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestCameraParams() {
  uint32_t length = 0;
  arc_pack_camera_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestLimitsParams() {
  uint32_t length = 0;
  arc_pack_limits_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestLogLevelParams() {
  uint32_t length = 0;
  arc_pack_loglevel_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestMountingParams() {
  uint32_t length = 0;
  arc_pack_mounting_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestImageProcessorParams() {
  uint32_t length = 0;
  arc_pack_imageprocessor_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestCentroidingParams() {
  uint32_t length = 0;
  arc_pack_centroiding_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestLisaParams() {
  uint32_t length = 0;
  arc_pack_lisa_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestMatchingParams() {
  uint32_t length = 0;
  arc_pack_matching_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestTrackingParams() {
  uint32_t length = 0;
  arc_pack_tracking_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestValidationParams() {
  uint32_t length = 0;
  arc_pack_validation_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestAlgoParams() {
  uint32_t length = 0;
  arc_pack_algo_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestSubscriptionParams() {
  uint32_t length = 0;
  arc_pack_subscription_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestLogSubscriptionParams() {
  uint32_t length = 0;
  arc_pack_logsubscription_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::prepareRequestDebugCameraParams() {
  uint32_t length = 0;
  arc_pack_debugcamera_parameter_req(commandBuffer, &length);
  rawPacket = commandBuffer;
  rawPacketLen = length;
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleSetParamReply(const uint8_t* rawFrame) {
  uint8_t status = startracker::getStatusField(rawFrame);
  if (status != startracker::STATUS_OK) {
    sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set "
                    "command with parameter ID "
                 << static_cast<unsigned int>(*(rawFrame + PARAMETER_ID_OFFSET)) << std::endl;
    if (internalState != InternalState::IDLE) {
      internalState = InternalState::IDLE;
    }
    return SET_PARAM_FAILED;
  }
  if (internalState != InternalState::IDLE) {
    handleStartup(*rawFrame, *(rawFrame + PARAMETER_ID_OFFSET));
  }
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleActionReply(const uint8_t* rawFrame) {
  uint8_t status = startracker::getStatusField(rawFrame);
  ReturnValue_t result = returnvalue::OK;
  if (status != startracker::STATUS_OK) {
    sif::warning << "StarTrackerHandler::handleActionReply: Failed to execute action "
                 << "command with action ID "
                 << static_cast<unsigned int>(*(rawFrame + ACTION_ID_OFFSET)) << " and status "
                 << static_cast<unsigned int>(status) << std::endl;
    result = ACTION_FAILED;
  }
  if (internalState != InternalState::IDLE) {
    handleStartup(*rawFrame, *(rawFrame + PARAMETER_ID_OFFSET));
  }
  return result;
}

ReturnValue_t StarTrackerHandler::handleChecksumReply(const uint8_t* rawFrame) {
  ReturnValue_t result = returnvalue::OK;
  result = handleActionReply(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  const uint8_t* replyData = rawFrame + ACTION_DATA_OFFSET;
  startracker::ChecksumReply checksumReply(replyData);
  if (checksumReply.getRegion() != checksumCmd.rememberRegion) {
    sif::warning << "StarTrackerHandler::handleChecksumReply: Region mismatch" << std::endl;
    return REGION_MISMATCH;
  }
  if (checksumReply.getAddress() != checksumCmd.rememberAddress) {
    sif::warning << "StarTrackerHandler::handleChecksumReply: Address mismatch" << std::endl;
    return ADDRESS_MISMATCH;
  }
  if (checksumReply.getLength() != checksumCmd.rememberLength) {
    sif::warning << "StarTrackerHandler::handleChecksumReply: Length mismatch" << std::endl;
    return LENGTH_MISSMATCH;
  }
  PoolReadGuard rg(&checksumSet);
  checksumSet.checksum = checksumReply.getChecksum();
  handleDeviceTm(checksumSet, startracker::CHECKSUM);
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
  checksumReply.printChecksum();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleParamRequest(const uint8_t* rawFrame,
                                                     LocalPoolDataSetBase& dataset, size_t size) {
  ReturnValue_t result = returnvalue::OK;
  result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return result;
  }
  const uint8_t* reply = rawFrame + PARAMS_OFFSET;
  dataset.setValidityBufferGeneration(false);
  result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    sif::warning << "StarTrackerHandler::handleParamRequest Deserialization failed" << std::endl;
  }
  dataset.setValidityBufferGeneration(true);
  dataset.setValidity(true, true);
  result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return result;
  }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
  dataset.printSet();
#endif
  return result;
}

ReturnValue_t StarTrackerHandler::handlePingReply(const uint8_t* rawFrame) {
  ReturnValue_t result = returnvalue::OK;
  uint32_t pingId = 0;
  uint8_t status = startracker::getStatusField(rawFrame);
  const uint8_t* buffer = rawFrame + ACTION_DATA_OFFSET;
  size_t size = sizeof(pingId);
  SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::LITTLE);
  sif::info << "StarTracker: Ping status: " << static_cast<unsigned int>(status) << std::endl;
  sif::info << "Ping ID: 0x" << std::hex << pingId << std::endl;
  if (status != startracker::STATUS_OK || pingId != PING_ID) {
    sif::warning << "STR: Ping failed" << std::endl;
    result = PING_FAILED;
  } else {
    sif::info << "STR: Ping OK" << std::endl;
  }
  return result;
}

ReturnValue_t StarTrackerHandler::checkProgram() {
  PoolReadGuard pg(&versionSet);
  switch (versionSet.program.value) {
    case startracker::Program::BOOTLOADER:
      if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
        startupState = StartupState::DONE;
      }
      if (bootState == FwBootState::VERIFY_BOOT) {
        sif::warning << "StarTrackerHandler::checkProgram: Failed to boot firmware" << std::endl;
        // Device handler will run into timeout and fall back to transition source mode
        triggerEvent(BOOTING_FIRMWARE_FAILED_EVENT);
        internalState = InternalState::FAILED_FIRMWARE_BOOT;
      } else if (internalState == InternalState::BOOTLOADER_CHECK) {
        internalState = InternalState::DONE;
      }
      break;
    case startracker::Program::FIRMWARE_BACKUP:
    case startracker::Program::FIRMWARE_MAIN: {
      if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
        startupState = StartupState::BOOT_BOOTLOADER;
      }
      if (bootState == FwBootState::VERIFY_BOOT) {
        bootState = FwBootState::SET_TIME;
      } else if (internalState == InternalState::BOOTLOADER_CHECK) {
        triggerEvent(BOOTING_BOOTLOADER_FAILED_EVENT);
        internalState = InternalState::FAILED_BOOTLOADER_BOOT;
      }
      break;
    }
    default:
      sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID "
                   << static_cast<int>(versionSet.program.value) << std::endl;
      return INVALID_PROGRAM;
  }
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
                                           const char* context) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return result;
  }
  const uint8_t* reply = rawFrame + TICKS_OFFSET;
  dataset.setValidityBufferGeneration(false);
  size_t sizeLeft = fullPacketLen;
  result = dataset.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for " << context << ": 0x"
                 << std::hex << std::setw(4) << result << std::dec << std::endl;
  }
  dataset.setValidityBufferGeneration(true);
  dataset.setValidity(true, true);
  result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return result;
  }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
  dataset.printSet();
#endif
  return result;
}

ReturnValue_t StarTrackerHandler::handleSolution(const uint8_t* rawFrame) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  PoolReadGuard pg(&solutionSet);
  if (pg.getReadResult() != returnvalue::OK) {
    return result;
  }
  const uint8_t* reply = rawFrame + TICKS_OFFSET;
  solutionSet.setValidityBufferGeneration(false);
  size_t sizeLeft = fullPacketLen;
  result = solutionSet.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for solution set: 0x"
                 << std::hex << std::setw(4) << result << std::dec << std::endl;
  }
  solutionSet.setValidityBufferGeneration(true);
  solutionSet.setValidity(true, true);
  solutionSet.caliQw.setValid(solutionSet.isTrustWorthy.value);
  solutionSet.caliQx.setValid(solutionSet.isTrustWorthy.value);
  solutionSet.caliQy.setValid(solutionSet.isTrustWorthy.value);
  solutionSet.caliQz.setValid(solutionSet.isTrustWorthy.value);
  solutionSet.trackQw.setValid(solutionSet.isTrustWorthy.value);
  solutionSet.trackQx.setValid(solutionSet.isTrustWorthy.value);
  solutionSet.trackQy.setValid(solutionSet.isTrustWorthy.value);
  solutionSet.trackQz.setValid(solutionSet.isTrustWorthy.value);
  return result;
}

ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  rawFrame += TICKS_OFFSET;
  size_t remainingLen = fullPacketLen;
  PoolReadGuard pg(&autoBlobSet);
  result = pg.getReadResult();
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&autoBlobSet.ticks, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&autoBlobSet.timeUs, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&autoBlobSet.threshold, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  autoBlobSet.setValidity(true, true);
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFrame) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  rawFrame += TICKS_OFFSET;
  size_t remainingLen = fullPacketLen;
  PoolReadGuard pg(&matchedCentroidsSet);
  result = pg.getReadResult();
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&matchedCentroidsSet.ticks, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&matchedCentroidsSet.timeUs, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&matchedCentroidsSet.numberOfMatchedCentroids, &rawFrame,
                                         &remainingLen, SerializeIF::Endianness::LITTLE);

  if (result != returnvalue::OK) {
    return result;
  }
  // Yeah, we serialize it like that because I can't model anything with that local datapool crap.
  for (unsigned idx = 0; idx < 16; idx++) {
    result = SerializeAdapter::deSerialize(&matchedCentroidsSet.starIds[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
    result = SerializeAdapter::deSerialize(&matchedCentroidsSet.xCoords[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
    result = SerializeAdapter::deSerialize(&matchedCentroidsSet.yCoords[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
    result = SerializeAdapter::deSerialize(&matchedCentroidsSet.xErrors[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
    result = SerializeAdapter::deSerialize(&matchedCentroidsSet.yErrors[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
  }
  matchedCentroidsSet.setValidity(true, true);
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleBlobTm(const uint8_t* rawFrame) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  rawFrame += TICKS_OFFSET;
  size_t remainingLen = fullPacketLen;
  PoolReadGuard pg(&blobsSet);
  result = pg.getReadResult();
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobSet.ticks, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobSet.timeUs, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobSet.blobCount, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  blobSet.setValidity(true, true);
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleBlobsTm(const uint8_t* rawFrame) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  rawFrame += TICKS_OFFSET;
  size_t remainingLen = fullPacketLen;
  PoolReadGuard pg(&blobsSet);
  result = pg.getReadResult();
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobsSet.ticks, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobsSet.timeUs, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobsSet.blobsCount, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobsSet.blobsCountUsed, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&blobsSet.nr4LinesSkipped, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  for (unsigned idx = 0; idx < 8; idx++) {
    result = SerializeAdapter::deSerialize(&blobsSet.xCoords[idx], &rawFrame, &remainingLen,
                                           SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
    result = SerializeAdapter::deSerialize(&blobsSet.yCoords[idx], &rawFrame, &remainingLen,
                                           SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
  }
  blobsSet.setValidity(true, true);
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleCentroidTm(const uint8_t* rawFrame) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  rawFrame += TICKS_OFFSET;
  size_t remainingLen = fullPacketLen;
  PoolReadGuard pg(&centroidsSet);
  result = pg.getReadResult();
  if (result != returnvalue::OK) {
    return result;
  }

  result = SerializeAdapter::deSerialize(&centroidSet.ticks, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&centroidSet.timeUs, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&centroidSet.centroidCount, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  centroidSet.setValidity(true, true);
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleCentroidsTm(const uint8_t* rawFrame) {
  ReturnValue_t result = statusFieldCheck(rawFrame);
  if (result != returnvalue::OK) {
    return result;
  }
  rawFrame += TICKS_OFFSET;
  size_t remainingLen = fullPacketLen;
  PoolReadGuard pg(&centroidsSet);
  result = pg.getReadResult();
  if (result != returnvalue::OK) {
    return result;
  }

  result = SerializeAdapter::deSerialize(&centroidsSet.ticksCentroidsTm, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&centroidsSet.timeUsCentroidsTm, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  result = SerializeAdapter::deSerialize(&centroidsSet.centroidsCount, &rawFrame, &remainingLen,
                                         SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    return result;
  }
  for (unsigned idx = 0; idx < 16; idx++) {
    result = SerializeAdapter::deSerialize(&centroidsSet.centroidsXCoords[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
    result = SerializeAdapter::deSerialize(&centroidsSet.centroidsYCoords[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
    result = SerializeAdapter::deSerialize(&centroidsSet.centroidsMagnitudes[idx], &rawFrame,
                                           &remainingLen, SerializeIF::Endianness::LITTLE);
    if (result != returnvalue::OK) {
      return result;
    }
  }
  centroidsSet.setValidity(true, true);
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::handleActionReplySet(const uint8_t* rawFrame,
                                                       LocalPoolDataSetBase& dataset, size_t size) {
  ReturnValue_t result = returnvalue::OK;
  uint8_t status = startracker::getStatusField(rawFrame);
  if (status != startracker::STATUS_OK) {
    sif::warning << "StarTrackerHandler::handleActionReplySet: Reply error: "
                 << static_cast<unsigned int>(status) << std::endl;
    return REPLY_ERROR;
  }
  result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return result;
  }
  const uint8_t* reply = rawFrame + ACTION_DATA_OFFSET;
  dataset.setValidityBufferGeneration(false);
  result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
  if (result != returnvalue::OK) {
    sif::warning << "StarTrackerHandler::handleActionReplySet Deserialization failed" << std::endl;
  }
  dataset.setValidityBufferGeneration(true);
  dataset.setValidity(true, true);
  result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
  if (result != returnvalue::OK) {
    return result;
  }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
  dataset.printSet();
#endif
  return result;
}

void StarTrackerHandler::handleStartup(uint8_t tmType, uint8_t parameterId) {
  switch (tmType) {
    case (TMTC_ACTIONREPLY): {
      case (ARC_ACTION_REQ_SETTIME_ID): {
        bootState = FwBootState::LOGLEVEL;
        return;
      }
      default: {
        break;
      }
    }
  }
  switch (parameterId) {
    case (startracker::ID::LOG_LEVEL): {
      bootState = FwBootState::LIMITS;
      break;
    }
    case (startracker::ID::LIMITS): {
      bootState = FwBootState::TRACKING;
      break;
    }
    case (ARC_PARAM_TRACKING_ID): {
      bootState = FwBootState::MOUNTING;
      break;
    }
    case (startracker::ID::MOUNTING): {
      bootState = FwBootState::IMAGE_PROCESSOR;
      break;
    }
    case (startracker::ID::IMAGE_PROCESSOR): {
      bootState = FwBootState::CAMERA;
      break;
    }
    case (startracker::ID::CAMERA): {
      bootState = FwBootState::CENTROIDING;
      break;
    }
    case (startracker::ID::CENTROIDING): {
      bootState = FwBootState::LISA;
      break;
    }
    case (startracker::ID::LISA): {
      bootState = FwBootState::MATCHING;
      break;
    }
    case (startracker::ID::MATCHING): {
      bootState = FwBootState::VALIDATION;
      break;
    }
    case (startracker::ID::VALIDATION): {
      bootState = FwBootState::ALGO;
      break;
    }
    case (startracker::ID::ALGO): {
      bootState = FwBootState::LOG_SUBSCRIPTION;
      break;
    }
    case (startracker::ID::LOG_SUBSCRIPTION): {
      bootState = FwBootState::DEBUG_CAMERA;
      break;
    }
    case (startracker::ID::DEBUG_CAMERA): {
      bootState = FwBootState::AUTO_THRESHOLD;
      break;
    }
    case (startracker::ID::AUTO_THRESHOLD): {
      bootState = FwBootState::NONE;
      internalState = InternalState::DONE;
      break;
    }
    default: {
      sif::warning << "StarTrackerHandler::handleStartup: Received parameter reply with unexpected"
                   << " parameter ID " << (int)parameterId << std::endl;
      break;
    }
  }
}

ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
  switch (actionId) {
    case startracker::REQ_INTERFACE:
    case startracker::REQ_TIME:
    case startracker::SWITCH_TO_BOOTLOADER_PROGRAM:
    case startracker::DOWNLOAD_IMAGE:
    case startracker::UPLOAD_IMAGE:
    case startracker::REQ_POWER:
    case startracker::TAKE_IMAGE:
    case startracker::REQ_SOLUTION:
    case startracker::REQ_TEMPERATURE:
    case startracker::REQ_HISTOGRAM:
    case startracker::REQ_CAMERA:
    case startracker::REQ_LIMITS:
    case startracker::REQ_LOG_LEVEL:
    case startracker::REQ_MOUNTING:
    case startracker::REQ_IMAGE_PROCESSOR:
    case startracker::REQ_CENTROIDING:
    case startracker::REQ_LISA:
    case startracker::REQ_MATCHING:
    case startracker::REQ_TRACKING:
    case startracker::REQ_VALIDATION:
    case startracker::REQ_ALGO:
    case startracker::REQ_SUBSCRIPTION:
    case startracker::REQ_LOG_SUBSCRIPTION:
    case startracker::REQ_DEBUG_CAMERA:
    case startracker::REQ_MATCHED_CENTROIDS:
    case startracker::REQ_BLOB:
    case startracker::REQ_BLOBS:
    case startracker::REQ_BLOB_STATS:
    case startracker::REQ_CENTROID:
    case startracker::REQ_CENTROIDS:
    case startracker::REQ_CONTRAST: {
      if (getMode() == MODE_ON and getSubmode() != startracker::SUBMODE_FIRMWARE) {
        return STARTRACKER_NOT_RUNNING_FIRMWARE;
      }
      break;
    }
    case startracker::FIRMWARE_UPDATE_MAIN:
    case startracker::FIRMWARE_UPDATE_BACKUP:
    case startracker::FLASH_READ:
      if (getMode() != MODE_ON or getSubmode() != startracker::SUBMODE_BOOTLOADER) {
        return STARTRACKER_NOT_RUNNING_BOOTLOADER;
      }
      break;
    default:
      break;
  }
  return returnvalue::OK;
}

ReturnValue_t StarTrackerHandler::acceptExternalDeviceCommands() { return returnvalue::OK; }

ReturnValue_t StarTrackerHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
                                               ParameterWrapper* parameterWrapper,
                                               const ParameterWrapper* newValues,
                                               uint16_t startAtIndex) {
  auto firmwareTargetUpdate = [&](bool persistent) {
    uint8_t value = 0;
    newValues->getElement(&value);
    if (value != static_cast<uint8_t>(startracker::FirmwareTarget::MAIN) &&
        value != static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP)) {
      return HasParametersIF::INVALID_VALUE;
    }
    parameterWrapper->set(firmwareTargetRaw);
    if (persistent) {
      if (sdCardIF.isSdCardUsable(std::nullopt)) {
        const char* prefix = sdCardIF.getCurrentMountPrefix();
        std::filesystem::path path =
            std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
        std::ofstream of(path, std::ofstream::out | std::ofstream::trunc);
        if (value == static_cast<uint8_t>(startracker::FirmwareTarget::MAIN)) {
          of << "main\n";
        } else {
          of << "backup\n";
        }
      } else {
        return HasFileSystemIF::FILESYSTEM_INACTIVE;
      }
    };
    return returnvalue::OK;
  };
  if (uniqueId == startracker::ParamId::FIRMWARE_TARGET) {
    return firmwareTargetUpdate(false);
  }
  if (uniqueId == startracker::ParamId::FIRMWARE_TARGET_PERSISTENT) {
    return firmwareTargetUpdate(true);
  }
  return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
                                         startAtIndex);
}