#include "ThermalController.h"

#include <bsp_q7s/core/CoreDefinitions.h>
#include <eive/objects.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/thermal/ThermalComponentIF.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
#include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <objects/systemObjectList.h>

ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater)
    : ExtendedControllerBase(objectId),
      heaterHandler(heater),
      sensorTemperatures(this),
      susTemperatures(this),
      deviceTemperatures(this),
      heaterInfo(this),
      imtqThermalSet(objects::IMTQ_HANDLER, ThermalStateCfg()),
      max31865Set0(objects::RTD_0_IC3_PLOC_HEATSPREADER,
                   EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set1(objects::RTD_1_IC4_PLOC_MISSIONBOARD,
                   EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set2(objects::RTD_2_IC5_4K_CAMERA, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set3(objects::RTD_3_IC6_DAC_HEATSPREADER, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set4(objects::RTD_4_IC7_STARTRACKER, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set5(objects::RTD_5_IC8_RW1_MX_MY, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set6(objects::RTD_6_IC9_DRO, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set7(objects::RTD_7_IC10_SCEX, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set8(objects::RTD_8_IC11_X8, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set9(objects::RTD_9_IC12_HPA, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set10(objects::RTD_10_IC13_PL_TX, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set11(objects::RTD_11_IC14_MPA, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set12(objects::RTD_12_IC15_ACU, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set13(objects::RTD_13_IC16_PLPCDU_HEATSPREADER,
                    EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set14(objects::RTD_14_IC17_TCS_BOARD, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      max31865Set15(objects::RTD_15_IC18_IMTQ, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      tmp1075SetTcs0(objects::TMP1075_HANDLER_TCS_0),
      tmp1075SetTcs1(objects::TMP1075_HANDLER_TCS_1),
      tmp1075SetPlPcdu0(objects::TMP1075_HANDLER_PLPCDU_0),
      // damaged
      // tmp1075SetPlPcdu1(objects::TMP1075_HANDLER_PLPCDU_1),
      tmp1075SetIfBoard(objects::TMP1075_HANDLER_IF_BOARD),
      susSet0(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
      susSet1(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
      susSet2(objects::SUS_2_N_LOC_XFYBZB_PT_YB),
      susSet3(objects::SUS_3_N_LOC_XFYBZF_PT_YF),
      susSet4(objects::SUS_4_N_LOC_XMYFZF_PT_ZF),
      susSet5(objects::SUS_5_N_LOC_XFYMZB_PT_ZB),
      susSet6(objects::SUS_6_R_LOC_XFYBZM_PT_XF),
      susSet7(objects::SUS_7_R_LOC_XBYBZM_PT_XB),
      susSet8(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
      susSet9(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
      susSet10(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
      susSet11(objects::SUS_11_R_LOC_XBYMZB_PT_ZB) {
  resetSensorsArray();
}

ReturnValue_t ThermalController::initialize() {
  auto* camSwitcher = ObjectManager::instance()->get<HasHealthIF>(objects::CAM_SWITCHER);
  if (camSwitcher == nullptr) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  camId = camSwitcher->getCommandQueue();
  return ExtendedControllerBase::initialize();
}

ReturnValue_t ThermalController::handleCommandMessage(CommandMessage* message) {
  return returnvalue::FAILED;
}

void ThermalController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
  trace::threadTrace(opCounter, "TCS Task");
#endif
  switch (internalState) {
    case InternalState::STARTUP: {
      initialCountdown.resetTimer();
      internalState = InternalState::INITIAL_DELAY;
      return;
    }
    case InternalState::INITIAL_DELAY: {
      if (initialCountdown.hasTimedOut()) {
        internalState = InternalState::READY;
      }
      return;
    }
    case InternalState::READY: {
      break;
    }
    default:
      break;
  }

  {
    PoolReadGuard pg(&sensorTemperatures);
    if (pg.getReadResult() == returnvalue::OK) {
      copySensors();
    }
  }
  {
    PoolReadGuard pg(&susTemperatures);
    if (pg.getReadResult() == returnvalue::OK) {
      copySus();
    }
  }
  {
    PoolReadGuard pg(&deviceTemperatures);
    if (pg.getReadResult() == returnvalue::OK) {
      copyDevices();
    }
  }

  std::array<HeaterHandler::SwitchState, 8> heaterStates;
  heaterHandler.getAllSwitchStates(heaterStates);
  {
    PoolReadGuard pg(&heaterInfo);
    std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8);
    {
      PoolReadGuard pg2(&currentVecPdu2);
      if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) {
        heaterInfo.heaterCurrent.value = currentVecPdu2.value[PDU2::Channels::TCS_HEATER_IN];
      }
    }
  }

  performThermalModuleCtrl();
}

ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                         LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_HEATSPREADER, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_MISSIONBOARD, new PoolEntry<float>({1.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_4K_CAMERA, new PoolEntry<float>({2.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_DAC_HEATSPREADER, new PoolEntry<float>({3.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_STARTRACKER, new PoolEntry<float>({4.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_RW1, new PoolEntry<float>({5.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_DRO, new PoolEntry<float>({6.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_SCEX, new PoolEntry<float>({7.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_X8, new PoolEntry<float>({8.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_HPA, new PoolEntry<float>({9.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_TX_MODUL, new PoolEntry<float>({10.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_MPA, new PoolEntry<float>({11.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_ACU, new PoolEntry<float>({12.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLPCDU_HEATSPREADER, new PoolEntry<float>({13.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_TCS_BOARD, new PoolEntry<float>({14.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_MAGNETTORQUER, new PoolEntry<float>({15.0}));
  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);

  localDataPoolMap.emplace(tcsCtrl::SUS_0_N_LOC_XFYFZM_PT_XF, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_6_R_LOC_XFYBZM_PT_XF, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_1_N_LOC_XBYFZM_PT_XB, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_7_R_LOC_XBYBZM_PT_XB, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_2_N_LOC_XFYBZB_PT_YB, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_8_R_LOC_XBYBZB_PT_YB, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_3_N_LOC_XFYBZF_PT_YF, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_9_R_LOC_XBYBZB_PT_YF, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_4_N_LOC_XMYFZF_PT_ZF, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_10_N_LOC_XMYBZF_PT_ZF, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_5_N_LOC_XFYMZB_PT_ZB, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::SUS_11_R_LOC_XBYMZB_PT_ZB, new PoolEntry<float>({0.0}));

  localDataPoolMap.emplace(tcsCtrl::COMPONENT_RW, new PoolEntry<float>({0.0}));

  localDataPoolMap.emplace(tcsCtrl::TEMP_Q7S, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_1, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_2, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_3, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_4, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_RW1, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_RW2, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_RW3, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_RW4, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_STAR_TRACKER, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_POWER_AMPLIFIER, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_BASEBAND_BOARD, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_MGT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_ACU, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU1, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU2, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_0_SIDE_A, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_1_SIDE_A, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_2_SIDE_B, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_3_SIDE_B, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_0_SIDE_A, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_2_SIDE_B, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::TEMP_ADC_PAYLOAD_PCDU, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(tcsCtrl::HEATER_SWITCH_LIST, &heaterSwitchStates);
  localDataPoolMap.emplace(tcsCtrl::HEATER_CURRENT, &heaterCurrent);

  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(sensorTemperatures.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(susTemperatures.getSid(), false, 10.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), false, 10.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), false, 10.0));
  return returnvalue::OK;
}

LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case tcsCtrl::SENSOR_TEMPERATURES:
      return &sensorTemperatures;
    case tcsCtrl::SUS_TEMPERATURES:
      return &susTemperatures;
    case tcsCtrl::DEVICE_TEMPERATURES:
      return &deviceTemperatures;
    case tcsCtrl::HEATER_SET:
      return &heaterInfo;
    default:
      return nullptr;
  }
}

ReturnValue_t ThermalController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                  uint32_t* msToReachTheMode) {
  if (submode != SUBMODE_NONE) {
    return INVALID_SUBMODE;
  }
  if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) {
    return INVALID_MODE;
  }
  return returnvalue::OK;
}

void ThermalController::copySensors() {
  {
    PoolReadGuard pg0(&max31865Set0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg0.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_ploc_heatspreader.value = max31865Set0.temperatureCelcius.value;
      sensorTemperatures.sensor_ploc_heatspreader.setValid(
          max31865Set0.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_ploc_heatspreader.isValid()) {
        sensorTemperatures.sensor_ploc_heatspreader.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg1(&max31865Set1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg1.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_ploc_missionboard.value = max31865Set1.temperatureCelcius.value;
      sensorTemperatures.sensor_ploc_missionboard.setValid(
          max31865Set1.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_ploc_missionboard.isValid()) {
        sensorTemperatures.sensor_ploc_missionboard.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg2(&max31865Set2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg2.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_4k_camera.value = max31865Set2.temperatureCelcius.value;
      sensorTemperatures.sensor_4k_camera.setValid(max31865Set2.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_4k_camera.isValid()) {
        sensorTemperatures.sensor_4k_camera.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg3(&max31865Set3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg3.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_dac_heatspreader.value = max31865Set3.temperatureCelcius.value;
      sensorTemperatures.sensor_dac_heatspreader.setValid(
          max31865Set3.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_dac_heatspreader.isValid()) {
        sensorTemperatures.sensor_dac_heatspreader.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg4(&max31865Set4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg4.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_startracker.value = max31865Set4.temperatureCelcius.value;
      sensorTemperatures.sensor_startracker.setValid(max31865Set4.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_startracker.isValid()) {
        sensorTemperatures.sensor_startracker.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg5(&max31865Set5, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg5.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_rw1.value = max31865Set5.temperatureCelcius.value;
      sensorTemperatures.sensor_rw1.setValid(max31865Set5.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_rw1.isValid()) {
        sensorTemperatures.sensor_rw1.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg6(&max31865Set6, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg6.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_dro.value = max31865Set6.temperatureCelcius.value;
      sensorTemperatures.sensor_dro.setValid(max31865Set6.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_dro.isValid()) {
        sensorTemperatures.sensor_dro.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg7(&max31865Set7, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg7.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_scex.value = max31865Set7.temperatureCelcius.value;
      sensorTemperatures.sensor_scex.setValid(max31865Set7.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_scex.isValid()) {
        sensorTemperatures.sensor_scex.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg8(&max31865Set8, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg8.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_x8.value = max31865Set8.temperatureCelcius.value;
      sensorTemperatures.sensor_x8.setValid(max31865Set8.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_x8.isValid()) {
        sensorTemperatures.sensor_x8.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg9(&max31865Set9, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg9.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_hpa.value = max31865Set9.temperatureCelcius.value;
      sensorTemperatures.sensor_hpa.setValid(max31865Set9.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_hpa.isValid()) {
        sensorTemperatures.sensor_hpa.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg10(&max31865Set10, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg10.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_tx_modul.value = max31865Set10.temperatureCelcius.value;
      sensorTemperatures.sensor_tx_modul.setValid(max31865Set10.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_tx_modul.isValid()) {
        sensorTemperatures.sensor_tx_modul.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg11(&max31865Set11, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg11.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_mpa.value = max31865Set11.temperatureCelcius.value;
      sensorTemperatures.sensor_mpa.setValid(max31865Set11.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_mpa.isValid()) {
        sensorTemperatures.sensor_mpa.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg12(&max31865Set12, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg12.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_acu.value = max31865Set12.temperatureCelcius.value;
      sensorTemperatures.sensor_acu.setValid(max31865Set12.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_acu.isValid()) {
        sensorTemperatures.sensor_acu.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg13(&max31865Set13, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg13.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_plpcdu_heatspreader.value = max31865Set13.temperatureCelcius.value;
      sensorTemperatures.sensor_plpcdu_heatspreader.setValid(
          max31865Set13.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_plpcdu_heatspreader.isValid()) {
        sensorTemperatures.sensor_plpcdu_heatspreader.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg14(&max31865Set14, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg14.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_tcs_board.value = max31865Set14.temperatureCelcius.value;
      sensorTemperatures.sensor_tcs_board.setValid(max31865Set14.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_tcs_board.isValid()) {
        sensorTemperatures.sensor_tcs_board.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg15(&max31865Set15, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg15.getReadResult() == returnvalue::OK) {
      sensorTemperatures.sensor_magnettorquer.value = max31865Set15.temperatureCelcius.value;
      sensorTemperatures.sensor_magnettorquer.setValid(max31865Set15.temperatureCelcius.isValid());
      if (not sensorTemperatures.sensor_magnettorquer.isValid()) {
        sensorTemperatures.sensor_magnettorquer.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg(&tmp1075SetTcs0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
      sensorTemperatures.tmp1075Tcs0.value = tmp1075SetTcs0.temperatureCelcius.value;
      sensorTemperatures.tmp1075Tcs0.setValid(tmp1075SetTcs0.temperatureCelcius.isValid());
      if (not tmp1075SetTcs0.temperatureCelcius.isValid()) {
        sensorTemperatures.tmp1075Tcs0.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg(&tmp1075SetTcs1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
      sensorTemperatures.tmp1075Tcs1.value = tmp1075SetTcs1.temperatureCelcius.value;
      sensorTemperatures.tmp1075Tcs1.setValid(tmp1075SetTcs1.temperatureCelcius.isValid());
      if (not tmp1075SetTcs1.temperatureCelcius.isValid()) {
        sensorTemperatures.tmp1075Tcs1.value = INVALID_TEMPERATURE;
      }
    }
  }
  {
    PoolReadGuard pg(&tmp1075SetPlPcdu0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
      sensorTemperatures.tmp1075PlPcdu0.value = tmp1075SetPlPcdu0.temperatureCelcius.value;
      sensorTemperatures.tmp1075PlPcdu0.setValid(tmp1075SetPlPcdu0.temperatureCelcius.isValid());
      if (not tmp1075SetPlPcdu0.temperatureCelcius.isValid()) {
        sensorTemperatures.tmp1075PlPcdu0.value = INVALID_TEMPERATURE;
      }
    }
  }
  // damaged
  /*
  {
    PoolReadGuard pg(&tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
      sensorTemperatures.tmp1075PlPcdu1.value = tmp1075SetPlPcdu1.temperatureCelcius.value;
      sensorTemperatures.tmp1075PlPcdu1.setValid(tmp1075SetPlPcdu1.temperatureCelcius.isValid());
      if (not tmp1075SetPlPcdu1.temperatureCelcius.isValid()) {
        sensorTemperatures.tmp1075PlPcdu1.value = INVALID_TEMPERATURE;
      }
    }
  }
  */
  {
    PoolReadGuard pg(&tmp1075SetIfBoard, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
      sensorTemperatures.tmp1075IfBrd.value = tmp1075SetIfBoard.temperatureCelcius.value;
      sensorTemperatures.tmp1075IfBrd.setValid(tmp1075SetIfBoard.temperatureCelcius.isValid());
      if (not tmp1075SetIfBoard.temperatureCelcius.isValid()) {
        sensorTemperatures.tmp1075IfBrd.value = INVALID_TEMPERATURE;
      }
    }
  }
}

void ThermalController::copySus() {
  {
    PoolReadGuard pg0(&susSet0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg0.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.tempC.value;
      susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.tempC.isValid());
      if (not susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.isValid()) {
        susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg1(&susSet1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg1.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.tempC.value;
      susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.tempC.isValid());
      if (not susTemperatures.sus_6_r_loc_xfybzm_pt_xf.isValid()) {
        susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg2(&susSet2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg2.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.tempC.value;
      susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.tempC.isValid());
      if (not susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.isValid()) {
        susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg3(&susSet3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg3.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.tempC.value;
      susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.tempC.isValid());
      if (not susTemperatures.sus_7_r_loc_xbybzm_pt_xb.isValid()) {
        susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg4(&susSet4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg4.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.tempC.value;
      susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.tempC.isValid());
      if (not susTemperatures.sus_2_n_loc_xfybzb_pt_yb.isValid()) {
        susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg5(&susSet5, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg5.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.tempC.value;
      susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.tempC.isValid());
      if (not susTemperatures.sus_8_r_loc_xbybzb_pt_yb.isValid()) {
        susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg6(&susSet6, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg6.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.tempC.value;
      susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.tempC.isValid());
      if (not susTemperatures.sus_3_n_loc_xfybzf_pt_yf.isValid()) {
        susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg7(&susSet7, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg7.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.tempC.value;
      susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.tempC.isValid());
      if (not susTemperatures.sus_9_r_loc_xbybzb_pt_yf.isValid()) {
        susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg8(&susSet8, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg8.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.tempC.value;
      susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.tempC.isValid());
      if (not susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.isValid()) {
        susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg9(&susSet9, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg9.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.tempC.value;
      susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.tempC.isValid());
      if (not susTemperatures.sus_10_n_loc_xmybzf_pt_zf.isValid()) {
        susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg10(&susSet10, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg10.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.tempC.value;
      susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.tempC.isValid());
      if (not susTemperatures.sus_5_n_loc_xfymzb_pt_zb.isValid()) {
        susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = INVALID_TEMPERATURE;
      }
    }
  }

  {
    PoolReadGuard pg11(&susSet11, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg11.getReadResult() == returnvalue::OK) {
      susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.tempC.value;
      susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.tempC.isValid());
      if (not susTemperatures.sus_11_r_loc_xbymzb_pt_zb.isValid()) {
        susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = INVALID_TEMPERATURE;
      }
    }
  }
}

void ThermalController::copyDevices() {
  lp_var_t<float> tempQ7s = lp_var_t<float>(objects::CORE_CONTROLLER, core::PoolIds::TEMPERATURE);
  {
    PoolReadGuard pg(&tempQ7s, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
      deviceTemperatures.q7s = tempQ7s;
      deviceTemperatures.q7s.setValid(tempQ7s.isValid());
    } else {
      deviceTemperatures.q7s.setValid(false);
      deviceTemperatures.q7s = static_cast<float>(INVALID_TEMPERATURE);
    }
  }

  {
    lp_var_t<int16_t> battTemp1 =
        lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_1);
    PoolReadGuard pg(&battTemp1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read battery temperature 1" << std::endl;
      deviceTemperatures.batteryTemp1.setValid(false);
      deviceTemperatures.batteryTemp1 = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.batteryTemp1 = battTemp1;
      deviceTemperatures.batteryTemp1.setValid(battTemp1.isValid());
    }
  }

  {
    lp_var_t<int16_t> battTemp2 =
        lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_2);
    PoolReadGuard pg(&battTemp2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read battery temperature 2" << std::endl;
      deviceTemperatures.batteryTemp2.setValid(false);
      deviceTemperatures.batteryTemp2 = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.batteryTemp2 = battTemp2;
      deviceTemperatures.batteryTemp2.setValid(battTemp2.isValid());
    }
  }

  {
    lp_var_t<int16_t> battTemp3 =
        lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_3);
    PoolReadGuard pg(&battTemp3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read battery temperature 3" << std::endl;
      deviceTemperatures.batteryTemp3.setValid(false);
      deviceTemperatures.batteryTemp3 = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.batteryTemp3 = battTemp3;
      deviceTemperatures.batteryTemp3.setValid(battTemp3.isValid());
    }
  }

  {
    lp_var_t<int16_t> battTemp4 =
        lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_4);
    PoolReadGuard pg(&battTemp4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read battery temperature 4" << std::endl;
      deviceTemperatures.batteryTemp4.setValid(false);
      deviceTemperatures.batteryTemp4 = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.batteryTemp4 = battTemp4;
      deviceTemperatures.batteryTemp4.setValid(battTemp4.isValid());
    }
  }

  {
    lp_var_t<int32_t> tempRw1 = lp_var_t<int32_t>(objects::RW1, rws::TEMPERATURE_C);
    PoolReadGuard pg(&tempRw1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl;
      deviceTemperatures.rw1.setValid(false);
      deviceTemperatures.rw1 = static_cast<int32_t>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.rw1.setValid(tempRw1.isValid());
      deviceTemperatures.rw1 = tempRw1;
    }
  }

  {
    lp_var_t<int32_t> tempRw2 = lp_var_t<int32_t>(objects::RW2, rws::TEMPERATURE_C);
    PoolReadGuard pg(&tempRw2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl;
      deviceTemperatures.rw2.setValid(false);
      deviceTemperatures.rw2 = static_cast<int32_t>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.rw2.setValid(tempRw2.isValid());
      deviceTemperatures.rw2 = tempRw2;
    }
  }

  {
    lp_var_t<int32_t> tempRw3 = lp_var_t<int32_t>(objects::RW3, rws::TEMPERATURE_C);
    PoolReadGuard pg(&tempRw3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl;
      deviceTemperatures.rw3.setValid(false);
      deviceTemperatures.rw3 = static_cast<int32_t>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.rw3.setValid(tempRw3.isValid());
      deviceTemperatures.rw3 = tempRw3;
    }
  }

  {
    lp_var_t<int32_t> tempRw4 = lp_var_t<int32_t>(objects::RW4, rws::TEMPERATURE_C);
    PoolReadGuard pg(&tempRw4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl;
      deviceTemperatures.rw4.setValid(false);
      deviceTemperatures.rw4 = static_cast<int32_t>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.rw4.setValid(tempRw4.isValid());
      deviceTemperatures.rw4 = tempRw4;
    }
  }

  {
    lp_var_t<float> tempStartracker =
        lp_var_t<float>(objects::STAR_TRACKER, startracker::MCU_TEMPERATURE);
    PoolReadGuard pg(&tempStartracker, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read startracker temperature" << std::endl;
      deviceTemperatures.startracker.setValid(false);
      deviceTemperatures.startracker = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.startracker.setValid(tempStartracker.isValid());
      deviceTemperatures.startracker = tempStartracker;
    }
  }

  {
    lp_var_t<float> tempSyrlinksPowerAmplifier =
        lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_POWER_AMPLIFIER);
    PoolReadGuard pg(&tempSyrlinksPowerAmplifier, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read syrlinks power amplifier temperature"
                   << std::endl;
      deviceTemperatures.syrlinksPowerAmplifier.setValid(false);
      deviceTemperatures.syrlinksPowerAmplifier = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.syrlinksPowerAmplifier.setValid(tempSyrlinksPowerAmplifier.isValid());
      deviceTemperatures.syrlinksPowerAmplifier = tempSyrlinksPowerAmplifier;
    }
  }

  {
    lp_var_t<float> tempSyrlinksBasebandBoard =
        lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_BASEBAND_BOARD);
    PoolReadGuard pg(&tempSyrlinksBasebandBoard, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read syrlinks baseband board temperature"
                   << std::endl;
      deviceTemperatures.syrlinksBasebandBoard.setValid(false);
      deviceTemperatures.syrlinksBasebandBoard = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.syrlinksBasebandBoard.setValid(tempSyrlinksBasebandBoard.isValid());
      deviceTemperatures.syrlinksBasebandBoard = tempSyrlinksBasebandBoard;
    }
  }

  {
    lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, imtq::MCU_TEMPERATURE);
    PoolReadGuard pg(&tempMgt, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl;
      deviceTemperatures.mgt.setValid(false);
      deviceTemperatures.mgt = static_cast<int16_t>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.mgt.setValid(tempMgt.isValid());
      deviceTemperatures.mgt = tempMgt;
    }
  }

  {
    lp_vec_t<float, 3> tempAcu =
        lp_vec_t<float, 3>(objects::ACU_HANDLER, ACU::pool::ACU_TEMPERATURES);
    PoolReadGuard pg(&tempAcu, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read ACU temperatures" << std::endl;
      deviceTemperatures.acu.setValid(false);
      deviceTemperatures.acu[0] = static_cast<float>(INVALID_TEMPERATURE);
      deviceTemperatures.acu[1] = static_cast<float>(INVALID_TEMPERATURE);
      deviceTemperatures.acu[2] = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.acu.setValid(tempAcu.isValid());
      deviceTemperatures.acu = tempAcu;
    }
  }

  {
    lp_var_t<float> tempPdu1 = lp_var_t<float>(objects::PDU1_HANDLER, PDU::pool::PDU_TEMPERATURE);
    PoolReadGuard pg(&tempPdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read PDU1 temperature" << std::endl;
      deviceTemperatures.pdu1.setValid(false);
      deviceTemperatures.pdu1 = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.pdu1.setValid(tempPdu1.isValid());
      deviceTemperatures.pdu1 = tempPdu1;
    }
  }

  {
    lp_var_t<float> tempPdu2 = lp_var_t<float>(objects::PDU2_HANDLER, PDU::pool::PDU_TEMPERATURE);
    PoolReadGuard pg(&tempPdu2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read PDU2 temperature" << std::endl;
      deviceTemperatures.pdu2.setValid(false);
      deviceTemperatures.pdu2 = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.pdu2.setValid(tempPdu2.isValid());
      deviceTemperatures.pdu2 = tempPdu2;
    }
  }

  {
    lp_var_t<float> temp1P60dock =
        lp_var_t<float>(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_1);
    PoolReadGuard pg(&temp1P60dock, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read P60 dock temperature 1" << std::endl;
      deviceTemperatures.temp1P60dock.setValid(false);
      deviceTemperatures.temp1P60dock = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.temp1P60dock.setValid(temp1P60dock.isValid());
      deviceTemperatures.temp1P60dock = temp1P60dock;
    }
  }

  {
    lp_var_t<float> temp2P60dock =
        lp_var_t<float>(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_2);
    PoolReadGuard pg(&temp2P60dock, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read P60 dock temperature 2" << std::endl;
      deviceTemperatures.temp2P60dock.setValid(false);
      deviceTemperatures.temp2P60dock = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.temp2P60dock.setValid(temp2P60dock.isValid());
      deviceTemperatures.temp2P60dock = temp2P60dock;
    }
  }

  {
    lp_var_t<float> tempGyro0 =
        lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE);
    PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl;
      deviceTemperatures.gyro0SideA.setValid(false);
      deviceTemperatures.gyro0SideA = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.gyro0SideA.setValid(tempGyro0.isValid());
      deviceTemperatures.gyro0SideA = tempGyro0;
    }
  }

  {
    lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE);
    PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl;
      deviceTemperatures.gyro1SideA.setValid(false);
      deviceTemperatures.gyro1SideA = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.gyro1SideA.setValid(tempGyro1.isValid());
      deviceTemperatures.gyro1SideA = tempGyro1;
    }
  }

  {
    lp_var_t<float> tempGyro2 =
        lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE);
    PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl;
      deviceTemperatures.gyro2SideB.setValid(false);
      deviceTemperatures.gyro2SideB = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.gyro2SideB.setValid(tempGyro2.isValid());
      deviceTemperatures.gyro2SideB = tempGyro2;
    }
  }

  {
    lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE);
    PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl;
      deviceTemperatures.gyro3SideB.setValid(false);
      deviceTemperatures.gyro3SideB = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.gyro3SideB.setValid(tempGyro3.isValid());
      deviceTemperatures.gyro3SideB = tempGyro3;
    }
  }

  {
    lp_var_t<float> tempMgm0 =
        lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
    PoolReadGuard pg(&tempMgm0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl;
      deviceTemperatures.mgm0SideA.setValid(false);
      deviceTemperatures.mgm0SideA = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.mgm0SideA.setValid(tempMgm0.isValid());
      deviceTemperatures.mgm0SideA = tempMgm0;
    }
  }

  {
    lp_var_t<float> tempMgm2 =
        lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
    PoolReadGuard pg(&tempMgm2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl;
      deviceTemperatures.mgm2SideB.setValid(false);
      deviceTemperatures.mgm2SideB = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.mgm2SideB.setValid(tempMgm2.isValid());
      deviceTemperatures.mgm2SideB = tempMgm2;
    }
  }

  {
    lp_var_t<float> tempAdcPayloadPcdu = lp_var_t<float>(objects::PLPCDU_HANDLER, plpcdu::TEMP);
    PoolReadGuard pg(&tempAdcPayloadPcdu, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() != returnvalue::OK) {
      sif::warning << "ThermalController: Failed to read payload PCDU ADC temperature" << std::endl;
      deviceTemperatures.adcPayloadPcdu.setValid(false);
      deviceTemperatures.adcPayloadPcdu = static_cast<float>(INVALID_TEMPERATURE);
    } else {
      deviceTemperatures.adcPayloadPcdu.setValid(tempAdcPayloadPcdu.isValid());
      deviceTemperatures.adcPayloadPcdu = tempAdcPayloadPcdu;
    }
  }
}

void ThermalController::ctrlAcsBoard() {
  // TODO: check
  heater::Switchers switchNr = heater::HEATER_2_ACS_BRD;
  heater::Switchers redSwitchNr = heater::HEATER_0_OBC_BRD;

  // A side
  thermalComponent = ACS_BOARD;
  sensors[0].first = deviceTemperatures.gyro0SideA.isValid();
  sensors[0].second = deviceTemperatures.gyro0SideA.value;
  sensors[1].first = deviceTemperatures.mgm0SideA.isValid();
  sensors[1].second = deviceTemperatures.mgm0SideA.value;
  sensors[2].first = deviceTemperatures.gyro1SideA.isValid();
  sensors[2].second = deviceTemperatures.gyro1SideA.value;
  sensors[3].first = sensorTemperatures.sensor_tcs_board.isValid();
  sensors[3].second = sensorTemperatures.sensor_tcs_board.value;
  numSensors = 4;
  if (selectAndReadSensorTemp()) {
    if (chooseHeater(switchNr, redSwitchNr)) {
      checkLimitsAndCtrlHeater(switchNr, redSwitchNr, acsBoardLimits);
    }
    resetSensorsArray();
    return;
  }

  // B side
  sensors[0].first = deviceTemperatures.gyro2SideB.isValid();
  sensors[0].second = deviceTemperatures.gyro2SideB.value;
  sensors[1].first = deviceTemperatures.mgm2SideB.isValid();
  sensors[1].second = deviceTemperatures.mgm2SideB.value;
  sensors[2].first = deviceTemperatures.gyro3SideB.isValid();
  sensors[2].second = deviceTemperatures.gyro3SideB.value;
  sensors[3].first = sensorTemperatures.sensor_tcs_board.isValid();
  sensors[3].second = sensorTemperatures.sensor_tcs_board.value;
  numSensors = 4;
  if (selectAndReadSensorTemp()) {
    if (chooseHeater(switchNr, redSwitchNr)) {
      checkLimitsAndCtrlHeater(switchNr, redSwitchNr, acsBoardLimits);
    }
  } else {
    if (chooseHeater(switchNr, redSwitchNr)) {
      if (heaterHandler.checkSwitchState(switchNr)) {
        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
        sif::info << "ThermalController::checkLimitsAndCtrlHeater: Heater"
                  << static_cast<int>(thermalComponent) << " OFF" << std::endl;
      }
    }
  }
  resetSensorsArray();
}

void ThermalController::ctrlMgt() {
  thermalComponent = MGT;
  PoolReadGuard pg(&imtqThermalSet);
  auto heaterReq =
      static_cast<ThermalComponentIF::StateRequest>(imtqThermalSet.heaterRequest.value);

  if (heaterReq == ThermalComponentIF::STATE_REQUEST_OPERATIONAL) {
    sensors[0].first = sensorTemperatures.sensor_magnettorquer.isValid();
    sensors[0].second = sensorTemperatures.sensor_magnettorquer.value;
    sensors[1].first = deviceTemperatures.mgt.isValid();
    sensors[1].second = deviceTemperatures.mgt.value;
    sensors[2].first = sensorTemperatures.sensor_plpcdu_heatspreader.isValid();
    sensors[2].second = sensorTemperatures.sensor_plpcdu_heatspreader.value;
    numSensors = 3;
    ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits);
  }
}

void ThermalController::ctrlRw() {
  Event eventToTrigger = 0;
  bool oneIsAboveLimit = false;

  std::array<uint32_t, 4> sensorTemps{};

  // RW1
  thermalComponent = RW;
  sensors[0].first = sensorTemperatures.sensor_rw1.isValid();
  sensors[0].second = sensorTemperatures.sensor_rw1.value;
  sensors[1].first = deviceTemperatures.rw1.isValid();
  sensors[1].second = deviceTemperatures.rw1.value;
  sensors[2].first = deviceTemperatures.rw4.isValid();
  sensors[2].second = deviceTemperatures.rw4.value;
  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
  sensors[3].second = sensorTemperatures.sensor_dro.value;
  numSensors = 4;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
  sensorTemps[0] = tempFloatToU32();
  if (componentAboveUpperLimit) {
    oneIsAboveLimit = true;
    eventToTrigger = overHeatEventToTrigger;
  }

  // RW2
  thermalComponent = RW;
  sensors[0].first = deviceTemperatures.rw2.isValid();
  sensors[0].second = deviceTemperatures.rw2.value;
  sensors[1].first = deviceTemperatures.rw3.isValid();
  sensors[1].second = deviceTemperatures.rw3.value;
  sensors[2].first = sensorTemperatures.sensor_rw1.isValid();
  sensors[2].second = sensorTemperatures.sensor_rw1.value;
  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
  sensors[3].second = sensorTemperatures.sensor_dro.value;
  numSensors = 4;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
  sensorTemps[1] = tempFloatToU32();
  if (componentAboveUpperLimit) {
    oneIsAboveLimit = true;
    if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) {
      eventToTrigger = overHeatEventToTrigger;
    }
  }

  // RW3
  thermalComponent = RW;
  sensors[0].first = deviceTemperatures.rw3.isValid();
  sensors[0].second = deviceTemperatures.rw3.value;
  sensors[1].first = deviceTemperatures.rw4.isValid();
  sensors[1].second = deviceTemperatures.rw4.value;
  sensors[2].first = sensorTemperatures.sensor_rw1.isValid();
  sensors[2].second = sensorTemperatures.sensor_rw1.value;
  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
  sensors[3].second = sensorTemperatures.sensor_dro.value;
  numSensors = 4;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
  sensorTemps[2] = tempFloatToU32();
  if (componentAboveUpperLimit) {
    oneIsAboveLimit = true;
    if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) {
      eventToTrigger = overHeatEventToTrigger;
    }
  }

  // RW4
  thermalComponent = RW;
  sensors[0].first = deviceTemperatures.rw4.isValid();
  sensors[0].second = deviceTemperatures.rw4.value;
  sensors[1].first = deviceTemperatures.rw1.isValid();
  sensors[1].second = deviceTemperatures.rw1.value;
  sensors[2].first = sensorTemperatures.sensor_rw1.isValid();
  sensors[2].second = sensorTemperatures.sensor_rw1.value;
  sensors[3].first = sensorTemperatures.sensor_dro.isValid();
  sensors[3].second = sensorTemperatures.sensor_dro.value;
  numSensors = 4;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_6_DRO, rwLimits);
  sensorTemps[3] = tempFloatToU32();
  if (componentAboveUpperLimit) {
    oneIsAboveLimit = true;
    if (eventToTrigger != ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH) {
      eventToTrigger = overHeatEventToTrigger;
    }
  }

  if (oneIsAboveLimit and not rwTooHotFlag) {
    EventManagerIF::triggerEvent(objects::RW1, eventToTrigger, sensorTemps[0]);
    EventManagerIF::triggerEvent(objects::RW2, eventToTrigger, sensorTemps[1]);
    EventManagerIF::triggerEvent(objects::RW3, eventToTrigger, sensorTemps[2]);
    EventManagerIF::triggerEvent(objects::RW4, eventToTrigger, sensorTemps[3]);
    rwTooHotFlag = true;
  } else if (not oneIsAboveLimit) {
    rwTooHotFlag = false;
  }
}

void ThermalController::ctrlStr() {
  thermalComponent = STR;
  sensors[0].first = sensorTemperatures.sensor_startracker.isValid();
  sensors[0].second = sensorTemperatures.sensor_startracker.value;
  sensors[1].first = deviceTemperatures.startracker.isValid();
  sensors[1].second = deviceTemperatures.startracker.value;
  sensors[2].first = sensorTemperatures.sensor_dro.isValid();
  sensors[2].second = sensorTemperatures.sensor_dro.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_5_STR, heater::HEATER_6_DRO, strLimits);
  tooHotHandler(objects::STAR_TRACKER, strTooHotFlag);
}

void ThermalController::ctrlIfBoard() {
  thermalComponent = IF_BOARD;
  sensors[0].first = sensorTemperatures.tmp1075IfBrd.isValid();
  sensors[0].second = sensorTemperatures.tmp1075IfBrd.value;
  sensors[1].first = sensorTemperatures.sensor_magnettorquer.isValid();
  sensors[1].second = sensorTemperatures.sensor_magnettorquer.value;
  sensors[2].first = deviceTemperatures.mgm2SideB.isValid();
  sensors[2].second = deviceTemperatures.mgm2SideB.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, ifBoardLimits);
  // TODO: special event overheating + could go back to safe mode
}

void ThermalController::ctrlTcsBoard() {
  thermalComponent = TCS_BOARD;
  sensors[0].first = sensorTemperatures.sensor_tcs_board.isValid();
  sensors[0].second = sensorTemperatures.sensor_tcs_board.value;
  sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid();
  sensors[1].second = sensorTemperatures.tmp1075Tcs0.value;
  sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs1.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits);
  // TODO: special event overheating + could go back to safe mode
}

void ThermalController::ctrlObc() {
  thermalComponent = OBC;
  sensors[0].first = deviceTemperatures.q7s.isValid();
  sensors[0].second = deviceTemperatures.q7s.value;
  sensors[1].first = sensorTemperatures.tmp1075Tcs1.isValid();
  sensors[1].second = sensorTemperatures.tmp1075Tcs1.value;
  sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs0.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits);
  if (componentAboveUpperLimit and not obcTooHotFlag) {
    triggerEvent(tcsCtrl::OBC_OVERHEATING);
    obcTooHotFlag = true;
  } else if (not componentAboveUpperLimit) {
    obcTooHotFlag = false;
  }
}

void ThermalController::ctrlObcIfBoard() {
  thermalComponent = OBCIF_BOARD;
  sensors[0].first = deviceTemperatures.q7s.isValid();
  sensors[0].second = deviceTemperatures.q7s.value;
  sensors[1].first = sensorTemperatures.tmp1075Tcs0.isValid();
  sensors[1].second = sensorTemperatures.tmp1075Tcs0.value;
  sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs1.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits);
  if (componentAboveUpperLimit and not obcTooHotFlag) {
    triggerEvent(tcsCtrl::OBC_OVERHEATING);
    obcTooHotFlag = true;
  } else if (not componentAboveUpperLimit) {
    obcTooHotFlag = false;
  }
}

void ThermalController::ctrlSBandTransceiver() {
  thermalComponent = SBAND_TRANSCEIVER;
  sensors[0].first = deviceTemperatures.syrlinksPowerAmplifier.isValid();
  sensors[0].second = deviceTemperatures.syrlinksPowerAmplifier.value;
  sensors[1].first = deviceTemperatures.syrlinksBasebandBoard.isValid();
  sensors[1].second = deviceTemperatures.syrlinksBasebandBoard.value;
  sensors[2].first = sensorTemperatures.sensor_4k_camera.isValid();
  sensors[2].second = sensorTemperatures.sensor_4k_camera.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_7_S_BAND, heater::HEATER_4_CAMERA,
                           sBandTransceiverLimits);
  if (componentAboveUpperLimit and not syrlinksTooHotFlag) {
    triggerEvent(tcsCtrl::SYRLINKS_OVERHEATING, tempFloatToU32());
    syrlinksTooHotFlag = true;
  } else if (not componentAboveUpperLimit) {
    pcduSystemTooHotFlag = false;
  }
}
void ThermalController::ctrlPcduP60Board() {
  thermalComponent = PCDUP60_BOARD;
  sensors[0].first = deviceTemperatures.temp1P60dock.isValid();
  sensors[0].second = deviceTemperatures.temp1P60dock.value;
  sensors[1].first = deviceTemperatures.temp2P60dock.isValid();
  sensors[1].second = deviceTemperatures.temp2P60dock.value;
  numSensors = 2;
  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits);
  if (componentAboveUpperLimit and not pcduSystemTooHotFlag) {
    triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32());
    pcduSystemTooHotFlag = true;
  } else if (not componentAboveUpperLimit) {
    pcduSystemTooHotFlag = false;
  }
}

void ThermalController::ctrlPcduAcu() {
  thermalComponent = PCDUACU;
  heater::Switchers switchNr = heater::HEATER_3_PCDU_PDU;
  heater::Switchers redSwitchNr = heater::HEATER_2_ACS_BRD;

  if (chooseHeater(switchNr, redSwitchNr)) {
    bool sensorTempAvailable = true;

    if (deviceTemperatures.acu.value[0] != INVALID_TEMPERATURE) {
      sensorTemp = deviceTemperatures.acu.value[0];
    } else if (deviceTemperatures.acu.value[1] != INVALID_TEMPERATURE) {
      sensorTemp = deviceTemperatures.acu.value[1];
    } else if (deviceTemperatures.acu.value[2] != INVALID_TEMPERATURE) {
      sensorTemp = deviceTemperatures.acu.value[2];
    } else if (sensorTemperatures.sensor_acu.isValid()) {
      sensorTemp = sensorTemperatures.sensor_acu.value;
    } else {
      triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent);
      sensorTempAvailable = false;
    }
    if (sensorTempAvailable) {
      checkLimitsAndCtrlHeater(switchNr, redSwitchNr, pcduAcuLimits);
    }
  }
  if (componentAboveUpperLimit and not pcduSystemTooHotFlag) {
    triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32());
    pcduSystemTooHotFlag = true;
  } else if (not componentAboveUpperLimit) {
    pcduSystemTooHotFlag = false;
  }
}

void ThermalController::ctrlPcduPdu() {
  thermalComponent = PCDUPDU;
  sensors[0].first = deviceTemperatures.pdu1.isValid();
  sensors[0].second = deviceTemperatures.pdu1.value;
  sensors[1].first = deviceTemperatures.pdu2.isValid();
  sensors[1].second = deviceTemperatures.pdu2.value;
  sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs0.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits);
  if (componentAboveUpperLimit and not pcduSystemTooHotFlag) {
    triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32());
    pcduSystemTooHotFlag = true;
  } else if (not componentAboveUpperLimit) {
    pcduSystemTooHotFlag = false;
  }
}

void ThermalController::ctrlPlPcduBoard() {
  thermalComponent = PLPCDU_BOARD;
  sensors[0].first = sensorTemperatures.tmp1075PlPcdu0.isValid();
  sensors[0].second = sensorTemperatures.tmp1075PlPcdu0.value;
  sensors[1].first = sensorTemperatures.tmp1075PlPcdu1.isValid();
  sensors[1].second = sensorTemperatures.tmp1075PlPcdu1.value;
  sensors[2].first = deviceTemperatures.adcPayloadPcdu.isValid();
  sensors[2].second = deviceTemperatures.adcPayloadPcdu.value;
  sensors[3].first = sensorTemperatures.sensor_plpcdu_heatspreader.isValid();
  sensors[3].second = sensorTemperatures.sensor_plpcdu_heatspreader.value;
  numSensors = 4;
  ctrlComponentTemperature(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
}

void ThermalController::ctrlPlocMissionBoard() {
  thermalComponent = PLOCMISSION_BOARD;
  sensors[0].first = sensorTemperatures.sensor_ploc_heatspreader.isValid();
  sensors[0].second = sensorTemperatures.sensor_ploc_heatspreader.value;
  sensors[1].first = sensorTemperatures.sensor_ploc_missionboard.isValid();
  sensors[1].second = sensorTemperatures.sensor_ploc_missionboard.value;
  sensors[2].first = sensorTemperatures.sensor_dac_heatspreader.isValid();
  sensors[2].second = sensorTemperatures.sensor_dac_heatspreader.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
                           plocMissionBoardLimits);
  tooHotHandler(objects::PLOC_MPSOC_HANDLER, plocTooHotFlag);
}

void ThermalController::ctrlPlocProcessingBoard() {
  thermalComponent = PLOCPROCESSING_BOARD;
  sensors[0].first = sensorTemperatures.sensor_ploc_missionboard.isValid();
  sensors[0].second = sensorTemperatures.sensor_ploc_missionboard.value;
  sensors[1].first = sensorTemperatures.sensor_ploc_heatspreader.isValid();
  sensors[1].second = sensorTemperatures.sensor_ploc_heatspreader.value;
  sensors[2].first = sensorTemperatures.sensor_dac_heatspreader.isValid();
  sensors[2].second = sensorTemperatures.sensor_dac_heatspreader.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
                           plocProcessingBoardLimits);
  tooHotHandler(objects::PLOC_MPSOC_HANDLER, plocTooHotFlag);
}

void ThermalController::ctrlDac() {
  thermalComponent = DAC;
  sensors[0].first = sensorTemperatures.sensor_dac_heatspreader.isValid();
  sensors[0].second = sensorTemperatures.sensor_dac_heatspreader.value;
  sensors[1].first = sensorTemperatures.sensor_ploc_missionboard.isValid();
  sensors[1].second = sensorTemperatures.sensor_ploc_missionboard.value;
  sensors[2].first = sensorTemperatures.sensor_ploc_heatspreader.isValid();
  sensors[2].second = sensorTemperatures.sensor_ploc_heatspreader.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, dacLimits);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
}

void ThermalController::ctrlCameraBody() {
  thermalComponent = CAMERA;
  sensors[0].first = sensorTemperatures.sensor_4k_camera.isValid();
  sensors[0].second = sensorTemperatures.sensor_4k_camera.value;
  sensors[1].first = sensorTemperatures.sensor_dro.isValid();
  sensors[1].second = sensorTemperatures.sensor_dro.value;
  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
  sensors[2].second = sensorTemperatures.sensor_mpa.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_4_CAMERA, heater::HEATER_6_DRO, cameraLimits);
  if (componentAboveUpperLimit and not camTooHotOneShotFlag) {
    triggerEvent(tcsCtrl::CAMERA_OVERHEATING, tempFloatToU32());
    CommandMessage msg;
    HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HealthState::FAULTY);
    ReturnValue_t result = commandQueue->sendMessage(camId, &msg);
    if (result != returnvalue::OK) {
      sif::error << "ThermalController::ctrlCameraBody(): Sending health message failed"
                 << std::endl;
    }
    camTooHotOneShotFlag = true;
  } else if (not componentAboveUpperLimit) {
    camTooHotOneShotFlag = false;
  }
}

void ThermalController::ctrlDro() {
  thermalComponent = DRO;
  sensors[0].first = sensorTemperatures.sensor_dro.isValid();
  sensors[0].second = sensorTemperatures.sensor_dro.value;
  sensors[1].first = sensorTemperatures.sensor_4k_camera.isValid();
  sensors[1].second = sensorTemperatures.sensor_4k_camera.value;
  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
  sensors[2].second = sensorTemperatures.sensor_mpa.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, droLimits);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
}

void ThermalController::ctrlX8() {
  thermalComponent = X8;
  sensors[0].first = sensorTemperatures.sensor_x8.isValid();
  sensors[0].second = sensorTemperatures.sensor_x8.value;
  sensors[1].first = sensorTemperatures.sensor_hpa.isValid();
  sensors[1].second = sensorTemperatures.sensor_hpa.value;
  sensors[2].first = sensorTemperatures.sensor_tx_modul.isValid();
  sensors[2].second = sensorTemperatures.sensor_tx_modul.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, x8Limits);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
}

void ThermalController::ctrlTx() {
  thermalComponent = TX;
  sensors[0].first = sensorTemperatures.sensor_tx_modul.isValid();
  sensors[0].second = sensorTemperatures.sensor_tx_modul.value;
  sensors[1].first = sensorTemperatures.sensor_x8.isValid();
  sensors[1].second = sensorTemperatures.sensor_x8.value;
  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
  sensors[2].second = sensorTemperatures.sensor_mpa.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, txLimits);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
}

void ThermalController::ctrlMpa() {
  thermalComponent = MPA;
  sensors[0].first = sensorTemperatures.sensor_mpa.isValid();
  sensors[0].second = sensorTemperatures.sensor_mpa.value;
  sensors[1].first = sensorTemperatures.sensor_hpa.isValid();
  sensors[1].second = sensorTemperatures.sensor_hpa.value;
  sensors[2].first = sensorTemperatures.sensor_tx_modul.isValid();
  sensors[2].second = sensorTemperatures.sensor_tx_modul.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, mpaLimits);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
}

void ThermalController::ctrlHpa() {
  thermalComponent = HPA;
  sensors[0].first = sensorTemperatures.sensor_hpa.isValid();
  sensors[0].second = sensorTemperatures.sensor_hpa.value;
  sensors[1].first = sensorTemperatures.sensor_x8.isValid();
  sensors[1].second = sensorTemperatures.sensor_x8.value;
  sensors[2].first = sensorTemperatures.sensor_mpa.isValid();
  sensors[2].second = sensorTemperatures.sensor_mpa.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_4_CAMERA, hpaLimits);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
}

void ThermalController::ctrlScexBoard() {
  thermalComponent = SCEX_BOARD;
  sensors[0].first = sensorTemperatures.sensor_scex.isValid();
  sensors[0].second = sensorTemperatures.sensor_scex.value;
  sensors[1].first = sensorTemperatures.sensor_x8.isValid();
  sensors[1].second = sensorTemperatures.sensor_x8.value;
  sensors[2].first = sensorTemperatures.sensor_hpa.isValid();
  sensors[2].second = sensorTemperatures.sensor_hpa.value;
  numSensors = 3;
  ctrlComponentTemperature(heater::HEATER_6_DRO, heater::HEATER_5_STR, scexBoardLimits);
  tooHotHandler(objects::SCEX, scexTooHotFlag);
}

void ThermalController::performThermalModuleCtrl() {
  ctrlAcsBoard();
  ctrlMgt();
  ctrlRw();
  ctrlStr();
  ctrlIfBoard();
  ctrlTcsBoard();
  ctrlObc();
  ctrlObcIfBoard();
  ctrlSBandTransceiver();
  ctrlPcduP60Board();
  ctrlPcduAcu();
  ctrlPcduPdu();
  ctrlPlPcduBoard();
  ctrlPlocMissionBoard();
  ctrlPlocProcessingBoard();
  ctrlDac();
  ctrlCameraBody();
  ctrlDro();
  ctrlX8();
  ctrlHpa();
  ctrlTx();
  ctrlMpa();
  ctrlScexBoard();
  heaterTransitionControl();
}
void ThermalController::ctrlComponentTemperature(heater::Switchers switchNr,
                                                 heater::Switchers redSwitchNr,
                                                 TempLimits& tempLimit) {
  if (selectAndReadSensorTemp()) {
    if (chooseHeater(switchNr, redSwitchNr)) {
      checkLimitsAndCtrlHeater(switchNr, redSwitchNr, tempLimit);
    }
  } else {
    if (chooseHeater(switchNr,
                     redSwitchNr)) {  // TODO: muss der Heater dann wirklich abgeschalten werden?
      if (heaterHandler.checkSwitchState(switchNr)) {
        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
        sif::info << "ThermalController::checkLimitsAndCtrlHeater: Heater"
                  << static_cast<int>(thermalComponent) << " OFF"
                  << std::endl;  // TODO: printouts löschen
      }
    }
  }
  resetSensorsArray();
}
bool ThermalController::selectAndReadSensorTemp() {
  for (unsigned i = 0; i < numSensors; i++) {
    if (sensors[i].first and sensors[i].second != INVALID_TEMPERATURE) {
      sensorTemp = sensors[i].second;
      thermalStates[thermalComponent].errorCounter = 0;
      return true;
    }
  }

  thermalStates[thermalComponent].errorCounter++;
  if (thermalComponent != RW and thermalComponent != ACS_BOARD) {
    if (thermalStates[thermalComponent].errorCounter <= 3) {
      triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent);
    }
  } else {
    if (thermalStates[thermalComponent].errorCounter <= 8) {
      triggerEvent(tcsCtrl::NO_VALID_SENSOR_TEMPERATURE, thermalComponent);
    }
  }

  return false;
}
bool ThermalController::chooseHeater(heater::Switchers& switchNr, heater::Switchers redSwitchNr) {
  bool heaterAvailable = true;

  if (heaterHandler.getHealth(switchNr) != HasHealthIF::HEALTHY) {
    if (heaterHandler.getHealth(redSwitchNr) == HasHealthIF::HEALTHY) {
      switchNr = redSwitchNr;
      redSwitchNrInUse = true;
    } else {
      heaterAvailable = false;
      triggerEvent(tcsCtrl::NO_HEALTHY_HEATER_AVAILABLE, switchNr, redSwitchNr);
    }
  } else {
    redSwitchNrInUse = false;
  }
  return heaterAvailable;
}
void ThermalController::checkLimitsAndCtrlHeater(heater::Switchers switchNr,
                                                 heater::Switchers redSwitchNr,
                                                 struct TempLimits& tempLimit) {
  componentAboveCutOffLimit = false;
  componentAboveUpperLimit = false;
  // if Heater off
  if (not heaterStates[switchNr].switchTransition) {
    if (not heaterHandler.checkSwitchState(switchNr)) {
      // TODO: check NOP limit and maybe trigger fdir
      if (sensorTemp < tempLimit.opLowerLimit) {
        heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::ON);
        sif::info << "ThermalController::checkLimitsAndCtrlHeater: Heater "
                  << static_cast<int>(thermalComponent) << " ON" << std::endl;
        heaterStates[switchNr].switchTransition = true;
        thermalStates[thermalComponent].heating = true;
      } else {
        thermalStates[thermalComponent].heating = false;
      }

      // if Heater on
    } else if (heaterHandler.checkSwitchState(switchNr)) {
      if (thermalStates[thermalComponent].heating) {
        if (sensorTemp >= tempLimit.opLowerLimit + TEMP_OFFSET) {
          heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
          sif::info << "ThermalController::checkLimitsAndCtrlHeater: Heater "
                    << static_cast<int>(thermalComponent) << " OFF" << std::endl;
          heaterStates[switchNr].switchTransition = true;
          thermalStates[thermalComponent].heating = false;
        }
      } else {
        auto tempTooHighHandler = [&](const char* whatLimit) {
          heaterHandler.switchHeater(switchNr, HeaterHandler::SwitchState::OFF);
          sif::info << "ThermalController::checkLimitsAndCtrlHeater: Reached " << whatLimit
                    << ": Heater " << static_cast<int>(thermalComponent) << " OFF" << std::endl;
          heaterStates[switchNr].switchTransition = true;
          if (heaterHandler.checkSwitchState(redSwitchNr)) {
            heaterHandler.switchHeater(redSwitchNr, HeaterHandler::SwitchState::OFF);
            heaterStates[redSwitchNr].switchTransition = true;
          }
        };
        if (sensorTemp >= tempLimit.nopUpperLimit) {
          componentAboveUpperLimit = true;
          tempTooHighHandler("NOP-Limit");
          overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH;
        } else if (sensorTemp >= tempLimit.opUpperLimit) {
          componentAboveUpperLimit = true;
          tempTooHighHandler("OP-Limit");
          overHeatEventToTrigger = ThermalComponentIF::COMPONENT_TEMP_HIGH;
        }
        if (sensorTemp >= tempLimit.cutOffLimit) {
          componentAboveCutOffLimit = true;
          tempTooHighHandler("CutOff-Limit");
        }
      }
    }
  }
}
void ThermalController::resetSensorsArray() {
  for (auto& validValuePair : sensors) {
    validValuePair.first = false;
    validValuePair.second = INVALID_TEMPERATURE;
  }
  thermalComponent = NONE;
}
void ThermalController::heaterTransitionControl() {
  // TODO: Test
  for (unsigned i = 0; i < 7; i++) {
    if (heaterStates[i].switchTransition) {
      if (heaterStates[i].heaterSwitchControlErrorCounter > 3) {
        heaterStates[i].switchTransition = false;
        heaterStates[i].heaterSwitchControlErrorCounter = 0;
      }
      heaterStates[i].heaterSwitchControlErrorCounter++;
    }
  }
}
uint32_t ThermalController::tempFloatToU32() const {
  auto sensorTempAsFloat = static_cast<float>(sensorTemp);
  uint32_t tempRaw = 0;
  size_t dummyLen = 0;
  SerializeAdapter::serialize(&sensorTempAsFloat, reinterpret_cast<uint8_t*>(&tempRaw), &dummyLen,
                              sizeof(tempRaw), SerializeIF::Endianness::NETWORK);
  return tempRaw;
}
void ThermalController::tooHotHandler(object_id_t object, bool& oneShotFlag) {
  if (componentAboveUpperLimit and not oneShotFlag) {
    EventManagerIF::triggerEvent(object, overHeatEventToTrigger, tempFloatToU32());
    oneShotFlag = true;
  } else if (not componentAboveUpperLimit) {
    oneShotFlag = false;
  }
}