#include "PDU1Handler.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/devices/devicedefinitions/GomSpacePackets.h>

#include "OBSWConfig.h"

PDU1Handler::PDU1Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie)
    : GomspaceDeviceHandler(objectId, comIF, comCookie, PDU::MAX_CONFIGTABLE_ADDRESS,
                            PDU::MAX_HKTABLE_ADDRESS, PDU::HK_TABLE_REPLY_SIZE,
                            &pdu1HkTableDataset),
      pdu1HkTableDataset(this) {}

PDU1Handler::~PDU1Handler() {}

ReturnValue_t PDU1Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  *id = GOMSPACE::REQUEST_HK_TABLE;
  return buildCommandFromCommand(*id, NULL, 0);
}

void PDU1Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
  parseHkTableReply(packet);
  handleDeviceTM(&pdu1HkTableDataset, id, true);

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PDU1 == 1
  pdu1HkTableDataset.read();
  sif::info << "PDU1 TCS Board voltage: " << pdu1HkTableDataset.voltageOutTCSBoard3V3 << std::endl;
  sif::info << "PDU1 Syrlinks voltage: " << pdu1HkTableDataset.voltageOutSyrlinks << std::endl;
  sif::info << "PDU1 star tracker voltage: " << pdu1HkTableDataset.voltageOutStarTracker
            << std::endl;
  sif::info << "PDU1 MGT voltage: " << pdu1HkTableDataset.voltageOutMGT << std::endl;
  sif::info << "PDU1 SUS nominal voltage: " << pdu1HkTableDataset.voltageOutSUSNominal << std::endl;
  sif::info << "PDU1 solar cell experiment voltage: " << pdu1HkTableDataset.voltageOutSolarCellExp
            << std::endl;
  sif::info << "PDU1 PLOC voltage: " << pdu1HkTableDataset.voltageOutPLOC << std::endl;
  sif::info << "PDU1 ACS Side A voltage: " << pdu1HkTableDataset.voltageOutACSBoardSideA
            << std::endl;
  sif::info << "PDU1 channel 8 voltage: " << pdu1HkTableDataset.voltageOutACSBoardSideA
            << std::endl;

  sif::info << "PDU1 TCS Board current: " << pdu1HkTableDataset.currentOutTCSBoard3V3 << std::endl;
  sif::info << "PDU1 Syrlinks current: " << pdu1HkTableDataset.currentOutSyrlinks << std::endl;
  sif::info << "PDU1 star tracker current: " << pdu1HkTableDataset.currentOutStarTracker
            << std::endl;
  sif::info << "PDU1 MGT current: " << pdu1HkTableDataset.currentOutMGT << std::endl;
  sif::info << "PDU1 SUS nominal current: " << pdu1HkTableDataset.currentOutSUSNominal << std::endl;
  sif::info << "PDU1 solar cell experiment current: " << pdu1HkTableDataset.currentOutSolarCellExp
            << std::endl;
  sif::info << "PDU1 PLOC current: " << pdu1HkTableDataset.currentOutPLOC << std::endl;
  sif::info << "PDU1 ACS Side A current: " << pdu1HkTableDataset.currentOutACSBoardSideA
            << std::endl;
  sif::info << "PDU1 channel 8 current: " << pdu1HkTableDataset.currentOutChannel8 << std::endl;
  printOutputSwitchStates();
  sif::info << "PDU1 battery mode: " << static_cast<unsigned int>(pdu1HkTableDataset.battMode.value)
            << std::endl;
  sif::info << "PDU1 VCC: " << pdu1HkTableDataset.vcc << " mV" << std::endl;
  float vbat = pdu1HkTableDataset.vbat.value * 0.001;
  sif::info << "PDU1 VBAT: " << vbat << "V" << std::endl;
  float temperatureC = pdu1HkTableDataset.temperature.value * 0.1;
  sif::info << "PDU1 Temperature: " << temperatureC << " °C" << std::endl;
  sif::info << "PDU1 csp1 watchdog pings before reboot: "
            << static_cast<unsigned int>(pdu1HkTableDataset.csp1WatchdogPingsLeft.value)
            << std::endl;
  sif::info << "PDU1 csp2 watchdog pings before reboot: "
            << static_cast<unsigned int>(pdu1HkTableDataset.csp2WatchdogPingsLeft.value)
            << std::endl;
  pdu1HkTableDataset.commit();
#endif
}

void PDU1Handler::assignChannelHookFunction(GOMSPACE::ChannelSwitchHook hook, void *args) {
  this->channelSwitchHook = hook;
  this->hookArgs = args;
}

ReturnValue_t PDU1Handler::setParamCallback(SetParamMessageUnpacker &unpacker,
                                            bool afterExecution) {
  using namespace PDU1;
  GOMSPACE::Pdu pdu = GOMSPACE::Pdu::PDU1;
  if (not afterExecution) {
    return HasReturnvaluesIF::RETURN_OK;
  }
  if (channelSwitchHook != nullptr and unpacker.getParameterSize() == 1) {
    switch (unpacker.getAddress()) {
      case (CONFIG_ADDRESS_OUT_EN_TCS_BOARD_3V3): {
        channelSwitchHook(pdu, 0, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_SYRLINKS): {
        channelSwitchHook(pdu, 1, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_STAR_TRACKER): {
        channelSwitchHook(pdu, 2, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_MGT): {
        channelSwitchHook(pdu, 3, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_SUS_NOMINAL): {
        channelSwitchHook(pdu, 4, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_SOLAR_CELL_EXP): {
        channelSwitchHook(pdu, 5, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_PLOC): {
        channelSwitchHook(pdu, 6, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_ACS_BOARD_SIDE_A): {
        channelSwitchHook(pdu, 7, unpacker.getParameter()[0], hookArgs);
        break;
      }
      case (CONFIG_ADDRESS_OUT_EN_CHANNEL8): {
        channelSwitchHook(pdu, 8, unpacker.getParameter()[0], hookArgs);
        break;
      }
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

void PDU1Handler::parseHkTableReply(const uint8_t *packet) {
  uint16_t dataOffset = 0;
  PoolReadGuard pg(&pdu1HkTableDataset);
  ReturnValue_t readResult = pg.getReadResult();
  if (readResult != HasReturnvaluesIF::RETURN_OK) {
    sif::warning << "Reading PDU1 HK table failed!" << std::endl;
    return;
  }
  /* Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table
   * address. */
  dataOffset += 12;
  pdu1HkTableDataset.currentOutTCSBoard3V3 =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutSyrlinks = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutStarTracker =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutMGT = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutSUSNominal =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutSolarCellExp =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutPLOC = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutACSBoardSideA =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.currentOutChannel8 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  pdu1HkTableDataset.voltageOutTCSBoard3V3 =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutSyrlinks = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutStarTracker =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutMGT = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutSUSNominal =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutSolarCellExp =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutPLOC = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutACSBoardSideA =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.voltageOutChannel8 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  pdu1HkTableDataset.vcc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.vbat = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.temperature = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  pdu1HkTableDataset.converterEnable1 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.converterEnable2 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.converterEnable3 = *(packet + dataOffset);
  dataOffset += 3;

  pdu1HkTableDataset.outEnabledTCSBoard3V3 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledSyrlinks = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledStarTracker = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledMGT = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledSUSNominal = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledSolarCellExp = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledPLOC = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledAcsBoardSideA = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.outEnabledChannel8 = *(packet + dataOffset);
  dataOffset += 3;

  pdu1HkTableDataset.bootcause = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                                 *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.bootcount = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                                 *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
                              *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.resetcause = *(packet + dataOffset + 1) << 8 | *(packet + dataOffset);
  dataOffset += 4;
  pdu1HkTableDataset.battMode = *(packet + dataOffset);
  /* +10 because here begins the second gomspace csp packet */
  dataOffset += 3 + 10;

  pdu1HkTableDataset.latchupsTcsBoard3V3 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsSyrlinks = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsStarTracker = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsMgt = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsSusNominal = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsSolarCellExp =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsPloc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsAcsBoardSideA =
      *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;
  pdu1HkTableDataset.latchupsChannel8 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
  dataOffset += 4;

  pdu1HkTableDataset.device0 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device1 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device2 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device3 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device4 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device5 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device6 = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device7 = *(packet + dataOffset);
  dataOffset += 3;

  pdu1HkTableDataset.device0Status = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device1Status = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device2Status = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device3Status = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device4Status = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device5Status = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device6Status = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.device7Status = *(packet + dataOffset);
  dataOffset += 3;

  pdu1HkTableDataset.gndWdtReboots = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.i2cWdtReboots = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.canWdtReboots = *(packet + dataOffset) << 24 |
                                     *(packet + dataOffset + 1) << 16 |
                                     *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.csp1WdtReboots = *(packet + dataOffset) << 24 |
                                      *(packet + dataOffset + 1) << 16 |
                                      *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.csp2WdtReboots = *(packet + dataOffset) << 24 |
                                      *(packet + dataOffset + 1) << 16 |
                                      *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.groundWatchdogSecondsLeft =
      *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
      *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.i2cWatchdogSecondsLeft =
      *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
      *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.canWatchdogSecondsLeft =
      *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
      *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
  dataOffset += 6;
  pdu1HkTableDataset.csp1WatchdogPingsLeft = *(packet + dataOffset);
  dataOffset += 3;
  pdu1HkTableDataset.csp2WatchdogPingsLeft = *(packet + dataOffset);

  pdu1HkTableDataset.setChanged(true);
  if (not pdu1HkTableDataset.isValid()) {
    pdu1HkTableDataset.setValidity(true, true);
  }
}

ReturnValue_t PDU1Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                   LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_TCS_BOARD_3V3,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_SYRLINKS, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_STAR_TRACKER,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_MGT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_SUS_NOMINAL,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_SOLAR_CELL_EXP,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_PLOC, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_ACS_BOARD_SIDE_A,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CURRENT_OUT_CHANNEL8, new PoolEntry<int16_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_TCS_BOARD_3V3,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_SYRLINKS, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_STAR_TRACKER,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_MGT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_SUS_NOMINAL,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_SOLAR_CELL_EXP,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_PLOC, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_ACS_BOARD_SIDE_A,
                           new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VOLTAGE_OUT_CHANNEL8, new PoolEntry<int16_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_VCC, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_VBAT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CONV_EN_1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CONV_EN_2, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_CONV_EN_3, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_TCS_BOARD_3V3, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_SYRLINKS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_STAR_TRACKER, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_MGT, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_SUS_NOMINAL, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_SOLAR_CELL_EXP,
                           new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_PLOC, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_ACS_BOARD_SIDE_A,
                           new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_OUT_EN_CHANNEL8, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_BOOTCAUSE, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_BOOTCNT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_UPTIME, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_RESETCAUSE, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_BATT_MODE, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_TCS_BOARD_3V3,
                           new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_SYRLINKS, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_STAR_TRACKER,
                           new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_MGT, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_SUS_NOMINAL, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_SOLAR_CELL_EXP,
                           new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_PLOC, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_ACS_BOARD_SIDE_A,
                           new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_LATCHUP_CHANNEL8, new PoolEntry<uint16_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_0, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_2, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_3, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_4, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_5, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_6, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_7, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_0_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_1_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_2_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_3_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_4_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_5_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_6_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_DEVICE_7_STATUS, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CNT_GND, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CNT_I2C, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CNT_CAN, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CNT_CSP1, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CNT_CSP2, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_GND_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_I2C_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CAN_LEFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CSP_LEFT1, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(P60System::pool::PDU1_WDT_CSP_LEFT2, new PoolEntry<uint8_t>({0}));
#if OBSW_ENABLE_PERIODIC_HK == 1
  poolManager.subscribeForPeriodicPacket(pdu1HkTableDataset.getSid(), false, 0.4, true);
#endif
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PDU1Handler::printStatus(DeviceCommandId_t cmd) {
  ReturnValue_t result = RETURN_OK;
  switch (cmd) {
    case (GOMSPACE::PRINT_SWITCH_V_I): {
      PoolReadGuard pg(&pdu1HkTableDataset);
      result = pg.getReadResult();
      if (result != HasReturnvaluesIF::RETURN_OK) {
        break;
      }
      printHkTableSwitchVI();
      break;
    }
    case (GOMSPACE::PRINT_LATCHUPS): {
      PoolReadGuard pg(&pdu1HkTableDataset);
      result = pg.getReadResult();
      if (result != HasReturnvaluesIF::RETURN_OK) {
        break;
      }
      printHkTableLatchups();
      break;
    }
    default: {
      return HasReturnvaluesIF::RETURN_FAILED;
    }
  }
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::warning << "Reading PDU1 HK table failed!" << std::endl;
  }
  return result;
}

void PDU1Handler::printHkTableSwitchVI() {
  sif::info << "PDU1 Info: " << std::endl;
  sif::info << "Boot Cause: " << pdu1HkTableDataset.bootcause << " | Boot Count: " << std::setw(4)
            << std::right << pdu1HkTableDataset.bootcount << std::endl;
  sif::info << "Reset Cause: " << pdu1HkTableDataset.resetcause
            << " | Battery Mode: " << static_cast<int>(pdu1HkTableDataset.battMode.value)
            << std::endl;
  sif::info << "SwitchState, Currents [mA], Voltages [mV]:" << std::endl;
  sif::info << std::setw(30) << std::left << "TCS Board" << std::dec << "| "
            << unsigned(pdu1HkTableDataset.outEnabledTCSBoard3V3.value) << ", " << std::setw(4)
            << std::right << pdu1HkTableDataset.currentOutTCSBoard3V3.value << ", " << std::setw(4)
            << pdu1HkTableDataset.voltageOutTCSBoard3V3.value << std::endl;
  sif::info << std::setw(30) << std::left << "Syrlinks" << std::dec << "| "
            << unsigned(pdu1HkTableDataset.outEnabledSyrlinks.value) << ", " << std::setw(4)
            << std::right << pdu1HkTableDataset.currentOutSyrlinks.value << ", " << std::setw(4)
            << pdu1HkTableDataset.voltageOutSyrlinks.value << std::endl;
  sif::info << std::setw(30) << std::left << "Star Tracker" << std::dec << "| "
            << static_cast<unsigned int>(pdu1HkTableDataset.outEnabledStarTracker.value) << ", "
            << std::setw(4) << std::right << pdu1HkTableDataset.currentOutStarTracker.value << ", "
            << std::setw(4) << pdu1HkTableDataset.voltageOutStarTracker.value << std::endl;
  sif::info << std::setw(30) << std::left << "MGT" << std::dec << "| "
            << static_cast<unsigned int>(pdu1HkTableDataset.outEnabledMGT.value) << ", "
            << std::setw(4) << std::right << pdu1HkTableDataset.currentOutMGT.value << ", "
            << std::setw(4) << pdu1HkTableDataset.voltageOutMGT.value << std::endl;
  sif::info << std::setw(30) << std::left << "SuS nominal" << std::dec << "| "
            << static_cast<unsigned int>(pdu1HkTableDataset.outEnabledSUSNominal.value) << ", "
            << std::setw(4) << std::right << pdu1HkTableDataset.currentOutSUSNominal.value << ", "
            << std::setw(4) << pdu1HkTableDataset.voltageOutSUSNominal.value << std::endl;
  sif::info << std::setw(30) << std::left << "Solar Cell Experiment" << std::dec << "| "
            << static_cast<unsigned int>(pdu1HkTableDataset.outEnabledSolarCellExp.value) << ", "
            << std::setw(4) << std::right << pdu1HkTableDataset.currentOutSolarCellExp.value << ", "
            << std::setw(4) << pdu1HkTableDataset.voltageOutSolarCellExp.value << std::endl;
  sif::info << std::setw(30) << std::left << "PLOC" << std::dec << "| "
            << static_cast<unsigned int>(pdu1HkTableDataset.outEnabledPLOC.value) << ", "
            << std::setw(4) << std::right << pdu1HkTableDataset.currentOutPLOC.value << ", "
            << std::setw(4) << pdu1HkTableDataset.voltageOutPLOC.value << std::endl;
  sif::info << std::setw(30) << std::left << "ACS Side A" << std::dec << "| "
            << static_cast<unsigned int>(pdu1HkTableDataset.outEnabledAcsBoardSideA.value) << ", "
            << std::setw(4) << std::right << pdu1HkTableDataset.currentOutACSBoardSideA.value
            << ", " << std::setw(4) << pdu1HkTableDataset.voltageOutACSBoardSideA.value
            << std::endl;
  sif::info << std::setw(30) << std::left << "Channel 8" << std::dec << "| "
            << static_cast<unsigned int>(pdu1HkTableDataset.outEnabledChannel8.value) << ", "
            << std::setw(4) << std::right << pdu1HkTableDataset.currentOutChannel8.value << ", "
            << std::setw(4) << pdu1HkTableDataset.voltageOutChannel8.value << std::right
            << std::endl;
}

void PDU1Handler::printHkTableLatchups() {
  sif::info << "PDU1 Latchup Information" << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "TCS Board" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsTcsBoard3V3 << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "Syrlinks" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsSyrlinks << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "Star Tracker" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsStarTracker << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "MGT" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsMgt << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "SuS Nominal" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsSusNominal << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "Solar Cell Experiment" << std::dec
            << "| " << std::setw(4) << std::right << pdu1HkTableDataset.latchupsSolarCellExp
            << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "PLOC" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsPloc << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "ACS A Side" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsAcsBoardSideA << std::endl;
  sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "Channel 8" << std::dec << "| "
            << std::setw(4) << std::right << pdu1HkTableDataset.latchupsChannel8 << std::endl;
}