#include "ObjectFactory.h"

#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/subsystem/Subsystem.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>
#include <fsfw_hal/common/gpio/GpioIF.h>
#include <fsfw_hal/common/gpio/gpioDefinitions.h>
#include <fsfw_hal/linux/serial/SerialCookie.h>
#include <fsfw_hal/linux/spi/SpiComIF.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <linux/callbacks/gpioCallbacks.h>
#include <linux/devices/Max31865RtdLowlevelHandler.h>
#include <mission/controller/AcsController.h>
#include <mission/devices/Max31865EiveHandler.h>
#include <mission/devices/ScexDeviceHandler.h>
#include <mission/devices/SusHandler.h>
#include <mission/system/fdir/RtdFdir.h>
#include <mission/system/fdir/SusFdir.h>
#include <mission/system/objects/SusAssembly.h>
#include <mission/system/objects/TcsBoardAssembly.h>

#include "OBSWConfig.h"
#include "devConf.h"
#include "devices/addresses.h"
#include "devices/gpioIds.h"
#include "eive/definitions.h"
#include "mission/system/tree/acsModeTree.h"
#include "mission/system/tree/payloadModeTree.h"
#include "mission/system/tree/tcsModeTree.h"

void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiComIF,
                                              PowerSwitchIF* pwrSwitcher, std::string spiDev,
                                              bool swap0And6) {
  using namespace gpio;
  GpioCookie* gpioCookieSus = new GpioCookie();
  GpioCallback* susgpio = nullptr;

  susgpio = new GpioCallback("Chip select SUS 0", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_0, susgpio);
  susgpio = new GpioCallback("Chip select SUS 1", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_1, susgpio);
  susgpio = new GpioCallback("Chip select SUS 2", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_2, susgpio);
  susgpio = new GpioCallback("Chip select SUS 3", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_3, susgpio);
  susgpio = new GpioCallback("Chip select SUS 4", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_4, susgpio);
  susgpio = new GpioCallback("Chip select SUS 5", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_5, susgpio);
  susgpio = new GpioCallback("Chip select SUS 6", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_6, susgpio);
  susgpio = new GpioCallback("Chip select SUS 7", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_7, susgpio);
  susgpio = new GpioCallback("Chip select SUS 8", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_8, susgpio);
  susgpio = new GpioCallback("Chip select SUS 9", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_9, susgpio);
  susgpio = new GpioCallback("Chip select SUS 10", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_10, susgpio);
  susgpio = new GpioCallback("Chip select SUS 11", Direction::OUT, Levels::HIGH,
                             &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  gpioCookieSus->addGpio(gpioIds::CS_SUS_11, susgpio);

  gpioChecker(gpioComIF->addGpios(gpioCookieSus), "Sun Sensors");

#if OBSW_ADD_SUN_SENSORS == 1
  SusFdir* fdir = nullptr;
  std::array<SusHandler*, 12> susHandlers = {};
  gpioId_t gpioId = gpioIds::CS_SUS_0;
  if (swap0And6) {
    gpioId = gpioIds::CS_SUS_6;
  }
  SpiCookie* spiCookie = new SpiCookie(addresses::SUS_0, gpioId, SUS::MAX_CMD_SIZE,
                                       spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[0] =
      new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
  susHandlers[0]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_1, gpioIds::CS_SUS_1, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[1] =
      new SusHandler(objects::SUS_1_N_LOC_XBYFZM_PT_XB, 1, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
  susHandlers[1]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_2, gpioIds::CS_SUS_2, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[2] =
      new SusHandler(objects::SUS_2_N_LOC_XFYBZB_PT_YB, 2, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
  susHandlers[2]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_3, gpioIds::CS_SUS_3, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[3] =
      new SusHandler(objects::SUS_3_N_LOC_XFYBZF_PT_YF, 3, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
  susHandlers[3]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_4, gpioIds::CS_SUS_4, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[4] =
      new SusHandler(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, 4, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
  susHandlers[4]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_5, gpioIds::CS_SUS_5, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[5] =
      new SusHandler(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, 5, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
  susHandlers[5]->setCustomFdir(fdir);

  gpioId = gpioIds::CS_SUS_6;
  if (swap0And6) {
    gpioId = gpioIds::CS_SUS_0;
  }
  spiCookie = new SpiCookie(addresses::SUS_6, gpioId, SUS::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE,
                            spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[6] =
      new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
  susHandlers[6]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_7, gpioIds::CS_SUS_7, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[7] =
      new SusHandler(objects::SUS_7_R_LOC_XBYBZM_PT_XB, 7, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
  susHandlers[7]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_8, gpioIds::CS_SUS_8, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[8] =
      new SusHandler(objects::SUS_8_R_LOC_XBYBZB_PT_YB, 8, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
  susHandlers[8]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_9, gpioIds::CS_SUS_9, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[9] =
      new SusHandler(objects::SUS_9_R_LOC_XBYBZB_PT_YF, 9, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
  susHandlers[9]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_10, gpioIds::CS_SUS_10, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[10] =
      new SusHandler(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, 10, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
  susHandlers[10]->setCustomFdir(fdir);

  spiCookie = new SpiCookie(addresses::SUS_11, gpioIds::CS_SUS_11, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[11] =
      new SusHandler(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, 11, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
  susHandlers[11]->setCustomFdir(fdir);

  std::array<object_id_t, 12> susIds = {
      objects::SUS_0_N_LOC_XFYFZM_PT_XF,  objects::SUS_1_N_LOC_XBYFZM_PT_XB,
      objects::SUS_2_N_LOC_XFYBZB_PT_YB,  objects::SUS_3_N_LOC_XFYBZF_PT_YF,
      objects::SUS_4_N_LOC_XMYFZF_PT_ZF,  objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
      objects::SUS_6_R_LOC_XFYBZM_PT_XF,  objects::SUS_7_R_LOC_XBYBZM_PT_XB,
      objects::SUS_8_R_LOC_XBYBZB_PT_YB,  objects::SUS_9_R_LOC_XBYBZB_PT_YF,
      objects::SUS_10_N_LOC_XMYBZF_PT_ZF, objects::SUS_11_R_LOC_XBYMZB_PT_ZB};
  SusAssHelper susAssHelper = SusAssHelper(susIds);
  auto susAss = new SusAssembly(objects::SUS_BOARD_ASS, pwrSwitcher, susAssHelper);
  for (auto& sus : susHandlers) {
    if (sus != nullptr) {
      ReturnValue_t result = sus->connectModeTreeParent(*susAss);
      if (result != returnvalue::OK) {
        sif::error << "Connecting SUS " << sus->getObjectId() << " to SUS assembly failed"
                   << std::endl;
      }
#if OBSW_TEST_SUS == 1
      sus->setStartUpImmediately();
      sus->setToGoToNormalMode(true);
#endif
#if OBSW_DEBUG_SUS == 1
      sus->enablePeriodicPrintout(true, 3);
#endif
    }
  }
  susAss->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
#endif /* OBSW_ADD_SUN_SENSORS == 1 */
}

void ObjectFactory::createRtdComponents(std::string spiDev, GpioIF* gpioComIF,
                                        PowerSwitchIF* pwrSwitcher, SpiComIF* comIF) {
  using namespace gpio;
  GpioCookie* rtdGpioCookie = new GpioCookie;

  GpioCallback* gpioRtdIc0 = new GpioCallback("Chip select RTD IC0", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_3, gpioRtdIc0);
  GpioCallback* gpioRtdIc1 = new GpioCallback("Chip select RTD IC1", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_4, gpioRtdIc1);
  GpioCallback* gpioRtdIc2 = new GpioCallback("Chip select RTD IC2", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_5, gpioRtdIc2);
  GpioCallback* gpioRtdIc3 = new GpioCallback("Chip select RTD IC3", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_6, gpioRtdIc3);
  GpioCallback* gpioRtdIc4 = new GpioCallback("Chip select RTD IC4", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_7, gpioRtdIc4);
  GpioCallback* gpioRtdIc5 = new GpioCallback("Chip select RTD IC5", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_8, gpioRtdIc5);
  GpioCallback* gpioRtdIc6 = new GpioCallback("Chip select RTD IC6", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_9, gpioRtdIc6);
  GpioCallback* gpioRtdIc7 = new GpioCallback("Chip select RTD IC7", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_10, gpioRtdIc7);
  GpioCallback* gpioRtdIc8 = new GpioCallback("Chip select RTD IC8", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_11, gpioRtdIc8);
  GpioCallback* gpioRtdIc9 = new GpioCallback("Chip select RTD IC9", Direction::OUT, Levels::HIGH,
                                              &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_12, gpioRtdIc9);
  GpioCallback* gpioRtdIc10 = new GpioCallback("Chip select RTD IC10", Direction::OUT, Levels::HIGH,
                                               &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_13, gpioRtdIc10);
  GpioCallback* gpioRtdIc11 = new GpioCallback("Chip select RTD IC11", Direction::OUT, Levels::HIGH,
                                               &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_14, gpioRtdIc11);
  GpioCallback* gpioRtdIc12 = new GpioCallback("Chip select RTD IC12", Direction::OUT, Levels::HIGH,
                                               &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_15, gpioRtdIc12);
  GpioCallback* gpioRtdIc13 = new GpioCallback("Chip select RTD IC13", Direction::OUT, Levels::HIGH,
                                               &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_16, gpioRtdIc13);
  GpioCallback* gpioRtdIc14 = new GpioCallback("Chip select RTD IC14", Direction::OUT, Levels::HIGH,
                                               &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_17, gpioRtdIc14);
  GpioCallback* gpioRtdIc15 = new GpioCallback("Chip select RTD IC15", Direction::OUT, Levels::HIGH,
                                               &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
  rtdGpioCookie->addGpio(gpioIds::RTD_IC_18, gpioRtdIc15);

  gpioChecker(gpioComIF->addGpios(rtdGpioCookie), "RTDs");

#if OBSW_ADD_RTD_DEVICES == 1
  using namespace EiveMax31855;
  std::array<std::pair<address_t, gpioId_t>, NUM_RTDS> cookieArgs = {{
      {addresses::RTD_IC_3, gpioIds::RTD_IC_3},
      {addresses::RTD_IC_4, gpioIds::RTD_IC_4},
      {addresses::RTD_IC_5, gpioIds::RTD_IC_5},
      {addresses::RTD_IC_6, gpioIds::RTD_IC_6},
      {addresses::RTD_IC_7, gpioIds::RTD_IC_7},
      {addresses::RTD_IC_8, gpioIds::RTD_IC_8},
      {addresses::RTD_IC_9, gpioIds::RTD_IC_9},
      {addresses::RTD_IC_10, gpioIds::RTD_IC_10},
      {addresses::RTD_IC_11, gpioIds::RTD_IC_11},
      {addresses::RTD_IC_12, gpioIds::RTD_IC_12},
      {addresses::RTD_IC_13, gpioIds::RTD_IC_13},
      {addresses::RTD_IC_14, gpioIds::RTD_IC_14},
      {addresses::RTD_IC_15, gpioIds::RTD_IC_15},
      {addresses::RTD_IC_16, gpioIds::RTD_IC_16},
      {addresses::RTD_IC_17, gpioIds::RTD_IC_17},
      {addresses::RTD_IC_18, gpioIds::RTD_IC_18},
  }};
  // HSPD: Heatspreader
  std::array<std::pair<object_id_t, std::string>, NUM_RTDS> rtdInfos = {{
      {objects::RTD_0_IC3_PLOC_HEATSPREADER, "RTD_0_PLOC_HSPD"},
      {objects::RTD_1_IC4_PLOC_MISSIONBOARD, "RTD_1_PLOC_MISSIONBRD"},
      {objects::RTD_2_IC5_4K_CAMERA, "RTD_2_4K_CAMERA"},
      {objects::RTD_3_IC6_DAC_HEATSPREADER, "RTD_3_DAC_HSPD"},
      {objects::RTD_4_IC7_STARTRACKER, "RTD_4_STARTRACKER"},
      {objects::RTD_5_IC8_RW1_MX_MY, "RTD_5_RW1_MX_MY"},
      {objects::RTD_6_IC9_DRO, "RTD_6_DRO"},
      {objects::RTD_7_IC10_SCEX, "RTD_7_SCEX"},
      {objects::RTD_8_IC11_X8, "RTD_8_X8"},
      {objects::RTD_9_IC12_HPA, "RTD_9_HPA"},
      {objects::RTD_10_IC13_PL_TX, "RTD_10_PL_TX,"},
      {objects::RTD_11_IC14_MPA, "RTD_11_MPA"},
      {objects::RTD_12_IC15_ACU, "RTD_12_ACU"},
      {objects::RTD_13_IC16_PLPCDU_HEATSPREADER, "RTD_13_PLPCDU_HSPD"},
      {objects::RTD_14_IC17_TCS_BOARD, "RTD_14_TCS_BOARD"},
      {objects::RTD_15_IC18_IMTQ, "RTD_15_IMTQ"},
  }};
  std::array<SpiCookie*, NUM_RTDS> rtdCookies = {};
  std::array<Max31865EiveHandler*, NUM_RTDS> rtds = {};
  RtdFdir* rtdFdir = nullptr;

  TcsBoardHelper helper(rtdInfos);
  TcsBoardAssembly* tcsBoardAss = new TcsBoardAssembly(
      objects::TCS_BOARD_ASS, pwrSwitcher, pcdu::Switches::PDU1_CH0_TCS_BOARD_3V3, helper);
  tcsBoardAss->connectModeTreeParent(satsystem::tcs::SUBSYSTEM);
  // Create special low level reader communication interface
  new Max31865RtdReader(objects::SPI_RTD_COM_IF, comIF, gpioComIF);
  for (uint8_t idx = 0; idx < NUM_RTDS; idx++) {
    rtdCookies[idx] = new SpiCookie(cookieArgs[idx].first, cookieArgs[idx].second,
                                    MAX31865::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
    rtdCookies[idx]->setMutexParams(MutexIF::TimeoutType::WAITING, spi::RTD_CS_TIMEOUT);
    Max31865ReaderCookie* rtdLowLevelCookie =
        new Max31865ReaderCookie(rtdInfos[idx].first, idx, rtdInfos[idx].second, rtdCookies[idx]);
    rtds[idx] =
        new Max31865EiveHandler(rtdInfos[idx].first, objects::SPI_RTD_COM_IF, rtdLowLevelCookie);
    rtds[idx]->setDeviceInfo(idx, rtdInfos[idx].second);
    ReturnValue_t result = rtds[idx]->connectModeTreeParent(*tcsBoardAss);
    if (result != returnvalue::OK) {
      sif::error << "Connecting RTD " << static_cast<int>(idx) << " to RTD Assembly failed"
                 << std::endl;
    }
    rtdFdir = new RtdFdir(rtdInfos[idx].first);
    rtds[idx]->setCustomFdir(rtdFdir);
#if OBSW_DEBUG_RTD == 1
    rtds[idx]->setDebugMode(true, 5);
#endif
#if OBSW_TEST_RTD == 1
    rtds[idx]->setInstantNormal(true);
    rtds[idx]->setStartUpImmediately();
#endif
  }

#endif  // OBSW_ADD_RTD_DEVICES == 1
}

void ObjectFactory::createScexComponents(std::string uartDev, PowerSwitchIF* pwrSwitcher,
                                         SdCardMountedIF& mountedIF, bool onImmediately,
                                         std::optional<power::Switch_t> switchId) {
  auto* cookie = new SerialCookie(objects::SCEX, uartDev, uart::SCEX_BAUD, 4096);
  cookie->setTwoStopBits();
  // cookie->setParityEven();
  auto scexUartReader = new ScexUartReader(objects::SCEX_UART_READER);
  auto scexHandler = new ScexDeviceHandler(objects::SCEX, *scexUartReader, cookie, mountedIF);
  if (onImmediately) {
    scexHandler->setStartUpImmediately();
  }
  if (switchId) {
    scexHandler->setPowerSwitcher(*pwrSwitcher, switchId.value());
  }
  scexHandler->connectModeTreeParent(satsystem::pl::SUBSYSTEM);
}

void ObjectFactory::createThermalController() {
  auto* tcsCtrl = new ThermalController(objects::THERMAL_CONTROLLER);
  tcsCtrl->connectModeTreeParent(satsystem::tcs::SUBSYSTEM);
}

AcsController* ObjectFactory::createAcsController(bool connectSubsystem) {
  auto acsCtrl = new AcsController(objects::ACS_CONTROLLER);
  if (connectSubsystem) {
    acsCtrl->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
  }
  return acsCtrl;
}

void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) {
  if (result != returnvalue::OK) {
    sif::error << "ObjectFactory: Adding GPIOs failed for " << output << std::endl;
  }
}

void ObjectFactory::addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler,
                                            PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) {
  cfdpFunnel.addDestination(ipCoreHandler, config::LIVE_TM);
  pusFunnel.addDestination(ipCoreHandler, config::LIVE_TM);
}