#include <commonConfig.h>
#include <fsfw/datapool/PoolEntry.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/datapoollocal/LocalDataPoolManager.h>
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h>
#include <fsfw/datapoollocal/localPoolDefinitions.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/ipc/MutexFactory.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/ipc/messageQueueDefinitions.h>
#include <fsfw/modes/ModeMessage.h>
#include <fsfw/objectmanager/SystemObjectIF.h>
#include <fsfw/power/definitions.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/serialize/SerializeAdapter.h>
#include <fsfw/serialize/SerializeIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Countdown.h>
#include <fsfw/timemanager/clockDefinitions.h>
#include <mission/devices/ImtqHandler.h>

#include <cmath>
#include <fsfw/datapoollocal/LocalPoolVariable.tpp>

#include "mission/config/torquer.h"

static constexpr bool ACTUATION_WIRETAPPING = false;

ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                         power::Switch_t pwrSwitcher)
    : DeviceHandlerBase(objectId, comIF, comCookie),
      statusSet(this),
      dipoleSet(*this),
      rawMtmNoTorque(this),
      hkDatasetNoTorque(this),
      rawMtmWithTorque(this),
      hkDatasetWithTorque(this),
      calMtmMeasurementSet(this),
      posXselfTestDataset(this),
      negXselfTestDataset(this),
      posYselfTestDataset(this),
      negYselfTestDataset(this),
      posZselfTestDataset(this),
      negZselfTestDataset(this),
      switcher(pwrSwitcher) {
  if (comCookie == nullptr) {
    sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
  }
}

ImtqHandler::~ImtqHandler() = default;

void ImtqHandler::doStartUp() {
  updatePeriodicReply(true, imtq::cmdIds::REPLY);
  if (goToNormalMode) {
    setMode(MODE_NORMAL);
  } else {
    setMode(_MODE_TO_ON);
  }
}

void ImtqHandler::doShutDown() {
  updatePeriodicReply(false, imtq::cmdIds::REPLY);
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  switch (requestStep) {
    case (imtq::RequestType::MEASURE): {
      *id = imtq::cmdIds::REQUEST;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (imtq::RequestType::ACTUATE): {
      *id = imtq::cmdIds::START_ACTUATION_DIPOLE;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  return NOTHING_TO_SEND;
}

ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                   const uint8_t* commandData,
                                                   size_t commandDataLen) {
  switch (deviceCommand) {
    case (imtq::cmdIds::POS_X_SELF_TEST): {
      // TODO: special request
      return returnvalue::OK;
    }
    case (imtq::cmdIds::NEG_X_SELF_TEST): {
      // TODO: special request
      return returnvalue::OK;
    }
    case (imtq::cmdIds::POS_Y_SELF_TEST): {
      // TODO: special request
      return returnvalue::OK;
    }
    case (imtq::cmdIds::NEG_Y_SELF_TEST): {
      // TODO: special request
      return returnvalue::OK;
    }
    case (imtq::cmdIds::POS_Z_SELF_TEST): {
      // TODO: special request
      return returnvalue::OK;
    }
    case (imtq::cmdIds::NEG_Z_SELF_TEST): {
      // TODO: special request
      return returnvalue::OK;
    }
    case (imtq::cmdIds::GET_SELF_TEST_RESULT): {
      // TODO: special request
      return returnvalue::OK;
    }
    case (imtq::cmdIds::REQUEST): {
      ImtqRequest request(commandBuffer, sizeof(commandBuffer));
      request.setMeasureRequest(imtq::SpecialRequest::NONE);
      rawPacket = commandBuffer;
      rawPacketLen = ImtqRequest::REQUEST_LEN;
      return returnvalue::OK;
    }
    case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
      /* IMTQ expects low byte first */
      // commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE;
      if (commandData != nullptr && commandDataLen < 8) {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
      }
      ImtqRequest request(commandBuffer, sizeof(commandBuffer));
      // Commands override anything which was set in the software
      if (commandData != nullptr) {
        // Read set dipole values from local pool
        PoolReadGuard pg(&dipoleSet);

        int16_t xDipole = 0, yDipole = 0, zDipole = 0;
        uint16_t torqueDuration = 0;
        dipoleSet.xDipole = xDipole;
        dipoleSet.yDipole = yDipole;
        dipoleSet.zDipole = zDipole;
        dipoleSet.currentTorqueDurationMs = torqueDuration;
      }

      request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value,
                                dipoleSet.zDipole.value, dipoleSet.currentTorqueDurationMs.value);
      if (ACTUATION_WIRETAPPING) {
        sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
                   << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
                   << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
      }
      MutexGuard mg(torquer::lazyLock());
      torquer::TORQUEING = true;
      torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
      rawPacket = commandBuffer;
      rawPacketLen = ImtqRequest::REQUEST_LEN;
      return returnvalue::OK;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::FAILED;
}

void ImtqHandler::fillCommandAndReplyMap() {
  insertInCommandMap(imtq::cmdIds::REQUEST);
  insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
  insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true);
}

ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                        DeviceCommandId_t* foundId, size_t* foundLen) {
  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
    return IGNORE_FULL_PACKET;
  }
  if (remainingSize > 0) {
    *foundLen = remainingSize;
    *foundId = imtq::cmdIds::REPLY;
    return returnvalue::OK;
  }
  return returnvalue::FAILED;
}

ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  ReturnValue_t result;
  ReturnValue_t status = returnvalue::OK;
  // arrayprinter::print(packet, ImtqReplies::BASE_LEN);
  if (requestStep == imtq::RequestType::MEASURE) {
    requestStep = imtq::RequestType::ACTUATE;
    ImtqRepliesDefault replies(packet);
    if (replies.wasEngHkRead()) {
      uint8_t* engHkReply = replies.getEngHk();
      result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
      if (result == returnvalue::OK) {
        fillEngHkDataset(hkDatasetNoTorque, engHkReply);
      } else {
        status = result;
      }
    }

    if (replies.wasGetSystemStateRead()) {
      uint8_t* sysStateReply = replies.getSystemState();
      result = parseStatusByte(imtq::CC::GET_SYSTEM_STATE, sysStateReply);
      if (result == returnvalue::OK) {
        fillSystemStateIntoDataset(sysStateReply);
      } else {
        status = result;
      }
    }
    if (replies.wasGetRawMgmMeasurementRead()) {
      uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
      result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
      if (result == returnvalue::OK) {
        fillRawMtmDataset(rawMgmMeasurement);
      } else {
        status = result;
      }
    }

    if (replies.wasCalibMgmMeasurementRead()) {
      uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
      result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
      if (result == returnvalue::OK) {
        fillRawMtmDataset(calibMgmMeasurement);
      } else {
        status = result;
      }
    }
  } else {
    requestStep = imtq::RequestType::MEASURE;
    ImtqRepliesWithTorque replies(packet);
    if (replies.wasDipoleActuationRead()) {
      parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
    }
    if (replies.wasGetRawMgmMeasurementRead()) {
      uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
      result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
      if (result == returnvalue::OK) {
        fillRawMtmDataset(rawMgmMeasurement);
      } else {
        status = result;
      }
    }
    if (replies.wasEngHkRead()) {
      uint8_t* engHkReply = replies.getEngHk();
      result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
      if (result != returnvalue::OK) {
        return result;
      } else {
        status = result;
      }
      fillEngHkDataset(hkDatasetNoTorque, engHkReply);
    }
  }
  return status;
}

LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
  if (sid == hkDatasetNoTorque.getSid()) {
    return &hkDatasetNoTorque;
  } else if (sid == dipoleSet.getSid()) {
    return &dipoleSet;
  } else if (sid == hkDatasetWithTorque.getSid()) {
    return &hkDatasetWithTorque;
  } else if (sid == rawMtmWithTorque.getSid()) {
    return &rawMtmWithTorque;
  } else if (sid == calMtmMeasurementSet.getSid()) {
    return &calMtmMeasurementSet;
  } else if (sid == rawMtmNoTorque.getSid()) {
    return &rawMtmNoTorque;
  } 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 housekeeping dataset */
  localDataPoolMap.emplace(imtq::STATUS_BYTE_MODE, &statusMode);
  localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig);
  localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError);
  localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime);
  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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));

  localDataPoolMap.emplace(imtq::DIPOLES_X, &dipoleXEntry);
  localDataPoolMap.emplace(imtq::DIPOLES_Y, &dipoleYEntry);
  localDataPoolMap.emplace(imtq::DIPOLES_Z, &dipoleZEntry);
  localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry);

  /** Entries of calibrated MTM measurement dataset */
  localDataPoolMap.emplace(imtq::MGM_CAL_NT, &mgmCalEntry);
  localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));

  /** Entries of raw MTM measurement dataset */
  localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>(3));
  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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::INIT_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::POS_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::POS_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_POS_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_NEG_X_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_POS_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_NEG_Y_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_POS_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_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<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(imtq::FINA_NEG_Z_COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));

  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(hkDatasetNoTorque.getSid(), false, 10.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(hkDatasetWithTorque.getSid(), false, 10.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(rawMtmNoTorque.getSid(), false, 10.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
  return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
}

ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
  DeviceCommandId_t commandId = getPendingCommand();
  switch (commandId) {
    case imtq::cmdIds::POS_X_SELF_TEST:
    case imtq::cmdIds::NEG_X_SELF_TEST:
    case imtq::cmdIds::POS_Y_SELF_TEST:
    case imtq::cmdIds::NEG_Y_SELF_TEST:
    case imtq::cmdIds::POS_Z_SELF_TEST:
    case imtq::cmdIds::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 returnvalue::OK;
}

ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) {
  uint8_t cmdErrorField = *(packet + 1) & 0xF;
  if (cmdErrorField == 0) {
    return returnvalue::OK;
  }
  sif::error << std::hex;
  switch (cmdErrorField) {
    case 1:
      sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
                 << " rejected without reason" << std::endl;
      return imtq::REJECTED_WITHOUT_REASON;
    case 2:
      sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
                 << " has invalid command code" << std::endl;
      return imtq::INVALID_COMMAND_CODE;
    case 3:
      sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
                 << " has missing parameter" << std::endl;
      return imtq::PARAMETER_MISSING;
    case 4:
      sif::error << "IMTQ::parseStatusByte: Command 0x" << std::setw(2) << command
                 << " has invalid parameter" << std::endl;
      return imtq::PARAMETER_INVALID;
    case 5:
      sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl;
      return imtq::CC_UNAVAILABLE;
    case 7:
      sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl;
      return imtq::INTERNAL_PROCESSING_ERROR;
    default:
      sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x"
                 << static_cast<int>(cmdErrorField) << std::endl;
      return imtq::CMD_ERR_UNKNOWN;
  }
}

void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) {
  PoolReadGuard rg(&hkDataset);
  uint8_t offset = 2;
  hkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 2;
  hkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 2;
  hkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
  offset += 2;
  hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
  offset += 2;
  hkDataset.coilXCurrentmA =
      static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
  offset += 2;
  hkDataset.coilYCurrentmA =
      static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
  offset += 2;
  hkDataset.coilZCurrentmA =
      static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
  offset += 2;
  hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
  offset += 2;
  hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
  offset += 2;
  hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
  offset += 2;
  size_t dummy = 2;
  SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy,
                                SerializeIF::Endianness::LITTLE);

  hkDataset.setValidity(true, true);

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

void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }

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 != returnvalue::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::cmdIds::GET_COMMANDED_DIPOLE);
}

void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
  PoolReadGuard rg(&calMtmMeasurementSet);
  calMtmMeasurementSet.setValidity(true, true);
  int8_t offset = 2;
  calMtmMeasurementSet.mgmXyz[0] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                   *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  calMtmMeasurementSet.mgmXyz[1] = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                   *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  calMtmMeasurementSet.mgmXyz[2] = *(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 (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mgmXyz[0] << " nT"
              << std::endl;
    sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mgmXyz[1] << " nT"
              << std::endl;
    sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mgmXyz[2] << " 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(&rawMtmNoTorque);
  unsigned int offset = 2;
  size_t deSerLen = 16;
  const uint8_t* dataStart = packet + offset;
  int32_t xRaw = 0;
  int32_t yRaw = 0;
  int32_t zRaw = 0;
  uint32_t coilActStatus = 0;
  auto res =
      SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
  if (res != returnvalue::OK) {
    return;
  }
  res =
      SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
  if (res != returnvalue::OK) {
    return;
  }
  res =
      SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
  if (res != returnvalue::OK) {
    return;
  }
  res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen,
                                      SerializeIF::Endianness::LITTLE);
  if (res != returnvalue::OK) {
    return;
  }
  rawMtmNoTorque.mtmRawNt[0] = xRaw * 7.5;
  rawMtmNoTorque.mtmRawNt[1] = yRaw * 7.5;
  rawMtmNoTorque.mtmRawNt[2] = zRaw * 7.5;
  rawMtmNoTorque.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
  rawMtmNoTorque.setValidity(true, true);
  if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "IMTQ raw MTM measurement X: " << rawMtmNoTorque.mtmRawNt[0] << " nT" << std::endl;
    sif::info << "IMTQ raw MTM measurement Y: " << rawMtmNoTorque.mtmRawNt[1] << " nT" << std::endl;
    sif::info << "IMTQ raw MTM measurement Z: " << rawMtmNoTorque.mtmRawNt[2] << " nT" << std::endl;
    sif::info << "IMTQ coil actuation status during MTM measurement: "
              << (unsigned int)rawMtmNoTorque.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::selfTest::step::X_POSITIVE: {
      handlePositiveXSelfTestReply(packet);
      break;
    }
    case imtq::selfTest::step::X_NEGATIVE: {
      handleNegativeXSelfTestReply(packet);
      break;
    }
    case imtq::selfTest::step::Y_POSITIVE: {
      handlePositiveYSelfTestReply(packet);
      break;
    }
    case imtq::selfTest::step::Y_NEGATIVE: {
      handleNegativeYSelfTestReply(packet);
      break;
    }
    case imtq::selfTest::step::Z_POSITIVE: {
      handlePositiveZSelfTestReply(packet);
      break;
    }
    case imtq::selfTest::step::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 (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 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 (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 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 (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 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 (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 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 (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 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 (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 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::setDebugMode(bool enable) { this->debugMode = enable; }

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

void ImtqHandler::fillSystemStateIntoDataset(const uint8_t* packet) {
  PoolReadGuard pg(&statusSet);
  statusSet.statusByteMode.value = packet[2];
  statusSet.statusByteError.value = packet[3];
  statusSet.statusByteConfig.value = packet[4];
  size_t dummy = 0;
  SerializeAdapter::deSerialize<uint32_t>(&statusSet.statusByteUptime.value, packet + 5, &dummy,
                                          SerializeIF::Endianness::LITTLE);
  statusSet.setValidity(true, true);
}

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

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