#include <fstream>

#include "StarTrackerHandler.h"
#include "OBSWConfig.h"
#include "StarTrackerJsonCommands.h"

#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/ipc/QueueFactory.h>

extern "C" {
    #include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
    #include <thirdparty/arcsec_star_tracker/client/generated/actionreq.h>
    #include "common/misc.h"
}

StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
        CookieIF * comCookie, StrHelper* strHelper) :
        DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet(
                this), interfaceSet(this), timeSet(this), solutionSet(this), histogramSet(this),
                contrastSet(this), strHelper(strHelper) {
    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);
}

StarTrackerHandler::~StarTrackerHandler() {
}

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

    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 != RETURN_OK) {
        return result;
    }
    result = manager->subscribeToEventRange(eventQueue->getId(),
            event::getEventId(StrHelper::IMAGE_UPLOAD_FAILED),
            event::getEventId(StrHelper::FLASH_READ_FAILED));
    if (result != RETURN_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;
    }

    result = strHelper->setComIF(communicationInterface);
    if (result != RETURN_OK) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }
    strHelper->setComCookie(comCookie);
    return  RETURN_OK;
}

ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
        const uint8_t* data, size_t size) {

    ReturnValue_t result = RETURN_OK;

    switch(actionId) {
    case(StarTracker::STOP_IMAGE_LOADER): {
        strHelper->stopProcess();
        return EXECUTION_FINISHED;
    }
    case(StarTracker::SET_JSON_FILE_NAME): {
        if (size > MAX_PATH_SIZE) {
            return FILE_PATH_TOO_LONG;
        }
        paramJsonFile = std::string(reinterpret_cast<const char*>(data), size);
        return EXECUTION_FINISHED;
    }
    default:
        break;
    }

    if (strHelperExecuting == true) {
        return IMAGE_LOADER_EXECUTING;
    }

    result = checkMode(actionId);
    if (result != RETURN_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 != RETURN_OK) {
            return result;
        }
        if (size > MAX_PATH_SIZE) {
            return FILE_PATH_TOO_LONG;
        }
        result = strHelper->startImageUpload(
                std::string(reinterpret_cast<const char*>(data), size));
        if (result != RETURN_OK) {
            return result;
        }
        strHelperExecuting = true;
        return EXECUTION_FINISHED;
    }
    case(StarTracker::DOWNLOAD_IMAGE): {
        result = DeviceHandlerBase::acceptExternalDeviceCommands();
        if (result != RETURN_OK) {
            return result;
        }
        if (size > MAX_PATH_SIZE) {
            return FILE_PATH_TOO_LONG;
        }
        result = strHelper->startImageDownload(
                std::string(reinterpret_cast<const char*>(data), size));
        if (result != RETURN_OK) {
            return result;
        }
        strHelperExecuting = true;
        return EXECUTION_FINISHED;
    }
    case(StarTracker::WRITE): {
        result = DeviceHandlerBase::acceptExternalDeviceCommands();
        if (result != RETURN_OK) {
            return result;
        }
        result = executeWriteCommand(data, size);
        if (result != RETURN_OK) {
            return result;
        }

        strHelperExecuting = true;
        return EXECUTION_FINISHED;
    }
    case(StarTracker::READ): {
        result = DeviceHandlerBase::acceptExternalDeviceCommands();
        if (result != RETURN_OK) {
            return result;
        }
        result = executeReadCommand(data, size);
        if (result != RETURN_OK) {
            return result;
        }
        strHelperExecuting = true;
        return EXECUTION_FINISHED;
    }
    case(StarTracker::CHANGE_DOWNLOAD_FILE): {
        if (size > MAX_FILE_NAME) {
            return FILENAME_TOO_LONG;
        }
        strHelper->setDownloadImageName(
                std::string(reinterpret_cast<const char*>(data), size));
        return EXECUTION_FINISHED;
    }
    case(StarTracker::SET_READ_FILENAME): {
        if (size > MAX_FILE_NAME) {
            return FILENAME_TOO_LONG;
        }
        strHelper->setDownloadImageName(
                std::string(reinterpret_cast<const char*>(data), size));
        return EXECUTION_FINISHED;
    }
    default:
        break;
    }
    return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
}

void StarTrackerHandler::performOperationHook() {
    EventMessage event;
    for (ReturnValue_t result = eventQueue->receiveMessage(&event);
            result == RETURN_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;
        }
    }
}

void StarTrackerHandler::doStartUp() {
    switch(startupState) {
    case StartupState::IDLE:
        startupState = StartupState::CHECK_BOOT_STATE;
        return;
    case StartupState::BOOT_DELAY:
        if (bootCountdown.hasTimedOut()) {
            startupState = StartupState::LIMITS;
        }
        return;
    case StartupState::DONE:
        break;
    default:
        return;
    }
    setMode(_MODE_TO_ON);
}

void StarTrackerHandler::doShutDown() {
    // If star tracker is shutdown also stop all running processes in the image loader task
    strHelper->stopProcess();
    setMode(_MODE_POWER_DOWN);
}

void StarTrackerHandler::doOffActivity() {
    startupState = StartupState::IDLE;
}

ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
    switch (internalState) {
    case InternalState::TEMPERATURE_REQUEST:
        *id = StarTracker::REQ_TEMPERATURE;
        break;
    default:
        sif::debug << "StarTrackerHandler::buildNormalDeviceCommand: Invalid internal step"
                << std::endl;
        break;
    }
    return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
    if (mode != _MODE_START_UP) {
        return NOTHING_TO_SEND;
    }
    switch (startupState) {
    case StartupState::CHECK_BOOT_STATE:
        *id = StarTracker::REQ_VERSION;
        startupState = StartupState::WAIT_FOR_EXECUTION;
        return buildCommandFromCommand(*id, nullptr, 0);
    case StartupState::BOOT:
        *id = StarTracker::BOOT;
        bootCountdown.setTimeout(BOOT_TIMEOUT);
        startupState = StartupState::BOOT_DELAY;
        return buildCommandFromCommand(*id, nullptr, 0);
    case StartupState::LIMITS:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::LIMITS;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::TRACKING:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::TRACKING;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::MOUNTING:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::MOUNTING;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::CAMERA:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::CAMERA;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::BLOB:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::BLOB;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::CENTROIDING:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::CENTROIDING;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::LISA:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::LISA;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::MATCHING:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::MATCHING;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::VALIDATION:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::VALIDATION;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    case StartupState::ALGO:
        startupState = StartupState::WAIT_FOR_EXECUTION;
        *id = StarTracker::ALGO;
        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
                paramJsonFile.size());
    default:
        break;
    }
    return NOTHING_TO_SEND;
}

ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
        const uint8_t * commandData, size_t commandDataLen) {
    ReturnValue_t result = RETURN_OK;
    switch (deviceCommand) {
    case (StarTracker::PING_REQUEST): {
        preparePingRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_TIME): {
        prepareTimeRequest();
        return RETURN_OK;
    }
    case (StarTracker::BOOT): {
        prepareBootCommand();
        return RETURN_OK;
    }
    case (StarTracker::REQ_VERSION): {
        prepareVersionRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_INTERFACE): {
        prepareInterfaceRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_POWER): {
        preparePowerRequest();
        return RETURN_OK;
    }
    case (StarTracker::SWITCH_TO_BOOTLOADER_PROGRAM): {
        prepareRebootCommand();
        return RETURN_OK;
    }
    case (StarTracker::TAKE_IMAGE): {
        prepareTakeImageCommand(commandData);
        return RETURN_OK;
    }
    case (StarTracker::SUBSCRIBE_TO_TM): {
        prepareSubscriptionCommand(commandData);
        return RETURN_OK;
    }
    case (StarTracker::REQ_SOLUTION): {
        prepareSolutionRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_TEMPERATURE): {
        prepareTemperatureRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_HISTOGRAM): {
        prepareHistogramRequest();
        return RETURN_OK;
    }
    case (StarTracker::REQ_CONTRAST): {
        prepareContrastRequest();
        return RETURN_OK;
    }
    case (StarTracker::RESET_ERROR): {
        prepareErrorResetRequest();
        return RETURN_OK;
    }
    case (StarTracker::LIMITS): {
        Limits limits;
        result = prepareParamCommand(commandData, commandDataLen, limits);
        return result;
    }
    case (StarTracker::MOUNTING): {
        Mounting mounting;
        result = prepareParamCommand(commandData, commandDataLen, mounting);
        return result;
    }
    case (StarTracker::CAMERA): {
        Camera camera;
        result = prepareParamCommand(commandData, commandDataLen, camera);
        return result;
    }
    case (StarTracker::BLOB): {
        Blob blob;
        result = prepareParamCommand(commandData, commandDataLen, blob);
        return result;
    }
    case (StarTracker::CENTROIDING): {
        Centroiding centroiding;
        result = prepareParamCommand(commandData, commandDataLen, centroiding);
        return result;
    }
    case (StarTracker::LISA): {
        Lisa lisa;
        result = prepareParamCommand(commandData, commandDataLen, lisa);
        return result;
    }
    case (StarTracker::MATCHING): {
        Matching matching;
        result = prepareParamCommand(commandData, commandDataLen, matching);
        return result;
    }
    case (StarTracker::VALIDATION): {
        Validation validation;
        result = prepareParamCommand(commandData, commandDataLen, validation);
        return result;
    }
    case (StarTracker::ALGO): {
        Algo algo;
        result = prepareParamCommand(commandData, commandDataLen, algo);
        return result;
    }
    case (StarTracker::TRACKING): {
        Tracking tracking;
        result = prepareParamCommand(commandData, commandDataLen, tracking);
        return result;
    }
    case (StarTracker::UNLOCK): {
        result = prepareUnlockCommand(commandData, commandDataLen);
        return result;
    }
    case (StarTracker::CHECKSUM): {
        result = prepareChecksumCommand(commandData, commandDataLen);
        return result;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return HasReturnvaluesIF::RETURN_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->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::SUBSCRIBE_TO_TM, 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::REQ_CONTRAST, 3, &contrastSet,
			StarTracker::MAX_FRAME_SIZE * 2 + 2);
    this->insertInCommandAndReplyMap(StarTracker::LIMITS, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
    this->insertInCommandAndReplyMap(StarTracker::MOUNTING, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
    this->insertInCommandAndReplyMap(StarTracker::CAMERA, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
    this->insertInCommandAndReplyMap(StarTracker::BLOB, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
    this->insertInCommandAndReplyMap(StarTracker::CENTROIDING, 3, 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::RESET_ERROR, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
    this->insertInCommandAndReplyMap(StarTracker::UNLOCK, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
    this->insertInCommandAndReplyMap(StarTracker::CHECKSUM, 3, nullptr,
            StarTracker::MAX_FRAME_SIZE * 2 + 2);
}

ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize,
        DeviceCommandId_t *foundId, size_t *foundLen) {

    ReturnValue_t result = RETURN_OK;
    size_t bytesLeft = 0;

    result = dataLinkLayer.decodeFrame(start, remainingSize, &bytesLeft);
    switch(result) {
    case ArcsecDatalinkLayer::DEC_IN_PROGRESS: {
        remainingSize = bytesLeft;
        // Need a second doSendRead pass to reaa in whole packet
        return IGNORE_REPLY_DATA;
    }
    case RETURN_OK: {
        break;
    }
    default:
        remainingSize = bytesLeft;
        return result;
    }

    switch (dataLinkLayer.getReplyFrameType()) {
    case TMTC_ACTIONREPLY: {
        *foundLen = remainingSize - bytesLeft;
        result = scanForActionReply(foundId);
        break;
    }
    case TMTC_SETPARAMREPLY: {
        *foundLen = remainingSize - bytesLeft;
        result = scanForParameterReply(foundId);
        break;
    }
    case TMTC_TELEMETRYREPLYA:
    case TMTC_TELEMETRYREPLY: {
        *foundLen = remainingSize - bytesLeft;
        result = scanForTmReply(foundId);
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
        result = RETURN_FAILED;
    }
    }

    remainingSize = bytesLeft;

    return result;
}

ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {

    ReturnValue_t result = RETURN_OK;

    switch (id) {
    case (StarTracker::SUBSCRIBE_TO_TM): {
        result = handleSetParamReply();
        break;
    }
    case (StarTracker::REQ_TIME): {
        result = handleTm(timeSet, StarTracker::TimeSet::SIZE);
        break;
    }
    case (StarTracker::PING_REQUEST): {
        result = handlePingReply();
        break;
    }
    case (StarTracker::BOOT):
    case (StarTracker::TAKE_IMAGE):
    case (StarTracker::RESET_ERROR):
    case (StarTracker::UNLOCK): {
        result = handleActionReply();
        break;
    }
    case (StarTracker::CHECKSUM): {
        result = handleChecksumReply();
        break;
    }
    case (StarTracker::REQ_VERSION): {
        result = handleTm(versionSet, StarTracker::VersionSet::SIZE);
        if (result != RETURN_OK) {
            return result;
        }
        result = checkProgram();
        if (result != RETURN_OK) {
            return result;
        }
        break;
    }
    case (StarTracker::REQ_INTERFACE): {
        result = handleTm(interfaceSet, StarTracker::InterfaceSet::SIZE);
        break;
    }
    case (StarTracker::REQ_POWER): {
        result = handleTm(powerSet, StarTracker::PowerSet::SIZE);
        break;
    }
    case (StarTracker::REQ_SOLUTION): {
        result = handleTm(solutionSet, StarTracker::SolutionSet::SIZE);
        break;
    }
    case (StarTracker::REQ_TEMPERATURE): {
        result = handleTm(temperatureSet, StarTracker::TemperatureSet::SIZE);
        break;
    }
    case (StarTracker::REQ_HISTOGRAM): {
        result = handleTm(histogramSet, StarTracker::HistogramSet::SIZE);
        break;
    }
    case (StarTracker::REQ_CONTRAST): {
        result = handleTm(contrastSet, StarTracker::ContrastSet::SIZE);
        break;
    }
    case (StarTracker::LIMITS):
    case (StarTracker::MOUNTING):
    case (StarTracker::CAMERA):
    case (StarTracker::BLOB):
    case (StarTracker::CENTROIDING):
    case (StarTracker::LISA):
    case (StarTracker::MATCHING):
    case (StarTracker::TRACKING):
    case (StarTracker::VALIDATION):
    case (StarTracker::ALGO): {
        result = handleSetParamReply();
        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 20000;
}

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::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::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::TICKS_CONTRAST_SET, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::TIME_CONTRAST_SET, new PoolEntry<uint64_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA0, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA1, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA2, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA3, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA4, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA5, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA6, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA7, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINA8, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB0, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB1, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB2, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB3, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB4, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB5, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB6, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB7, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINB8, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC0, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC1, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC2, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC3, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC4, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC5, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC6, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC7, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND0, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND1, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND2, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND3, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND4, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND5, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND6, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND7, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(StarTracker::CONTRAST_BIND8, new PoolEntry<uint32_t>( { 0 }));

    return RETURN_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(strHelperExecuting) {
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) {
    switch(actionId) {
    case StarTracker::UPLOAD_IMAGE:
    case StarTracker::DOWNLOAD_IMAGE: {
        return DeviceHandlerBase::acceptExternalDeviceCommands();
    default:
        break;
    }
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) {
    const uint8_t* reply = dataLinkLayer.getReply();
    switch (*reply) {
    case (StarTracker::ID::PING): {
        *foundId = StarTracker::PING_REQUEST;
        break;
    }
    case (StarTracker::ID::WRITE): {
        *foundId = StarTracker::WRITE;
        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 (StarTracker::ID::ERROR_RESET): {
        *foundId = StarTracker::RESET_ERROR;
        break;
    }
    case (StarTracker::ID::UNLOCK): {
        *foundId = StarTracker::UNLOCK;
        break;
    }
    case (StarTracker::ID::CHECKSUM): {
        *foundId = StarTracker::CHECKSUM;
        break;
    }
    default:
        sif::warning << "StarTrackerHandler::scanForParameterReply: Unknown parameter reply id"
            << std::endl;
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::scanForParameterReply(DeviceCommandId_t *foundId) {
    const uint8_t* reply = dataLinkLayer.getReply();
    switch (*reply) {
    case (StarTracker::ID::SUBSCRIBE): {
        *foundId = StarTracker::SUBSCRIBE_TO_TM;
        break;
    }
    case (StarTracker::ID::LIMITS): {
        *foundId = StarTracker::LIMITS;
        break;
    }
    case (StarTracker::ID::MOUNTING): {
        *foundId = StarTracker::MOUNTING;
        break;
    }
    case (StarTracker::ID::CAMERA): {
        *foundId = StarTracker::CAMERA;
        break;
    }
    case (StarTracker::ID::BLOB): {
        *foundId = StarTracker::BLOB;
        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;
    }
    default:
        sif::debug << "StarTrackerHandler::scanForParameterReply: Unknown parameter reply id"
            << std::endl;
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
    const uint8_t* reply = dataLinkLayer.getReply();
    switch (*reply) {
        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::CONTRAST): {
            *foundId = StarTracker::REQ_CONTRAST;
            break;
        }
        case (StarTracker::ID::TIME): {
            *foundId = StarTracker::REQ_TIME;
            break;
        }
        case (StarTracker::ID::SOLUTION): {
            *foundId = StarTracker::REQ_SOLUTION;
            break;
        }
        default: {
            sif::debug << "StarTrackerHandler::scanForTmReply: Reply contains invalid reply id"
                    << std::endl;
            return RETURN_FAILED;
            break;
        }
    }
    return RETURN_OK;
}

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

ReturnValue_t StarTrackerHandler::executeWriteCommand(const uint8_t* commandData,
        size_t commandDataLen) {
    ReturnValue_t result = RETURN_OK;
    if (commandDataLen < WriteCmd::MIN_LENGTH) {
        sif::warning << "StarTrackerHandler::executeWriteCommand: Command too short"  << std::endl;
        return COMMAND_TOO_SHORT;
    }
    uint8_t region = *(commandData);
    uint32_t address;
    size_t size = sizeof(address);
    const uint8_t* addressPtr = commandData + WriteCmd::ADDRESS_OFFSET;
    result = SerializeAdapter::deSerialize(&address, addressPtr, &size,
                SerializeIF::Endianness::BIG);
    if (result != RETURN_OK) {
        sif::debug << "StarTrackerHandler::executeWriteCommand: Deserialization of address failed"
                << std::endl;
        return result;
    }
    if (commandDataLen - sizeof(address) - sizeof(region) > MAX_PATH_SIZE) {
        sif::warning << "StarTrackerHandler::executeWriteCommand: Received command with invalid"
                << " path and filename" << std::endl;
        return FILE_PATH_TOO_LONG;
    }
    const uint8_t* filePtr = commandData + WriteCmd::FILE_OFFSET;
    std::string fullname = std::string(reinterpret_cast<const char*>(filePtr),
            commandDataLen - sizeof(address) - sizeof(region));
    result = strHelper->startFlashWrite(fullname, region, address);
    if (result != RETURN_OK) {
        return result;
    }
    return result;
}

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

void StarTrackerHandler::prepareBootCommand() {
    uint32_t length = 0;
    struct BootActionRequest bootRequest = {BOOT_REGION_ID};
    arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

ReturnValue_t StarTrackerHandler::prepareUnlockCommand(const uint8_t* commandData, size_t commandDataLen) {
    ReturnValue_t result = RETURN_OK;
    uint32_t length = 0;
    struct UnlockActionRequest req;
    req.region = *commandData;
    size_t size = sizeof(req.code);
    const uint8_t* codePtr = commandData + UnlockCmd::CODE_OFFSET;
    result = SerializeAdapter::deSerialize(&req.code, &codePtr, &size,
            SerializeIF::Endianness::BIG);
    if (result != RETURN_OK) {
        return result;
    }
    arc_pack_unlock_action_req(&req, commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
    return result;
}

ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandData,
        size_t commandDataLen) {
    struct ChecksumActionRequest req;
    ReturnValue_t result = RETURN_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 != RETURN_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 != RETURN_OK) {
        sif::debug << "StarTrackerHandler::prepareChecksumCommand: Deserialization of length failed"
                << std::endl;
        return result;
    }
    uint32_t rawCmdLength = 0;
    arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
    dataLinkLayer.encodeFrame(commandBuffer, rawCmdLength);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
    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);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::preparePingRequest() {
    uint32_t length = 0;
    struct PingActionRequest pingRequest = {PING_ID};
    arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareVersionRequest() {
    uint32_t length = 0;
    arc_tm_pack_version_req(commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareInterfaceRequest() {
    uint32_t length = 0;
    arc_tm_pack_interface_req(commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::preparePowerRequest() {
    uint32_t length = 0;
    arc_tm_pack_power_req(commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareRebootCommand() {
    uint32_t length = 0;
    struct RebootActionRequest rebootReq;
    arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

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

void StarTrackerHandler::prepareSubscriptionCommand(const uint8_t* tmId) {
    uint32_t length = 18;
    commandBuffer[0] = TMTC_SETPARAMREQ;
    commandBuffer[1] = StarTracker::ID::SUBSCRIBE;
    // Fill all other fields with invalid tm id
    commandBuffer[2] = *tmId;
    commandBuffer[3] = 0;
    commandBuffer[4] = 0;
    commandBuffer[5] = 0;
    commandBuffer[6] = 0;
    commandBuffer[7] = 0;
    commandBuffer[8] = 0;
    commandBuffer[9] = 0;
    commandBuffer[10] = 0;
    commandBuffer[11] = 0;
    commandBuffer[12] = 0;
    commandBuffer[13] = 0;
    commandBuffer[14] = 0;
    commandBuffer[15] = 0;
    commandBuffer[16] = 0;
    commandBuffer[17] = 0;
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareSolutionRequest() {
    uint32_t length = 0;
    arc_tm_pack_solution_req(commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareTemperatureRequest() {
    uint32_t length = 0;
    arc_tm_pack_temperature_req(commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareHistogramRequest() {
    uint32_t length = 0;
    arc_tm_pack_histogram_req(commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareContrastRequest() {
    uint32_t length = 0;
    arc_tm_pack_contrast_req(commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

void StarTrackerHandler::prepareErrorResetRequest() {
    uint32_t length = 0;
    struct ResetErrorSignalActionRequest req;
    arc_pack_reseterrorsignal_action_req(&req, commandBuffer, &length);
    dataLinkLayer.encodeFrame(commandBuffer, length);
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
}

ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
        size_t commandDataLen, ArcsecJsonParamBase& paramSet) {
    ReturnValue_t result = RETURN_OK;
    if (commandDataLen > MAX_PATH_SIZE) {
        return FILE_PATH_TOO_LONG;
    }
    std::string fullName(reinterpret_cast<const char*>(commandData), commandDataLen);

    result = paramSet.create(fullName, commandBuffer);
    if (result != RETURN_OK) {
        sif::warning << "StarTrackerHandler::prepareParamCommand: Failed to create parameter "
                "command" << std::endl;
        return result;
    }
    dataLinkLayer.encodeFrame(commandBuffer, paramSet.getSize());
    rawPacket = dataLinkLayer.getEncodedFrame();
    rawPacketLen = dataLinkLayer.getEncodedLength();
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::handleSetParamReply() {
    const uint8_t* reply = dataLinkLayer.getReply();
    uint8_t status = *(reply + STATUS_OFFSET);
    if (status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set "
                " command with parameter ID" <<
                static_cast<unsigned int>(*(reply + PARAMETER_ID_OFFSET)) << std::endl;
        if (startupState != StartupState::IDLE) {
            startupState = StartupState::IDLE;
        }
        return SET_PARAM_FAILED;
    }
    if (startupState != StartupState::IDLE) {
        handleStartup(reply + PARAMETER_ID_OFFSET);
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::handleActionReply() {
    const uint8_t* reply = dataLinkLayer.getReply();
    uint8_t status = *(reply + STATUS_OFFSET);
    if (status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleActionReply: Failed to execute action "
                << " command with action ID "
                << static_cast<unsigned int>(*(reply + ACTION_ID_OFFSET))
                << " and status "<< static_cast<unsigned int>(status) << std::endl;
        return ACTION_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::handleChecksumReply() {
    ReturnValue_t result = RETURN_OK;
    result = handleActionReply();
    if (result != RETURN_OK) {
        return result;
    }
    const uint8_t* replyData = dataLinkLayer.getReply() + 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;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    checksumReply.printChecksum();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::handlePingReply() {
    ReturnValue_t result = RETURN_OK;
    uint32_t pingId = 0;
    const uint8_t* reply = dataLinkLayer.getReply();
    uint8_t status = dataLinkLayer.getStatusField();
    const uint8_t* buffer = reply + ACTION_DATA_OFFSET;
    size_t size = sizeof(pingId);
    SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::LITTLE);
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    sif::info << "StarTracker: Ping status: "<< static_cast<unsigned int>(status) << std::endl;
    sif::info << "Ping id: 0x"<< std::hex << pingId << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    if (status != StarTracker::STATUS_OK || pingId != PING_ID) {
        sif::warning << "StarTrackerHandler::handlePingReply: Ping failed" << std::endl;
        result = PING_FAILED;
    }
    else {
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
        sif::info << "StarTracker: Ping successful" << std::endl;
#endif
    }
    return result;
}

ReturnValue_t StarTrackerHandler::checkProgram() {
    PoolReadGuard pg(&versionSet);
    switch(versionSet.program.value) {
    case StarTracker::Program::BOOTLOADER:
        // Star tracker currently in bootloader program. Need to send boot command to switch to
        // firmware program
        if (startupState != StartupState::IDLE) {
            startupState = StartupState::BOOT;
        }
        break;
    case StarTracker::Program::FIRMWARE:
        // Firmware already booted
        if (startupState != StartupState::IDLE) {
            startupState = StartupState::LIMITS;
        }
        break;
    default:
        sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID"
            << std::endl;
        return INVALID_PROGRAM;
    }
    return RETURN_OK;
}

ReturnValue_t StarTrackerHandler::handleTm(LocalPoolDataSetBase& dataset, size_t size) {
    ReturnValue_t result = RETURN_OK;
    uint8_t status = *(dataLinkLayer.getReply() + STATUS_OFFSET);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        return TM_REPLY_ERROR;
    }
    result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    const uint8_t* reply = dataLinkLayer.getReply() + TICKS_OFFSET;
    dataset.setValidityBufferGeneration(false);
    result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
    if (result != RETURN_OK) {
        sif::warning << "StarTrackerHandler::handleTm: Deserialization failed"
                << std::endl;
    }
    dataset.setValidityBufferGeneration(true);
    dataset.setValidity(true, true);
    result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    dataset.printSet();
#endif
    return result;
}

void StarTrackerHandler::handleStartup(const uint8_t* parameterId) {
    switch(*parameterId) {
    case (StarTracker::ID::LIMITS): {
        startupState = StartupState::TRACKING;
        break;
    }
    case (StarTracker::ID::TRACKING): {
        startupState = StartupState::MOUNTING;
        break;
    }
    case (StarTracker::ID::MOUNTING): {
        startupState = StartupState::CAMERA;
        break;
    }
    case (StarTracker::ID::CAMERA): {
        startupState = StartupState::BLOB;
        break;
    }
    case (StarTracker::ID::BLOB): {
        startupState = StartupState::CENTROIDING;
        break;
    }
    case (StarTracker::ID::CENTROIDING): {
        startupState = StartupState::LISA;
        break;
    }
    case (StarTracker::ID::LISA): {
        startupState = StartupState::MATCHING;
        break;
    }
    case (StarTracker::ID::MATCHING): {
        startupState = StartupState::VALIDATION;
        break;
    }
    case (StarTracker::ID::VALIDATION): {
        startupState = StartupState::ALGO;
        break;
    }
    case (StarTracker::ID::ALGO): {
        startupState = StartupState::DONE;
        break;
    }
    default: {
        sif::debug << "StarTrackerHandler::handleStartup: Received parameter reply with unexpected"
                << " parameter ID" << std::endl;
        break;
    }
    }
}