#include "IMTQHandler.h"
#include "OBSWConfig.h"

#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <math.h>

IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
        DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet(
                this), rawMtmMeasurementSet(this), posXselfTestDataset(this), negXselfTestDataset(
                this), posYselfTestDataset(this), negYselfTestDataset(this), posZselfTestDataset(
                this), negZselfTestDataset(this) {
    if (comCookie == NULL) {
        sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
    }
}

IMTQHandler::~IMTQHandler() {
}

void IMTQHandler::doStartUp() {
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
    setMode(MODE_NORMAL);
#else
    setMode(_MODE_TO_ON);
#endif
}

void IMTQHandler::doShutDown() {

}

ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
    switch (communicationStep) {
    case CommunicationStep::GET_ENG_HK_DATA:
        *id = IMTQ::GET_ENG_HK_DATA;
        communicationStep = CommunicationStep::START_MTM_MEASUREMENT;
        break;
    case CommunicationStep::START_MTM_MEASUREMENT:
        *id = IMTQ::START_MTM_MEASUREMENT;
        communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
        break;
    case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
        *id = IMTQ::GET_CAL_MTM_MEASUREMENT;
        communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
        break;
    case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
        *id = IMTQ::GET_RAW_MTM_MEASUREMENT;
        communicationStep = CommunicationStep::GET_ENG_HK_DATA;
        break;
    default:
        sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
                << std::endl;
        break;
    }
    return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
    return RETURN_OK;
}

ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
        const uint8_t * commandData, size_t commandDataLen) {
    switch (deviceCommand) {
    case (IMTQ::POS_X_SELF_TEST): {
        commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
        commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        return RETURN_OK;
    }
    case (IMTQ::NEG_X_SELF_TEST): {
        commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
        commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_NEGATIVE;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        return RETURN_OK;
    }
    case (IMTQ::POS_Y_SELF_TEST): {
        commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
        commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_POSITIVE;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        return RETURN_OK;
    }
    case (IMTQ::NEG_Y_SELF_TEST): {
        commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
        commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_NEGATIVE;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        return RETURN_OK;
    }
    case (IMTQ::POS_Z_SELF_TEST): {
        commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
        commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_POSITIVE;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        return RETURN_OK;
    }
    case (IMTQ::NEG_Z_SELF_TEST): {
        commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
        commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_NEGATIVE;
        rawPacket = commandBuffer;
        rawPacketLen = 2;
        return RETURN_OK;
    }
    case (IMTQ::GET_SELF_TEST_RESULT): {
        commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT;
        rawPacket = commandBuffer;
        rawPacketLen = 1;
        return RETURN_OK;
    }
    case (IMTQ::START_ACTUATION_DIPOLE): {
        /* IMTQ expects low byte first */
        commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
        commandBuffer[1] = *(commandData + 1);
        commandBuffer[2] = *(commandData);
        commandBuffer[3] = *(commandData + 3);
        commandBuffer[4] = *(commandData + 2);
        commandBuffer[5] = *(commandData + 5);
        commandBuffer[6] = *(commandData + 4);
        commandBuffer[7] = *(commandData + 7);
        commandBuffer[8] = *(commandData + 6);
        rawPacket = commandBuffer;
        rawPacketLen = 9;
        return RETURN_OK;
    }
    case (IMTQ::GET_ENG_HK_DATA): {
        commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
        rawPacket = commandBuffer;
        rawPacketLen = 1;
        return RETURN_OK;
    }
    case (IMTQ::GET_COMMANDED_DIPOLE): {
        commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE;
        rawPacket = commandBuffer;
        rawPacketLen = 1;
        return RETURN_OK;
    }
    case (IMTQ::START_MTM_MEASUREMENT): {
        commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
        rawPacket = commandBuffer;
        rawPacketLen = 1;
        return RETURN_OK;
    }
    case (IMTQ::GET_CAL_MTM_MEASUREMENT): {
        commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT;
        rawPacket = commandBuffer;
        rawPacketLen = 1;
        return RETURN_OK;
    }
    case (IMTQ::GET_RAW_MTM_MEASUREMENT): {
        commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT;
        rawPacket = commandBuffer;
        rawPacketLen = 1;
        return RETURN_OK;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return HasReturnvaluesIF::RETURN_FAILED;
}

void IMTQHandler::fillCommandAndReplyMap() {
    this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
            IMTQ::SIZE_SELF_TEST_RESULTS);
    this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
            IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
            IMTQ::SIZE_ENG_HK_DATA_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
            IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
            IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
            IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
    this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
            IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
}

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

    ReturnValue_t result = RETURN_OK;

    switch (*start) {
    case (IMTQ::CC::START_ACTUATION_DIPOLE):
        *foundLen = IMTQ::SIZE_STATUS_REPLY;
        *foundId = IMTQ::START_ACTUATION_DIPOLE;
        break;
    case (IMTQ::CC::START_MTM_MEASUREMENT):
        *foundLen = IMTQ::SIZE_STATUS_REPLY;
        *foundId = IMTQ::START_MTM_MEASUREMENT;
        break;
    case (IMTQ::CC::GET_ENG_HK_DATA):
        *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
        *foundId = IMTQ::GET_ENG_HK_DATA;
        break;
    case (IMTQ::CC::GET_COMMANDED_DIPOLE):
        *foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY;
        *foundId = IMTQ::GET_COMMANDED_DIPOLE;
        break;
    case (IMTQ::CC::GET_CAL_MTM_MEASUREMENT):
        *foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT;
        *foundId = IMTQ::GET_CAL_MTM_MEASUREMENT;
        break;
    case (IMTQ::CC::GET_RAW_MTM_MEASUREMENT):
        *foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT;
        *foundId = IMTQ::GET_RAW_MTM_MEASUREMENT;
        break;
    case (IMTQ::CC::SELF_TEST_CMD):
        *foundLen = IMTQ::SIZE_STATUS_REPLY;
        result = getSelfTestCommandId(foundId);
        break;
    case (IMTQ::CC::GET_SELF_TEST_RESULT):
        *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
        *foundId = IMTQ::GET_SELF_TEST_RESULT;
        break;
    default:
        sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
        result = IGNORE_REPLY_DATA;
        break;
    }

    return result;
}

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

    ReturnValue_t result = RETURN_OK;

    result = parseStatusByte(packet);

    if (result != RETURN_OK) {
        return result;
    }

    switch (id) {
    case (IMTQ::POS_X_SELF_TEST):
    case (IMTQ::NEG_X_SELF_TEST):
    case (IMTQ::POS_Y_SELF_TEST):
    case (IMTQ::NEG_Y_SELF_TEST):
    case (IMTQ::POS_Z_SELF_TEST):
    case (IMTQ::NEG_Z_SELF_TEST):
    case (IMTQ::START_ACTUATION_DIPOLE):
    case (IMTQ::START_MTM_MEASUREMENT):
        /* Replies only the status byte which is already handled with parseStatusByte */
        break;
    case (IMTQ::GET_ENG_HK_DATA):
        fillEngHkDataset(packet);
        break;
    case (IMTQ::GET_COMMANDED_DIPOLE):
        handleGetCommandedDipoleReply(packet);
        break;
    case (IMTQ::GET_CAL_MTM_MEASUREMENT):
        fillCalibratedMtmDataset(packet);
        break;
    case (IMTQ::GET_RAW_MTM_MEASUREMENT):
        fillRawMtmDataset(packet);
        break;
    case (IMTQ::GET_SELF_TEST_RESULT):
        handleSelfTestReply(packet);
        break;
    default: {
        sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
    }

    return RETURN_OK;
}

void IMTQHandler::setNormalDatapoolEntriesInvalid() {

}

LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
    if (sid == engHkDataset.getSid()) {
        return &engHkDataset;
    } else if (sid == calMtmMeasurementSet.getSid()) {
        return &calMtmMeasurementSet;
    } else if (sid == rawMtmMeasurementSet.getSid()) {
        return &rawMtmMeasurementSet;
    } else if (sid == posXselfTestDataset.getSid()) {
        return &posXselfTestDataset;
    } else if (sid == negXselfTestDataset.getSid()) {
        return &negXselfTestDataset;
    } else if (sid == posYselfTestDataset.getSid()) {
        return &posYselfTestDataset;
    } else if (sid == negYselfTestDataset.getSid()) {
        return &negYselfTestDataset;
    } else if (sid == posZselfTestDataset.getSid()) {
        return &posZselfTestDataset;
    } else if (sid == negZselfTestDataset.getSid()) {
        return &negZselfTestDataset;
    } else {
        sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl;
        return nullptr;
    }
}

uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
    return 5000;
}

ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {

    /** Entries of engineering housekeeping dataset */
    localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** Entries of calibrated MTM measurement dataset */
    localDataPoolMap.emplace(IMTQ::MTM_CAL_X, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry<int32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>( { 0 }));

    /** Entries of raw MTM measurement dataset */
    localDataPoolMap.emplace(IMTQ::MTM_RAW_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::MTM_RAW_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>( { 0 }));

    /** INIT measurements for positive X axis test */
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** INIT measurements for negative X axis test */
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** INIT measurements for positive Y axis test */
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** INIT measurements for negative Y axis test */
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** INIT measurements for positive Z axis test */
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** INIT measurements for negative Z axis test */
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::INIT_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(IMTQ::POS_X_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(IMTQ::NEG_X_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(IMTQ::POS_Y_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(IMTQ::NEG_Y_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(IMTQ::POS_Z_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(IMTQ::NEG_Z_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_RAW_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_X, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Y, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_CAL_MAG_Z, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_CURRENT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** FINA measurements for positive X axis test */
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** FINA measurements for negative X axis test */
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** FINA measurements for positive Y axis test */
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** FINA measurements for negative Y axis test */
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** FINA measurements for positive Z axis test */
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    /** FINA measurements for negative Z axis test */
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_ERR, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_RAW_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_X, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Y, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_CAL_MAG_Z, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_CURRENT, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
    DeviceCommandId_t commandId = getPendingCommand();
    switch (commandId) {
    case IMTQ::POS_X_SELF_TEST:
    case IMTQ::NEG_X_SELF_TEST:
    case IMTQ::POS_Y_SELF_TEST:
    case IMTQ::NEG_Y_SELF_TEST:
    case IMTQ::POS_Z_SELF_TEST:
    case IMTQ::NEG_Z_SELF_TEST:
        *id = commandId;
        break;
    default:
        sif::error << "IMTQHandler::getSelfTestCommandId: Reply does not match to pending "
                << "command" << std::endl;
        return UNEXPECTED_SELF_TEST_REPLY;
    }
    return RETURN_OK;
}

ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
    uint8_t cmdErrorField = *(packet + 1) & 0xF;
    switch (cmdErrorField) {
    case 0:
        return RETURN_OK;
    case 1:
        sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl;
        return REJECTED_WITHOUT_REASON;
    case 2:
        sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl;
        return INVALID_COMMAND_CODE;
    case 3:
        sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl;
        return PARAMETER_MISSING;
    case 4:
        sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl;
        return PARAMETER_INVALID;
    case 5:
        sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl;
        return CC_UNAVAILABLE;
    case 7:
        sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error"
                << std::endl;
        return INTERNAL_PROCESSING_ERROR;
    default:
        sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code "
                << cmdErrorField << std::endl;
        return CMD_ERR_UNKNOWN;
    }
}

void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
    PoolReadGuard rg(&engHkDataset);
    uint8_t offset = 2;
    engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    engHkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    engHkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
    offset += 2;
    engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
    offset += 2;
    engHkDataset.coilXCurrentmA = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    engHkDataset.coilYCurrentmA = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    engHkDataset.coilZCurrentmA = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
    offset += 2;
    engHkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
    offset += 2;
    engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
    offset += 2;
    engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));

#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl;
    sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl;
    sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentmA << " mA" << std::endl;
    sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentmA << " mA" << std::endl;
    sif::info << "IMTQ coil X current: " << engHkDataset.coilXCurrentmA << " mA" << std::endl;
    sif::info << "IMTQ coil Y current: " << engHkDataset.coilYCurrentmA << " mA" << std::endl;
    sif::info << "IMTQ coil Z current: " << engHkDataset.coilZCurrentmA << " mA" << std::endl;
    sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C"
            << std::endl;
    sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C"
            << std::endl;
    sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C"
            << std::endl;
    sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C"
            << std::endl;
#endif
}

void IMTQHandler::setModeNormal() {
    mode = MODE_NORMAL;
}

void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {

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

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

    if (queueId == NO_COMMANDER) {
        return;
    }

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

void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
    uint8_t tmData[6];
    /* Switching endianess of received dipole values */
    tmData[0] = *(packet + 3);
    tmData[1] = *(packet + 2);
    tmData[2] = *(packet + 5);
    tmData[3] = *(packet + 4);
    tmData[4] = *(packet + 7);
    tmData[5] = *(packet + 6);
    handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
}

void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
    PoolReadGuard rg(&calMtmMeasurementSet);
    int8_t offset = 2;
    calMtmMeasurementSet.mtmXnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    calMtmMeasurementSet.mtmYnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    calMtmMeasurementSet.mtmZnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24)
            | (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT"
            << std::endl;
    sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT"
            << std::endl;
    sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mtmZnT << " nT"
            << std::endl;
    sif::info << "IMTQ coil actuation status during MTM measurement: "
            << (unsigned int) calMtmMeasurementSet.coilActuationStatus.value << std::endl;
#endif
}

void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
    PoolReadGuard rg(&rawMtmMeasurementSet);
    int8_t offset = 2;
    rawMtmMeasurementSet.mtmXnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    rawMtmMeasurementSet.mtmYnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    rawMtmMeasurementSet.mtmZnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    rawMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24)
            | (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT"
            << std::endl;
    sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT"
            << std::endl;
    sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmZnT << " nT"
            << std::endl;
    sif::info << "IMTQ coil actuation status during MTM measurement: "
            << (unsigned int) rawMtmMeasurementSet.coilActuationStatus.value << std::endl;
#endif
}

void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
    uint16_t offset = 2;
    checkErrorByte(*(packet + offset), *(packet + offset + 1));

    switch (*(packet + IMTQ::MAIN_STEP_OFFSET)) {
    case IMTQ::SELF_TEST_STEPS::X_POSITIVE: {
        handlePositiveXSelfTestReply(packet);
        break;
    }
    case IMTQ::SELF_TEST_STEPS::X_NEGATIVE: {
        handleNegativeXSelfTestReply(packet);
        break;
    }
    case IMTQ::SELF_TEST_STEPS::Y_POSITIVE: {
        handlePositiveYSelfTestReply(packet);
        break;
    }
    case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE: {
        handleNegativeYSelfTestReply(packet);
        break;
    }
    case IMTQ::SELF_TEST_STEPS::Z_POSITIVE: {
        handlePositiveZSelfTestReply(packet);
        break;
    }
    case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE: {
        handleNegativeZSelfTestReply(packet);
        break;
    }
    default:
        break;
    }
}

void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
    PoolReadGuard rg(&posXselfTestDataset);

    uint16_t offset = 2;
    /** Init measurements */
    posXselfTestDataset.initErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.initCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.initCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.initCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** +X measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    posXselfTestDataset.err = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.coilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.coilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.coilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** FINA measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    posXselfTestDataset.finaErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posXselfTestDataset.finaCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.finaCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.finaCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ self test (INIT) err: "
            << static_cast<unsigned int>(posXselfTestDataset.initErr.value) << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posXselfTestDataset.initRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posXselfTestDataset.initRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posXselfTestDataset.initRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
            << posXselfTestDataset.initCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
            << posXselfTestDataset.initCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
            << posXselfTestDataset.initCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X current: " << posXselfTestDataset.initCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y current: " << posXselfTestDataset.initCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z current: " << posXselfTestDataset.initCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X temperature: "
            << posXselfTestDataset.initCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y temperature: "
            << posXselfTestDataset.initCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z temperature: "
            << posXselfTestDataset.initCoilZTemperature << " °C" << std::endl;

    sif::info << "IMTQ self test (+X) err: "
            << static_cast<unsigned int>(posXselfTestDataset.err.value) << std::endl;
    sif::info << "IMTQ self test (+X) raw magnetic field X: " << posXselfTestDataset.rawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+X) raw magnetic field Y: " << posXselfTestDataset.rawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+X) raw magnetic field Z: " << posXselfTestDataset.rawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+X) calibrated magnetic field X: " << posXselfTestDataset.calMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+X) calibrated magnetic field Y: " << posXselfTestDataset.calMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+X) calibrated magnetic field Z: " << posXselfTestDataset.calMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+X) coil X current: " << posXselfTestDataset.coilXCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+X) coil Y current: " << posXselfTestDataset.coilYCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+X) coil Z current: " << posXselfTestDataset.coilZCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+X) coil X temperature: " << posXselfTestDataset.coilXTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (+X) coil Y temperature: " << posXselfTestDataset.coilYTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (+X) coil Z temperature: " << posXselfTestDataset.coilZTemperature
            << " °C" << std::endl;

    sif::info << "IMTQ self test (FINA) err: "
            << static_cast<unsigned int>(posXselfTestDataset.finaErr.value) << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posXselfTestDataset.finaRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posXselfTestDataset.finaRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posXselfTestDataset.finaRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
            << posXselfTestDataset.finaCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
            << posXselfTestDataset.finaCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
            << posXselfTestDataset.finaCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X current: " << posXselfTestDataset.finaCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y current: " << posXselfTestDataset.finaCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z current: " << posXselfTestDataset.finaCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X temperature: "
            << posXselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y temperature: "
            << posXselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z temperature: "
            << posXselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}

void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
    PoolReadGuard rg(&posXselfTestDataset);

    uint16_t offset = 2;
    /** Init measurements */
    negXselfTestDataset.initErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negXselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.initCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.initCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.initCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negXselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negXselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** +X measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    negXselfTestDataset.err = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negXselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.coilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.coilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.coilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negXselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negXselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** FINA measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    negXselfTestDataset.finaErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negXselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negXselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negXselfTestDataset.finaCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.finaCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.finaCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negXselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negXselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negXselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ self test (INIT) err: "
            << static_cast<unsigned int>(negXselfTestDataset.initErr.value) << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negXselfTestDataset.initRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negXselfTestDataset.initRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negXselfTestDataset.initRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
            << negXselfTestDataset.initCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
            << negXselfTestDataset.initCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
            << negXselfTestDataset.initCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X current: " << negXselfTestDataset.initCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y current: " << negXselfTestDataset.initCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z current: " << negXselfTestDataset.initCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X temperature: "
            << negXselfTestDataset.initCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y temperature: "
            << negXselfTestDataset.initCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z temperature: "
            << negXselfTestDataset.initCoilZTemperature << " °C" << std::endl;

    sif::info << "IMTQ self test (-X) err: "
            << static_cast<unsigned int>(negXselfTestDataset.err.value) << std::endl;
    sif::info << "IMTQ self test (-X) raw magnetic field X: " << negXselfTestDataset.rawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-X) raw magnetic field Y: " << negXselfTestDataset.rawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-X) raw magnetic field Z: " << negXselfTestDataset.rawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-X) calibrated magnetic field X: " << negXselfTestDataset.calMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-X) calibrated magnetic field Y: " << negXselfTestDataset.calMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-X) calibrated magnetic field Z: " << negXselfTestDataset.calMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-X) coil X current: " << negXselfTestDataset.coilXCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-X) coil Y current: " << negXselfTestDataset.coilYCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-X) coil Z current: " << negXselfTestDataset.coilZCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-X) coil X temperature: " << negXselfTestDataset.coilXTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (-X) coil Y temperature: " << negXselfTestDataset.coilYTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (-X) coil Z temperature: " << negXselfTestDataset.coilZTemperature
            << " °C" << std::endl;

    sif::info << "IMTQ self test (FINA) err: "
            << static_cast<unsigned int>(negXselfTestDataset.finaErr.value) << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negXselfTestDataset.finaRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negXselfTestDataset.finaRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negXselfTestDataset.finaRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
            << negXselfTestDataset.finaCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
            << negXselfTestDataset.finaCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
            << negXselfTestDataset.finaCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X current: " << negXselfTestDataset.finaCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y current: " << negXselfTestDataset.finaCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z current: " << negXselfTestDataset.finaCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X temperature: "
            << negXselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y temperature: "
            << negXselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z temperature: "
            << negXselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}

void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
    PoolReadGuard rg(&posXselfTestDataset);

    uint16_t offset = 2;
    /** Init measurements */
    posYselfTestDataset.initErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.initCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.initCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.initCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** +X measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    posYselfTestDataset.err = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.coilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.coilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.coilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** FINA measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    posYselfTestDataset.finaErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posYselfTestDataset.finaCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.finaCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.finaCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ self test (INIT) err: "
            << static_cast<unsigned int>(posYselfTestDataset.initErr.value) << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posYselfTestDataset.initRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posYselfTestDataset.initRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posYselfTestDataset.initRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
            << posYselfTestDataset.initCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
            << posYselfTestDataset.initCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
            << posYselfTestDataset.initCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X current: " << posYselfTestDataset.initCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y current: " << posYselfTestDataset.initCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z current: " << posYselfTestDataset.initCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X temperature: "
            << posYselfTestDataset.initCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y temperature: "
            << posYselfTestDataset.initCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z temperature: "
            << posYselfTestDataset.initCoilZTemperature << " °C" << std::endl;

    sif::info << "IMTQ self test (+Y) err: "
            << static_cast<unsigned int>(posYselfTestDataset.err.value) << std::endl;
    sif::info << "IMTQ self test (+Y) raw magnetic field X: " << posYselfTestDataset.rawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Y) raw magnetic field Y: " << posYselfTestDataset.rawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Y) raw magnetic field Z: " << posYselfTestDataset.rawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Y) calibrated magnetic field X: " << posYselfTestDataset.calMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Y) calibrated magnetic field Y: " << posYselfTestDataset.calMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Y) calibrated magnetic field Z: " << posYselfTestDataset.calMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Y) coil X current: " << posYselfTestDataset.coilXCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+Y) coil Y current: " << posYselfTestDataset.coilYCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+Y) coil Z current: " << posYselfTestDataset.coilZCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+Y) coil X temperature: " << posYselfTestDataset.coilXTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (+Y) coil Y temperature: " << posYselfTestDataset.coilYTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (+Y) coil Z temperature: " << posYselfTestDataset.coilZTemperature
            << " °C" << std::endl;

    sif::info << "IMTQ self test (FINA) err: "
            << static_cast<unsigned int>(posYselfTestDataset.finaErr.value) << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posYselfTestDataset.finaRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posYselfTestDataset.finaRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posYselfTestDataset.finaRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
            << posYselfTestDataset.finaCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
            << posYselfTestDataset.finaCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
            << posYselfTestDataset.finaCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X current: " << posYselfTestDataset.finaCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y current: " << posYselfTestDataset.finaCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z current: " << posYselfTestDataset.finaCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X temperature: "
            << posYselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y temperature: "
            << posYselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z temperature: "
            << posYselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}

void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
    PoolReadGuard rg(&posXselfTestDataset);

    uint16_t offset = 2;
    /** Init measurements */
    posZselfTestDataset.initErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negYselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.initCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.initCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.initCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negYselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negYselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** +X measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    negYselfTestDataset.err = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negYselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.coilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.coilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.coilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negYselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negYselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** FINA measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    negYselfTestDataset.finaErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negYselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negYselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negYselfTestDataset.finaCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.finaCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.finaCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negYselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negYselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negYselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ self test (INIT) err: "
            << static_cast<unsigned int>(negYselfTestDataset.initErr.value) << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negYselfTestDataset.initRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negYselfTestDataset.initRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negYselfTestDataset.initRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
            << negYselfTestDataset.initCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
            << negYselfTestDataset.initCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
            << negYselfTestDataset.initCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X current: " << negYselfTestDataset.initCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y current: " << negYselfTestDataset.initCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z current: " << negYselfTestDataset.initCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X temperature: "
            << negYselfTestDataset.initCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y temperature: "
            << negYselfTestDataset.initCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z temperature: "
            << negYselfTestDataset.initCoilZTemperature << " °C" << std::endl;

    sif::info << "IMTQ self test (-Y) err: "
            << static_cast<unsigned int>(negYselfTestDataset.err.value) << std::endl;
    sif::info << "IMTQ self test (-Y) raw magnetic field X: " << negYselfTestDataset.rawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Y) raw magnetic field Y: " << negYselfTestDataset.rawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Y) raw magnetic field Z: " << negYselfTestDataset.rawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Y) calibrated magnetic field X: " << negYselfTestDataset.calMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Y) calibrated magnetic field Y: " << negYselfTestDataset.calMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Y) calibrated magnetic field Z: " << negYselfTestDataset.calMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Y) coil X current: " << negYselfTestDataset.coilXCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-Y) coil Y current: " << negYselfTestDataset.coilYCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-Y) coil Z current: " << negYselfTestDataset.coilZCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-Y) coil X temperature: " << negYselfTestDataset.coilXTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (-Y) coil Y temperature: " << negYselfTestDataset.coilYTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (-Y) coil Z temperature: " << negYselfTestDataset.coilZTemperature
            << " °C" << std::endl;

    sif::info << "IMTQ self test (FINA) err: "
            << static_cast<unsigned int>(negYselfTestDataset.finaErr.value) << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negYselfTestDataset.finaRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negYselfTestDataset.finaRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negYselfTestDataset.finaRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
            << negYselfTestDataset.finaCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
            << negYselfTestDataset.finaCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
            << negYselfTestDataset.finaCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X current: " << negYselfTestDataset.finaCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y current: " << negYselfTestDataset.finaCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z current: " << negYselfTestDataset.finaCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X temperature: "
            << negYselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y temperature: "
            << negYselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z temperature: "
            << negYselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}

void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
    PoolReadGuard rg(&posXselfTestDataset);

    uint16_t offset = 2;
    /** Init measurements */
    posZselfTestDataset.initErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.initCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.initCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.initCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** +X measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    posZselfTestDataset.err = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.coilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.coilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.coilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** FINA measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    posZselfTestDataset.finaErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    posZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    posZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    posZselfTestDataset.finaCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.finaCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.finaCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    posZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    posZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ self test (INIT) err: "
            << static_cast<unsigned int>(posZselfTestDataset.initErr.value) << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field X: " << posZselfTestDataset.initRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << posZselfTestDataset.initRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << posZselfTestDataset.initRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
            << posZselfTestDataset.initCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
            << posZselfTestDataset.initCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
            << posZselfTestDataset.initCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X current: " << posZselfTestDataset.initCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y current: " << posZselfTestDataset.initCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z current: " << posZselfTestDataset.initCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X temperature: "
            << posZselfTestDataset.initCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y temperature: "
            << posZselfTestDataset.initCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z temperature: "
            << posZselfTestDataset.initCoilZTemperature << " °C" << std::endl;

    sif::info << "IMTQ self test (+Z) err: "
            << static_cast<unsigned int>(posZselfTestDataset.err.value) << std::endl;
    sif::info << "IMTQ self test (+Z) raw magnetic field X: " << posZselfTestDataset.rawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Z) raw magnetic field Y: " << posZselfTestDataset.rawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Z) raw magnetic field Z: " << posZselfTestDataset.rawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Z) calibrated magnetic field X: " << posZselfTestDataset.calMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Z) calibrated magnetic field Y: " << posZselfTestDataset.calMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Z) calibrated magnetic field Z: " << posZselfTestDataset.calMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (+Z) coil X current: " << posZselfTestDataset.coilXCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+Z) coil Y current: " << posZselfTestDataset.coilYCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+Z) coil Z current: " << posZselfTestDataset.coilZCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (+Z) coil X temperature: " << posZselfTestDataset.coilXTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (+Z) coil Y temperature: " << posZselfTestDataset.coilYTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (+Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature
            << " °C" << std::endl;

    sif::info << "IMTQ self test (FINA) err: "
            << static_cast<unsigned int>(posZselfTestDataset.finaErr.value) << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field X: " << posZselfTestDataset.finaRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << posZselfTestDataset.finaRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << posZselfTestDataset.finaRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
            << posZselfTestDataset.finaCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
            << posZselfTestDataset.finaCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
            << posZselfTestDataset.finaCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X current: " << posZselfTestDataset.finaCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y current: " << posZselfTestDataset.finaCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z current: " << posZselfTestDataset.finaCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X temperature: "
            << posZselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y temperature: "
            << posZselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z temperature: "
            << posZselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}

void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
    PoolReadGuard rg(&posXselfTestDataset);

    uint16_t offset = 2;
    /** Init measurements */
    negZselfTestDataset.initErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negZselfTestDataset.initRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.initRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.initRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.initCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.initCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.initCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.initCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.initCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.initCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.initCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negZselfTestDataset.initCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negZselfTestDataset.initCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** +X measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    negZselfTestDataset.err = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negZselfTestDataset.rawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.rawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.rawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.calMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.calMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.calMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.coilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.coilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.coilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.coilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negZselfTestDataset.coilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negZselfTestDataset.coilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

    /** FINA measurements */
    checkErrorByte(*(packet + offset), *(packet + offset + 1));
    negZselfTestDataset.finaErr = *(packet + offset);
    offset += 2; // STEP byte will not be stored
    negZselfTestDataset.finaRawMagX = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.finaRawMagY = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.finaRawMagZ = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
    offset += 4;
    negZselfTestDataset.finaCalMagX = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.finaCalMagY = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.finaCalMagZ = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
            | *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;
    negZselfTestDataset.finaCoilXCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.finaCoilYCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.finaCoilZCurrent = static_cast<int16_t>(*(packet + offset + 1) << 8
            | *(packet + offset)) * 0.1;
    offset += 2;
    negZselfTestDataset.finaCoilXTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negZselfTestDataset.finaCoilYTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 2;
    negZselfTestDataset.finaCoilZTemperature = *(packet + offset + 1) << 8 | *(packet + offset);
    offset += 4;

#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
    sif::info << "IMTQ self test (INIT) err: "
            << static_cast<unsigned int>(negZselfTestDataset.initErr.value) << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field X: " << negZselfTestDataset.initRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Y: " << negZselfTestDataset.initRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) raw magnetic field Z: " << negZselfTestDataset.initRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field X: "
            << negZselfTestDataset.initCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Y: "
            << negZselfTestDataset.initCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) calibrated magnetic field Z: "
            << negZselfTestDataset.initCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X current: " << negZselfTestDataset.initCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y current: " << negZselfTestDataset.initCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z current: " << negZselfTestDataset.initCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (INIT) coil X temperature: "
            << negZselfTestDataset.initCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Y temperature: "
            << negZselfTestDataset.initCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (INIT) coil Z temperature: "
            << negZselfTestDataset.initCoilZTemperature << " °C" << std::endl;

    sif::info << "IMTQ self test (-Z) err: "
            << static_cast<unsigned int>(negZselfTestDataset.err.value) << std::endl;
    sif::info << "IMTQ self test (-Z) raw magnetic field X: " << negZselfTestDataset.rawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Z) raw magnetic field Y: " << negZselfTestDataset.rawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Z) raw magnetic field Z: " << negZselfTestDataset.rawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Z) calibrated magnetic field X: " << negZselfTestDataset.calMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Z) calibrated magnetic field Y: " << negZselfTestDataset.calMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Z) calibrated magnetic field Z: " << negZselfTestDataset.calMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (-Z) coil X current: " << negZselfTestDataset.coilXCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-Z) coil Y current: " << negZselfTestDataset.coilYCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-Z) coil Z current: " << negZselfTestDataset.coilZCurrent << " mA"
            << std::endl;
    sif::info << "IMTQ self test (-Z) coil X temperature: " << negZselfTestDataset.coilXTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (-Z) coil Y temperature: " << negZselfTestDataset.coilYTemperature
            << " °C" << std::endl;
    sif::info << "IMTQ self test (-Z) coil Z temperature: " << negYselfTestDataset.coilZTemperature
            << " °C" << std::endl;

    sif::info << "IMTQ self test (FINA) err: "
            << static_cast<unsigned int>(negZselfTestDataset.finaErr.value) << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field X: " << negZselfTestDataset.finaRawMagX
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Y: " << negZselfTestDataset.finaRawMagY
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) raw magnetic field Z: " << negZselfTestDataset.finaRawMagZ
            << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field X: "
            << negZselfTestDataset.finaCalMagX << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Y: "
            << negZselfTestDataset.finaCalMagY << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) calibrated magnetic field Z: "
            << negZselfTestDataset.finaCalMagZ << " nT" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X current: " << negZselfTestDataset.finaCoilXCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y current: " << negZselfTestDataset.finaCoilYCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z current: " << negZselfTestDataset.finaCoilZCurrent
            << " mA" << std::endl;
    sif::info << "IMTQ self test (FINA) coil X temperature: "
            << negZselfTestDataset.finaCoilXTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Y temperature: "
            << negZselfTestDataset.finaCoilYTemperature << " °C" << std::endl;
    sif::info << "IMTQ self test (FINA) coil Z temperature: "
            << negZselfTestDataset.finaCoilZTemperature << " °C" << std::endl;
#endif
}

void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
    std::string stepString("");
    if (step < 8) {
        stepString = makeStepString(step);
    } else {
        /** This should normally never happen */
        sif::debug << "IMTQHandler::checkErrorByte: Invalid step" << std::endl;
        return;
    }

    if (errorByte == 0) {
        return;
    }

    if (errorByte & IMTQ::I2C_FAILURE_MASK) {
        triggerEvent(SELF_TEST_I2C_FAILURE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test I2C failure for step " << stepString
                << std::endl;
    }
    if (errorByte & IMTQ::SPI_FAILURE_MASK) {
        triggerEvent(SELF_TEST_SPI_FAILURE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test SPI failure for step " << stepString
                << std::endl;
    }
    if (errorByte & IMTQ::ADC_FAILURE_MASK) {
        triggerEvent(SELF_TEST_ADC_FAILURE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test ADC failure for step " << stepString
                << std::endl;
    }
    if (errorByte & IMTQ::PWM_FAILURE_MASK) {
        triggerEvent(SELF_TEST_PWM_FAILURE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test PWM failure for step " << stepString
                << std::endl;
    }
    if (errorByte & IMTQ::TC_FAILURE_MASK) {
        triggerEvent(SELF_TEST_TC_FAILURE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test TC failure (system failure) for step "
                << stepString << std::endl;
    }
    if (errorByte & IMTQ::MTM_RANGE_FAILURE_MASK) {
        triggerEvent(SELF_TEST_TC_FAILURE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test MTM range failure for step "
                << stepString << std::endl;
    }
    if (errorByte & IMTQ::COIL_CURRENT_FAILURE_MASK) {
        triggerEvent(SELF_TEST_COIL_CURRENT_FAILURE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test coil current outside of expected "
                "range for step " << stepString << std::endl;
    }

    if (errorByte & IMTQ::INVALID_ERROR_BYTE) {
        triggerEvent(INVALID_ERROR_BYTE, step);
        sif::error << "IMTQHandler::checkErrorByte: Self test result of step " << stepString
                << " has invalid error byte" << std::endl;
    }
}

std::string IMTQHandler::makeStepString(const uint8_t step) {
    std::string stepString("");
    switch (step) {
    case IMTQ::SELF_TEST_STEPS::INIT:
        stepString = std::string("INIT");
        break;
    case IMTQ::SELF_TEST_STEPS::X_POSITIVE:
        stepString = std::string("+X");
        break;
    case IMTQ::SELF_TEST_STEPS::X_NEGATIVE:
        stepString = std::string("-X");
        break;
    case IMTQ::SELF_TEST_STEPS::Y_POSITIVE:
        stepString = std::string("+Y");
        break;
    case IMTQ::SELF_TEST_STEPS::Y_NEGATIVE:
        stepString = std::string("-Y");
        break;
    case IMTQ::SELF_TEST_STEPS::Z_POSITIVE:
        stepString = std::string("+Z");
        break;
    case IMTQ::SELF_TEST_STEPS::Z_NEGATIVE:
        stepString = std::string("-Z");
        break;
    case IMTQ::SELF_TEST_STEPS::FINA:
        stepString = std::string("FINA");
        break;
    default:
        sif::error << "IMTQHandler::checkErrorByte: Received packet with invalid step information"
                << std::endl;
        break;
    }
    return stepString;
}