#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, StrImageLoader* strImageLoader) :
        DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet(
                this), interfaceSet(this), timeSet(this), solutionSet(this), strImageLoader(
                strImageLoader) {
    if (comCookie == nullptr) {
        sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
    }
    if (strImageLoader == 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(StrImageLoader::IMAGE_UPLOAD_FAILED),
            event::getEventId(StrImageLoader::IMAGE_DOWNLOAD_SUCCESSFUL));
    if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "StarTrackerHandler::initialize: Failed to subscribe to events form image"
                " loader" << std::endl;
#endif
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    result = strImageLoader->setComIF(communicationInterface);
    if (result != RETURN_OK) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }
    strImageLoader->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;

    if (actionId == StarTracker::STOP_IMAGE_LOADER) {
        strImageLoader->stopProcess();
        return EXECUTION_FINISHED;
    }

    if (imageLoaderExecuting == 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 = strImageLoader->startImageUpload(
                std::string(reinterpret_cast<const char*>(data), size));
        if (result != RETURN_OK) {
            return result;
        }
        imageLoaderExecuting = 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 = strImageLoader->startImageDownload(
                std::string(reinterpret_cast<const char*>(data), size));
        if (result != RETURN_OK) {
            return result;
        }
        imageLoaderExecuting = true;
        return EXECUTION_FINISHED;
    }
    case(StarTracker::CHANGE_DOWNLOAD_FILE): {
        if (size > MAX_FILE_NAME) {
            return FILENAME_TOO_LONG;
        }
        strImageLoader->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
    strImageLoader->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::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;
    }
    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::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);
}

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 = handleTimeTm();
        break;
    }
    case (StarTracker::PING_REQUEST): {
        result = handlePingReply();
        break;
    }
    case (StarTracker::BOOT): {
        result = handleActionReply();
        break;
    }
    case (StarTracker::RESET_ERROR): {
        result = handleActionReply();
        break;
    }
    case (StarTracker::TAKE_IMAGE): {
        result = handleActionReply();
        break;
    }
    case (StarTracker::REQ_VERSION): {
        result = handleVersionTm();
        if (result != RETURN_OK) {
            return result;
        }
        result = checkProgram();
        if (result != RETURN_OK) {
            return result;
        }
        break;
    }
    case (StarTracker::REQ_INTERFACE): {
        result = handleInterfaceTm();
        break;
    }
    case (StarTracker::REQ_POWER): {
        result = handlePowerTm();
        break;
    }
    case (StarTracker::REQ_SOLUTION): {
        handleSolutionTm();
        break;
    }
    case (StarTracker::REQ_TEMPERATURE): {
        handleTemperatureTm();
        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 5000;
}

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 }));

    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(imageLoaderExecuting) {
        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::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;
    }
    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::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_IMG_LOADER: {
        // All events from image loader signal either that the operation was successful or that it
        // failed
        imageLoaderExecuting = false;
        break;
    }
    default:
        sif::debug << "StarTrackerHandler::handleEvent: Did not subscribe to this event"
            << std::endl;
        break;
    }
}

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();
}

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::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::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::handleTimeTm() {
    ReturnValue_t result = RETURN_OK;
    result = timeSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleTimeTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = VERSION_REQ_FAILED;
        return result;
    }
    const uint8_t* reply = dataLinkLayer.getReply();
    timeSet.ticks = ticks;
    timeSet.time = time;
    timeSet.runTime = deserializeUint32(reply + offset);
    offset += sizeof(uint32_t);
    timeSet.unixTime = deserializeUint64(reply + offset);
    result = timeSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    timeSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return result;
}

ReturnValue_t StarTrackerHandler::handleVersionTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = VERSION_REQ_FAILED;
        return result;
    }
    result = versionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    const uint8_t* reply = dataLinkLayer.getReply();
    versionSet.ticks = ticks;
    versionSet.time = time;
    versionSet.program = *(reply + offset);
    offset += 1;
    versionSet.major = *(reply + offset);
    offset += 1;
    versionSet.minor = *(reply + offset);
    result = versionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    versionSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    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::handleInterfaceTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleInterfaceTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = INTERFACE_REQ_FAILED;
        return result;
    }
    result = interfaceSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    const uint8_t* reply = dataLinkLayer.getReply();
    interfaceSet.ticks = ticks;
    interfaceSet.time = time;
    size_t size = sizeof(uint32_t);
    interfaceSet.frameCount = deserializeUint32(reply + offset);
    offset += size;
    interfaceSet.checksumerrorCount = deserializeUint32(reply + offset);
    result = interfaceSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    interfaceSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return result;
}

ReturnValue_t StarTrackerHandler::handlePowerTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handlePowerTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = POWER_REQ_FAILED;
        return result;
    }
    result = powerSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    const uint8_t* reply = dataLinkLayer.getReply();
    powerSet.ticks= ticks;
    powerSet.time= time;
    float value = 0;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.mcuCurrent = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.mcuVoltage = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.fpgaCoreCurrent = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.fpgaCoreVoltage = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.fpga18Current = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.fpga18Voltage = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.fpga25Current = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.fpga25Voltage = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmv21Current = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmv21Voltage = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmvPixCurrent= value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmvPixVoltage = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmv33Current= value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmv33Voltage = value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmvResCurrent= value;
    offset += 4;
    std::memcpy(&value, reply + offset, sizeof(value));
    powerSet.cmvResVoltage = value;
    result = powerSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    powerSet.printSet();
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
    return result;
}

ReturnValue_t StarTrackerHandler::handleSolutionTm() {
    ReturnValue_t result = RETURN_OK;
    result = solutionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleSolutioneTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = TEMPERATURE_REQ_FAILED;
        return result;
    }
    const uint8_t* reply = dataLinkLayer.getReply();
    solutionSet.ticks= ticks;
    solutionSet.time= time;
    float word = 0;
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.caliQw = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.caliQx = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.caliQy = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.caliQz = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.trackConfidence = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.trackQw = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.trackQx = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.trackQy = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.trackQz = word;
    offset += sizeof(float);
    solutionSet.trackRemoved = *(reply + offset);
    offset += sizeof(uint8_t);
    solutionSet.starsCentroided = *(reply + offset);
    offset += sizeof(uint8_t);
    solutionSet.starsMatchedDatabase = *(reply + offset);
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.lisaQw = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.lisaQx = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.lisaQy = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.lisaQz = word;
    offset += sizeof(float);
    std::memcpy(&word, reply + offset, sizeof(float));
    solutionSet.lisaPercentageClose = word;
    offset += sizeof(float);
    solutionSet.lisaNrClose = *(reply + offset);
    offset += sizeof(uint8_t);
    solutionSet.isTrustWorthy = *(reply + offset);
    offset += sizeof(uint8_t);
    solutionSet.stableCount = *(reply + offset);
    offset += sizeof(uint32_t);
    solutionSet.stableCount = *(reply + offset);
    result = solutionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    solutionSet.printSet();
#endif
    return result;
}

ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
    ReturnValue_t result = RETURN_OK;
    uint32_t offset = TM_DATA_FIELD_OFFSET;
    uint8_t status = 0;
    uint32_t ticks = 0;
    uint64_t time = 0;
    getTmHeaderData(&status, &ticks, &time);
    if(status != StarTracker::STATUS_OK) {
        sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
                << static_cast<unsigned int>(status) << std::endl;
        result = TEMPERATURE_REQ_FAILED;
        return result;
    }
    result = temperatureSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
    const uint8_t* reply = dataLinkLayer.getReply();
    temperatureSet.ticks= ticks;
    temperatureSet.time= time;
    float temperature = 0;
    std::memcpy(&temperature, reply + offset, sizeof(temperature));
    temperatureSet.mcuTemperature = temperature;
    offset += sizeof(float);
    std::memcpy(&temperature, reply + offset, sizeof(temperature));
    temperatureSet.cmosTemperature = temperature;
    result = temperatureSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
    if (result != RETURN_OK) {
        return result;
    }
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
    temperatureSet.printSet();
#endif
    return result;
}

void StarTrackerHandler::getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time) {
    const uint8_t* reply = dataLinkLayer.getReply();
    *status = *(reply + STATUS_OFFSET);
    *ticks = deserializeUint32(reply + TICKS_OFFSET);
    *time = deserializeUint64(reply + TIME_OFFSET);
}

uint32_t StarTrackerHandler::deserializeUint32(const uint8_t* buffer) {
    uint32_t word = 0;
    word = *(buffer + 3) << 24
        | *(buffer + 2) << 16
        | *(buffer + 1) << 8
        | *(buffer);
    return word;
}

uint64_t StarTrackerHandler::deserializeUint64(const uint8_t* buffer) {
    uint64_t word = 0;
    word = static_cast<uint64_t>(*(buffer + 7)) << 56
        | static_cast<uint64_t>(*(buffer + 6)) << 48
        | static_cast<uint64_t>(*(buffer + 5)) << 40
        | static_cast<uint64_t>(*(buffer + 4)) << 32
        | static_cast<uint64_t>(*(buffer + 3)) << 24
        | static_cast<uint64_t>(*(buffer + 2)) << 16
        | static_cast<uint64_t>(*(buffer + 1)) << 8
        | static_cast<uint64_t>(*(buffer));
    return word;
}

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;
    }
    }
}