#include "PayloadPcduHandler.h"

#include <fsfw/src/fsfw/datapool/PoolReadGuard.h>

#include "OBSWConfig.h"

#ifdef XIPHOS_Q7S
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <fsfw_hal/linux/spi/SpiComIF.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <fsfw_hal/linux/utility.h>
#include <sys/ioctl.h>
#endif

#include "devices/gpioIds.h"

PayloadPcduHandler::PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie,
                                       GpioIF* gpioIF, SdCardMountedIF* sdcMan,
                                       PowerSwitchIF* pwrSwitcher, power::Switch_t switchA,
                                       power::Switch_t switchB, bool periodicPrintout)
    : DeviceHandlerBase(objectId, comIF, cookie),
      adcSet(this),
      periodicPrintout(periodicPrintout),
      gpioIF(gpioIF),
      sdcMan(sdcMan),
      pwrStateMachine(switchA, switchB, pwrSwitcher) {}

void PayloadPcduHandler::doStartUp() {
  if ((state != States::PL_PCDU_OFF) and (state != States::ON_TRANS_SSR)) {
    // Config error
    sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
  }
  if (pwrStateMachine.getState() == power::States::IDLE) {
    pwrStateMachine.start(MODE_ON, pwrSubmode);
  }
  auto opCode = pwrStateMachine.fsm();
  if (opCode == power::OpCodes::TO_NOT_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
    pwrStateMachine.reset();
    quickTransitionAlreadyCalled = false;
    state = States::POWER_CHANNELS_ON;
    setMode(_MODE_TO_ON);
  }
}

void PayloadPcduHandler::doShutDown() {
  if (not quickTransitionAlreadyCalled) {
    quickTransitionBackToOff(false, false);
    quickTransitionAlreadyCalled = true;
  }
  if (pwrStateMachine.getState() == power::States::IDLE) {
    pwrStateMachine.start(MODE_OFF, 0);
  }
  auto opCode = pwrStateMachine.fsm();
  if (opCode == power::OpCodes::TO_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
    pwrStateMachine.reset();
    state = States::PL_PCDU_OFF;
    // No need to set mode _MODE_POWER_DOWN, power switching was already handled
    setMode(MODE_OFF);
  }
}

void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  if (mode == _MODE_TO_NORMAL) {
    stateMachineToNormal(modeFrom, subModeFrom);
    return;
  }
  DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
}

ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_t subModeFrom) {
  using namespace plpcdu;
  bool doFinish = true;
  if (((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == 1) {
    if (state == States::PL_PCDU_OFF) {
      sif::error << "PayloadPcduHandler::stateMachineToNormal: Unexpected state PL_PCDU_OFF"
                 << "detected" << std::endl;
      setMode(MODE_OFF);
      return returnvalue::FAILED;
    }
    if (state == States::POWER_CHANNELS_ON) {
#if OBSW_VERBOSE_LEVEL >= 1
      sif::info << "Switching on SSR VBAT0 & VBAT1 GPIOs" << std::endl;
#endif
      // Switch on relays here
      gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0);
      gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1);
      state = States::ON_TRANS_SSR;
      transitionOk = true;
      doFinish = false;
    }
    if (state == States::ON_TRANS_SSR) {
      // If necessary, check whether a certain amount of time has elapsed
      if (transitionOk) {
        transitionOk = false;
        state = States::ON_TRANS_ADC_CLOSE_ZERO;
        adcCountdown.setTimeout(50);
        adcCountdown.resetTimer();
        adcState = AdcStates::BOOT_DELAY;
        doFinish = false;
        // If the values are not close to zero, we should not allow transition
        monMode = MonitoringMode::CLOSE_TO_ZERO;
      }
    }
    if (state == States::ON_TRANS_ADC_CLOSE_ZERO) {
      if (adcState == AdcStates::BOOT_DELAY) {
        doFinish = false;
        if (adcCountdown.hasTimedOut()) {
          adcState = AdcStates::SEND_SETUP;
          adcCmdExecuted = false;
        }
      }
      if (adcState == AdcStates::SEND_SETUP) {
        if (adcCmdExecuted) {
          adcState = AdcStates::NORMAL;
          doFinish = true;
          adcCountdown.setTimeout(100);
          adcCountdown.resetTimer();
          adcCmdExecuted = false;
        }
      }
    }
  }

  auto switchHandler = [&](NormalSubmodeBits bit, gpioId_t id, std::string info) {
    if (((diffMask >> bit) & 1) == 1) {
      if (((submode >> bit) & 1) == 1) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::info << "Enabling PL PCDU " << info << " module" << std::endl;
#endif
        // Switch on DRO and start monitoring for negative voltages
        updateSwitchGpio(id, gpio::Levels::HIGH);
      } else {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::info << "Disabling PL PCDU " << info << " module" << std::endl;
#endif
        updateSwitchGpio(id, gpio::Levels::LOW);
      }
    }
  };

  switchHandler(DRO_ON, gpioIds::PLPCDU_ENB_DRO, "DRO");
  switchHandler(X8_ON, gpioIds::PLPCDU_ENB_X8, "X8");
  switchHandler(TX_ON, gpioIds::PLPCDU_ENB_TX, "TX");
  switchHandler(MPA_ON, gpioIds::PLPCDU_ENB_MPA, "MPA");
  switchHandler(HPA_ON, gpioIds::PLPCDU_ENB_HPA, "HPA");
  if (doFinish) {
    setMode(MODE_NORMAL, submode);
  }
  return returnvalue::OK;
}

ReturnValue_t PayloadPcduHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  switch (adcState) {
    case (AdcStates::SEND_SETUP): {
      *id = plpcdu::SETUP_CMD;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (AdcStates::NORMAL): {
      *id = plpcdu::READ_WITH_TEMP_EXT;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    default: {
      break;
    }
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t PayloadPcduHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  if (adcState == AdcStates::SEND_SETUP) {
    *id = plpcdu::SETUP_CMD;
    return buildCommandFromCommand(*id, nullptr, 0);
  }
  if (mode == _MODE_TO_NORMAL) {
    return buildNormalDeviceCommand(id);
  }
  return NOTHING_TO_SEND;
}

void PayloadPcduHandler::updateSwitchGpio(gpioId_t id, gpio::Levels level) {
  if (level == gpio::Levels::HIGH) {
    gpioIF->pullHigh(id);
  } else {
    gpioIF->pullLow(id);
  }
  adcCountdown.setTimeout(100);
  adcCountdown.resetTimer();
}

void PayloadPcduHandler::fillCommandAndReplyMap() {
  insertInCommandAndReplyMap(plpcdu::READ_CMD, 2, &adcSet);
  insertInCommandAndReplyMap(plpcdu::READ_TEMP_EXT, 1, &adcSet);
  insertInCommandAndReplyMap(plpcdu::READ_WITH_TEMP_EXT, 1, &adcSet);
  insertInCommandAndReplyMap(plpcdu::SETUP_CMD, 1);
}

ReturnValue_t PayloadPcduHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                          const uint8_t* commandData,
                                                          size_t commandDataLen) {
  switch (deviceCommand) {
    case (plpcdu::SETUP_CMD): {
      cmdBuf[0] = plpcdu::SETUP_BYTE;
      rawPacket = cmdBuf.data();
      rawPacketLen = 1;
      break;
    }
    case (plpcdu::READ_CMD): {
      max1227::prepareExternallyClockedRead0ToN(cmdBuf.data(), plpcdu::CHANNEL_N, rawPacketLen);
      rawPacket = cmdBuf.data();
      break;
    }
    case (plpcdu::READ_TEMP_EXT): {
      max1227::prepareExternallyClockedTemperatureRead(cmdBuf.data(), rawPacketLen);
      rawPacket = cmdBuf.data();
      break;
    }
    case (plpcdu::READ_WITH_TEMP_EXT): {
      size_t sz = 0;
      max1227::prepareExternallyClockedRead0ToN(cmdBuf.data(), plpcdu::CHANNEL_N, sz);
      max1227::prepareExternallyClockedTemperatureRead(cmdBuf.data() + sz, sz);
      rawPacketLen = sz;
      rawPacket = cmdBuf.data();
      break;
    }
    default: {
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t PayloadPcduHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                               DeviceCommandId_t* foundId, size_t* foundLen) {
  // SPI is full duplex
  *foundId = getPendingCommand();
  *foundLen = remainingSize;
  return returnvalue::OK;
}

ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                       const uint8_t* packet) {
  using namespace plpcdu;
  switch (id) {
    case (SETUP_CMD): {
      if (mode == _MODE_TO_NORMAL) {
        adcCmdExecuted = true;
      }
      break;
    }
    case (READ_TEMP_EXT): {
      uint8_t tempStartIdx = TEMP_REPLY_SIZE - 2;
      adcSet.tempC.value =
          max1227::getTemperature(packet[tempStartIdx] << 8 | packet[tempStartIdx + 1]);
      break;
    }
    case (READ_CMD): {
      PoolReadGuard pg(&adcSet);
      if (pg.getReadResult() != returnvalue::OK) {
        return pg.getReadResult();
      }
      handleExtConvRead(packet);
      checkAdcValues();
      adcSet.setValidity(true, true);
      handlePrintout();
      break;
    }
    case (READ_WITH_TEMP_EXT): {
      PoolReadGuard pg(&adcSet);
      if (pg.getReadResult() != returnvalue::OK) {
        return pg.getReadResult();
      }
      handleExtConvRead(packet);
      uint8_t tempStartIdx = ADC_REPLY_SIZE + TEMP_REPLY_SIZE - 2;
      adcSet.tempC.value =
          max1227::getTemperature(packet[tempStartIdx] << 8 | packet[tempStartIdx + 1]);
      checkAdcValues();
      adcSet.setValidity(true, true);
      handlePrintout();
      break;
    }
    default: {
      break;
    }
  }
  return returnvalue::OK;
}

uint32_t PayloadPcduHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
  // 20 minutes transition delay is allowed
  return 20 * 60 * 1000;
}

ReturnValue_t PayloadPcduHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                          LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::CHANNEL_VEC, &channelValues);
  localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::PROCESSED_VEC, &processedValues);
  localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::TEMP, &tempC);
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(adcSet.getSid(), false, 5.0));
  return returnvalue::OK;
}

void PayloadPcduHandler::setToGoToNormalModeImmediately(bool enable) {
  this->goToNormalMode = enable;
}

void PayloadPcduHandler::handleExtConvRead(const uint8_t* bufStart) {
  for (uint8_t idx = 0; idx < 12; idx++) {
    adcSet.channels[idx] = bufStart[idx * 2 + 1] << 8 | bufStart[idx * 2 + 2];
  }
}

void PayloadPcduHandler::handlePrintout() {
  using namespace plpcdu;
  if (periodicPrintout) {
    if (opDivider.checkAndIncrement()) {
      sif::info << "PL PCDU ADC hex [" << std::setfill('0') << std::hex;
      for (uint8_t idx = 0; idx < 12; idx++) {
        sif::info << std::setw(3) << adcSet.channels[idx];
        if (idx < 11) {
          sif::info << ",";
        }
      }
      sif::info << "] | T[C] " << std::dec << adcSet.tempC.value << std::endl;
      sif::info << "Neg V: " << adcSet.processed[U_NEG_V_FB] << std::endl;
      sif::info << "I HPA [mA]: " << adcSet.processed[I_HPA] << std::endl;
      sif::info << "U HPA [V]: " << adcSet.processed[U_HPA_DIV_6] << std::endl;
      sif::info << "I MPA [mA]: " << adcSet.processed[I_MPA] << std::endl;
      sif::info << "U MPA [V]: " << adcSet.processed[U_MPA_DIV_6] << std::endl;
      sif::info << "I TX [mA]: " << adcSet.processed[I_TX] << std::endl;
      sif::info << "U TX [V]: " << adcSet.processed[U_TX_DIV_6] << std::endl;
      sif::info << "I X8 [mA]: " << adcSet.processed[I_X8] << std::endl;
      sif::info << "U X8 [V]: " << adcSet.processed[U_X8_DIV_6] << std::endl;
      sif::info << "I DRO [mA]: " << adcSet.processed[I_DRO] << std::endl;
      sif::info << "U DRO [V]: " << adcSet.processed[U_DRO_DIV_6] << std::endl;
    }
  }
}

void PayloadPcduHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
  this->periodicPrintout = enable;
  opDivider.setDivider(divider);
}

void PayloadPcduHandler::quickTransitionBackToOff(bool startTransitionToOff, bool notifyFdir) {
  States currentState = state;
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_HPA);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_MPA);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_X8);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_DRO);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT0);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT1);
  state = States::PL_PCDU_OFF;
  adcState = AdcStates::OFF;
  if (startTransitionToOff) {
    startTransition(MODE_OFF, 0);
  }
  if (notifyFdir) {
    triggerEvent(TRANSITION_BACK_TO_OFF, static_cast<uint32_t>(currentState));
  }
}

void PayloadPcduHandler::checkAdcValues() {
  using namespace plpcdu;
  checkJsonFileInit();
  adcSet.processed[U_BAT_DIV_6] =
      static_cast<float>(adcSet.channels[0]) * VOLTAGE_DIV / MAX122X_BIT * MAX122X_VREF;
  adcSet.processed[U_NEG_V_FB] =
      V_POS - VOLTAGE_DIV_U_NEG *
                  (V_POS - static_cast<float>(adcSet.channels[1]) / MAX122X_BIT * MAX122X_VREF);
  adcSet.processed[I_HPA] = static_cast<float>(adcSet.channels[2]) * SCALE_CURRENT_HPA * 1000.0;
  adcSet.processed[U_HPA_DIV_6] = static_cast<float>(adcSet.channels[3]) * SCALE_VOLTAGE;
  adcSet.processed[I_MPA] = static_cast<float>(adcSet.channels[4]) * SCALE_CURRENT_MPA * 1000.0;
  adcSet.processed[U_MPA_DIV_6] = static_cast<float>(adcSet.channels[5]) * SCALE_VOLTAGE;
  adcSet.processed[I_TX] = static_cast<float>(adcSet.channels[6]) * SCALE_CURRENT_TX * 1000.0;
  adcSet.processed[U_TX_DIV_6] = static_cast<float>(adcSet.channels[7]) * SCALE_VOLTAGE;
  adcSet.processed[I_X8] = static_cast<float>(adcSet.channels[8]) * SCALE_CURRENT_X8 * 1000.0;
  adcSet.processed[U_X8_DIV_6] = static_cast<float>(adcSet.channels[9]) * SCALE_VOLTAGE;
  adcSet.processed[I_DRO] = static_cast<float>(adcSet.channels[10]) * SCALE_CURRENT_DRO * 1000.0;
  adcSet.processed[U_DRO_DIV_6] = static_cast<float>(adcSet.channels[11]) * SCALE_VOLTAGE;
  float lowerBound = 0.0;
  float upperBound = 0.0;
  bool adcTransition = false;
  adcTransition = state == States::ON_TRANS_DRO and adcCountdown.isBusy();
  // Now check against voltage and current limits, depending on state
  if (state >= States::ON_TRANS_DRO and not adcTransition) {
    if (ssrToDroInjectionRequested) {
      handleFailureInjection("SSR to DRO", NEG_V_OUT_OF_BOUNDS);
      ssrToDroInjectionRequested = false;
      return;
    }
    params.getValue(PARAM_KEY_MAP[NEG_V_LOWER_BOUND], lowerBound);
    params.getValue(PARAM_KEY_MAP[NEG_V_UPPER_BOUND], upperBound);
    if (not checkVoltage(adcSet.processed[U_NEG_V_FB], lowerBound, upperBound,
                         NEG_V_OUT_OF_BOUNDS)) {
      return;
    }
    params.getValue(PARAM_KEY_MAP[DRO_U_LOWER_BOUND], lowerBound);
    params.getValue(PARAM_KEY_MAP[DRO_U_UPPER_BOUND], upperBound);
    if (not checkVoltage(adcSet.processed[U_DRO_DIV_6], lowerBound, upperBound,
                         U_DRO_OUT_OF_BOUNDS)) {
      return;
    }
    params.getValue(PARAM_KEY_MAP[DRO_I_UPPER_BOUND], upperBound);
    if (not checkCurrent(adcSet.processed[I_DRO], upperBound, I_DRO_OUT_OF_BOUNDS)) {
#if OBSW_VERBOSE_LEVEL >= 1
      sif::warning << "Detected out of bounds current for DRO: " << adcSet.processed[I_DRO]
                   << ", Raw: " << adcSet.channels[I_DRO] << std::endl;
#endif
      return;
    }
  }
  adcTransition = state == States::ON_TRANS_X8 and adcCountdown.isBusy();
  if (state >= States::ON_TRANS_X8 and not adcTransition) {
    if (droToX8InjectionRequested) {
      handleFailureInjection("X8 to TX", U_X8_OUT_OF_BOUNDS);
      droToX8InjectionRequested = false;
      return;
    }
    params.getValue(PARAM_KEY_MAP[X8_U_LOWER_BOUND], lowerBound);
    params.getValue(PARAM_KEY_MAP[X8_U_UPPER_BOUND], upperBound);
    if (not checkVoltage(adcSet.processed[U_X8_DIV_6], lowerBound, upperBound,
                         U_X8_OUT_OF_BOUNDS)) {
      return;
    }
    params.getValue(PARAM_KEY_MAP[X8_I_UPPER_BOUND], upperBound);
    if (not checkCurrent(adcSet.processed[I_X8], upperBound, I_X8_OUT_OF_BOUNDS)) {
      return;
    }
  }
  adcTransition = state == States::ON_TRANS_TX and adcCountdown.isBusy();
  if (state >= States::ON_TRANS_TX and not adcTransition) {
    if (txToMpaInjectionRequested) {
      handleFailureInjection("TX to MPA", U_TX_OUT_OF_BOUNDS);
      txToMpaInjectionRequested = false;
      return;
    }
    params.getValue(PARAM_KEY_MAP[TX_U_LOWER_BOUND], lowerBound);
    params.getValue(PARAM_KEY_MAP[TX_U_UPPER_BOUND], upperBound);
    if (not checkVoltage(adcSet.processed[U_TX_DIV_6], lowerBound, upperBound,
                         U_TX_OUT_OF_BOUNDS)) {
      return;
    }
    params.getValue(PARAM_KEY_MAP[TX_I_UPPER_BOUND], upperBound);
    if (not checkCurrent(adcSet.processed[I_TX], upperBound, I_TX_OUT_OF_BOUNDS)) {
      return;
    }
  }
  adcTransition = state == States::ON_TRANS_MPA and adcCountdown.isBusy();
  if (state >= States::ON_TRANS_MPA and not adcTransition) {
    if (mpaToHpaInjectionRequested) {
      handleFailureInjection("MPA to HPA", U_HPA_OUT_OF_BOUNDS);
      mpaToHpaInjectionRequested = false;
      return;
    }
    params.getValue(PARAM_KEY_MAP[MPA_U_LOWER_BOUND], lowerBound);
    params.getValue(PARAM_KEY_MAP[MPA_U_UPPER_BOUND], upperBound);
    if (not checkVoltage(adcSet.processed[U_MPA_DIV_6], lowerBound, upperBound,
                         U_MPA_OUT_OF_BOUNDS)) {
      return;
    }
    params.getValue(PARAM_KEY_MAP[MPA_I_UPPER_BOUND], upperBound);
    if (not checkCurrent(adcSet.processed[I_MPA], upperBound, I_MPA_OUT_OF_BOUNDS)) {
      return;
    }
  }
  adcTransition = state == States::ON_TRANS_HPA and adcCountdown.isBusy();
  if (state >= States::ON_TRANS_HPA and not adcTransition) {
    if (allOnInjectRequested) {
      handleFailureInjection("All On", U_HPA_OUT_OF_BOUNDS);
      allOnInjectRequested = false;
      return;
    }
    params.getValue(PARAM_KEY_MAP[HPA_U_LOWER_BOUND], lowerBound);
    params.getValue(PARAM_KEY_MAP[HPA_U_UPPER_BOUND], upperBound);
    if (not checkVoltage(adcSet.processed[U_HPA_DIV_6], lowerBound, upperBound,
                         U_HPA_OUT_OF_BOUNDS)) {
      return;
    }
    params.getValue(PARAM_KEY_MAP[HPA_I_UPPER_BOUND], upperBound);
    if (not checkCurrent(adcSet.processed[I_HPA], upperBound, I_HPA_OUT_OF_BOUNDS)) {
      sif::warning << "PayloadPcduHandler::checkCurrent: I HPA exceeded limit: Measured "
                   << adcSet.processed[I_HPA] << " mA" << std::endl;
      return;
    }
  }
  transitionOk = true;
}

void PayloadPcduHandler::checkJsonFileInit() {
  if (not jsonFileInitComplete) {
    sd::SdCard activeSd = sdcMan->getActiveSdCard();
    if (sdcMan->isSdCardUsable(activeSd)) {
      params.initialize(sdcMan->getCurrentMountPrefix());
      jsonFileInitComplete = true;
    }
  }
}

bool PayloadPcduHandler::checkVoltage(float val, float lowerBound, float upperBound, Event event) {
  bool tooLarge = false;
  if (val < lowerBound or val > upperBound) {
    if (val > upperBound) {
      tooLarge = true;
    } else {
      tooLarge = false;
    }
    uint32_t p2 = 0;
    serializeFloat(p2, val);
    triggerEvent(event, tooLarge, p2);
    transitionOk = false;
    quickTransitionBackToOff(true, true);
    quickTransitionAlreadyCalled = true;
    return false;
  }
  return true;
}

bool PayloadPcduHandler::checkCurrent(float val, float upperBound, Event event) {
  if (val > upperBound) {
    uint32_t p2 = 0;
    serializeFloat(p2, val);
    triggerEvent(event, true, p2);
    transitionOk = false;
    quickTransitionBackToOff(true, true);
    quickTransitionAlreadyCalled = true;
    return false;
  }
  return true;
}

ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
  using namespace plpcdu;
  if (mode == MODE_NORMAL) {
    diffMask = submode ^ this->submode;
    // Also deals with the case where the mode is MODE_ON, submode should be 0 here
    if ((((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == SOLID_STATE_RELAYS_ADC_ON) and
        (this->mode == MODE_NORMAL and this->submode != ALL_OFF_SUBMODE)) {
      return TRANS_NOT_ALLOWED;
    }
    if (((((submode >> DRO_ON) & 1) == 1) and
         ((this->submode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
      return TRANS_NOT_ALLOWED;
    }
    if ((((submode >> X8_ON) & 1) == 1) and
        ((this->submode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
      return TRANS_NOT_ALLOWED;
    }
    if (((((submode >> TX_ON) & 1) == 1) and
         ((this->submode & 0b111) !=
          ((1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
      return TRANS_NOT_ALLOWED;
    }
    if ((((submode >> MPA_ON) & 1) == 1 and
         ((this->submode & 0b1111) !=
          ((1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
      return TRANS_NOT_ALLOWED;
    }
    if ((((submode >> HPA_ON) & 1) == 1 and
         ((this->submode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) |
                                        (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
      return TRANS_NOT_ALLOWED;
    }
    return returnvalue::OK;
  }
  return DeviceHandlerBase::isModeCombinationValid(mode, submode);
}

ReturnValue_t PayloadPcduHandler::serializeFloat(uint32_t& param, float val) {
  size_t dummy = 0;
  return SerializeAdapter::serialize(&val, reinterpret_cast<uint8_t*>(&param), &dummy, 4,
                                     SerializeIF::Endianness::NETWORK);
}

ReturnValue_t PayloadPcduHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
                                               ParameterWrapper* parameterWrapper,
                                               const ParameterWrapper* newValues,
                                               uint16_t startAtIndex) {
  using namespace plpcdu;
  switch (uniqueId) {
    case (PlPcduParamIds::NEG_V_LOWER_BOUND):
    case (PlPcduParamIds::NEG_V_UPPER_BOUND):
    case (PlPcduParamIds::DRO_U_LOWER_BOUND):
    case (PlPcduParamIds::DRO_U_UPPER_BOUND):
    case (PlPcduParamIds::DRO_I_UPPER_BOUND):
    case (PlPcduParamIds::X8_U_LOWER_BOUND):
    case (PlPcduParamIds::X8_U_UPPER_BOUND):
    case (PlPcduParamIds::X8_I_UPPER_BOUND):
    case (PlPcduParamIds::TX_U_LOWER_BOUND):
    case (PlPcduParamIds::TX_U_UPPER_BOUND):
    case (PlPcduParamIds::TX_I_UPPER_BOUND):
    case (PlPcduParamIds::MPA_U_LOWER_BOUND):
    case (PlPcduParamIds::MPA_U_UPPER_BOUND):
    case (PlPcduParamIds::MPA_I_UPPER_BOUND):
    case (PlPcduParamIds::HPA_U_LOWER_BOUND):
    case (PlPcduParamIds::HPA_U_UPPER_BOUND):
    case (PlPcduParamIds::HPA_I_UPPER_BOUND):
    case (PlPcduParamIds::SSR_TO_DRO_WAIT_TIME):
    case (PlPcduParamIds::DRO_TO_X8_WAIT_TIME):
    case (PlPcduParamIds::X8_TO_TX_WAIT_TIME):
    case (PlPcduParamIds::TX_TO_MPA_WAIT_TIME):
    case (PlPcduParamIds::MPA_TO_HPA_WAIT_TIME): {
      handleDoubleParamUpdate(PARAM_KEY_MAP[static_cast<PlPcduParamIds>(uniqueId)],
                              parameterWrapper, newValues);
      break;
    }
    case (PlPcduParamIds::INJECT_SSR_TO_DRO_FAILURE): {
      ssrToDroInjectionRequested = true;
      break;
    }
    case (PlPcduParamIds::INJECT_DRO_TO_X8_FAILURE): {
      droToX8InjectionRequested = true;
      break;
    }
    case (PlPcduParamIds::INJECT_X8_TO_TX_FAILURE): {
      x8ToTxInjectionRequested = true;
      break;
    }
    case (PlPcduParamIds::INJECT_TX_TO_MPA_FAILURE): {
      txToMpaInjectionRequested = true;
      break;
    }
    case (PlPcduParamIds::INJECT_MPA_TO_HPA_FAILURE): {
      mpaToHpaInjectionRequested = true;
      break;
    }
    case (PlPcduParamIds::INJECT_ALL_ON_FAILURE): {
      allOnInjectRequested = true;
      break;
    }
    default: {
      return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
                                             startAtIndex);
    }
  }
  return returnvalue::OK;
}

void PayloadPcduHandler::handleFailureInjection(std::string output, Event event) {
  sif::info << "PayloadPcduHandler::checkAdcValues: " << output
            << " failure injection. "
               "Transitioning back to off"
            << std::endl;
  triggerEvent(event, 0, 0);
  transitionOk = false;
  quickTransitionBackToOff(true, true);
  quickTransitionAlreadyCalled = true;
  droToX8InjectionRequested = false;
}

ReturnValue_t PayloadPcduHandler::handleDoubleParamUpdate(std::string key,
                                                          ParameterWrapper* parameterWrapper,
                                                          const ParameterWrapper* newValues) {
  double newValue = 0.0;
  ReturnValue_t result = newValues->getElement<double>(&newValue, 0, 0);
  if (result != returnvalue::OK) {
    return result;
  }
  params.setValue(key, newValue);
  // Do this so the dumping and loading with the framework works as well
  doubleDummy = newValue;
  parameterWrapper->set(doubleDummy);
  return params.writeJsonFile();
}

#ifdef XIPHOS_Q7S
ReturnValue_t PayloadPcduHandler::extConvAsTwoCallback(SpiComIF* comIf, SpiCookie* cookie,
                                                       const uint8_t* sendData, size_t sendLen,
                                                       void* args) {
  auto handler = reinterpret_cast<PayloadPcduHandler*>(args);
  if (handler == nullptr) {
    sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
               << std::endl;
    return returnvalue::FAILED;
  }
  DeviceCommandId_t currentCommand = handler->getPendingCommand();
  switch (currentCommand) {
    case (plpcdu::READ_WITH_TEMP_EXT): {
      return transferAsTwo(comIf, cookie, sendData, sendLen, false);
    }
    case (plpcdu::READ_TEMP_EXT): {
      return transferAsTwo(comIf, cookie, sendData, sendLen, true);
    }
    default: {
      return comIf->performRegularSendOperation(cookie, sendData, sendLen);
    }
  }
  return returnvalue::OK;
}

ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cookie,
                                                const uint8_t* sendData, size_t sendLen,
                                                bool tempOnly) {
  ReturnValue_t result = returnvalue::OK;
  int retval = 0;
  // Prepare transfer
  int fileDescriptor = 0;
  UnixFileGuard fileHelper(comIf->getSpiDev(), &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
  if (fileHelper.getOpenResult() != returnvalue::OK) {
    return SpiComIF::OPENING_FILE_FAILED;
  }
  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
  uint32_t spiSpeed = 0;
  cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
  comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
  cookie->assignWriteBuffer(sendData);
  size_t transferLen = plpcdu::TEMP_REPLY_SIZE;
  if (not tempOnly) {
    transferLen += plpcdu::ADC_REPLY_SIZE;
  }
  cookie->setTransferSize(transferLen);

  gpioId_t gpioId = cookie->getChipSelectPin();
  GpioIF* gpioIF = comIf->getGpioInterface();
  MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
  uint32_t timeoutMs = 0;
  MutexIF* mutex = comIf->getCsMutex();
  if (mutex == nullptr or gpioIF == nullptr) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::warning << "GyroADIS16507Handler::spiSendCallback: "
                    "Mutex or GPIO interface invalid"
                 << std::endl;
    return returnvalue::FAILED;
#endif
  }

  if (gpioId != gpio::NO_GPIO) {
    cookie->getMutexParams(timeoutType, timeoutMs);
    result = mutex->lockMutex(timeoutType, timeoutMs);
    if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
      return result;
    }
  }
  spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
  uint64_t origTx = transferStruct->tx_buf;
  uint64_t origRx = transferStruct->rx_buf;
  if (tempOnly) {
    transferLen = 1;
  } else {
    transferLen = plpcdu::ADC_REPLY_SIZE + 1;
  }
  transferStruct->len = transferLen;
  // Pull SPI CS low. For now, no support for active high given
  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullLow(gpioId);
  }

  // Execute transfer
  // Initiate a full duplex SPI transfer.
  retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
  if (retval < 0) {
    utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
    result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
  }
#if FSFW_HAL_SPI_WIRETAPPING == 1
  comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */

  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullHigh(gpioId);
  }

  transferStruct->tx_buf += transferLen;
  transferStruct->rx_buf += transferLen;
  transferStruct->len = plpcdu::TEMP_REPLY_SIZE - 1;
  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullLow(gpioId);
  }

  // Execute transfer
  // Initiate a full duplex SPI transfer.
  retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
  if (retval < 0) {
    utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
    result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
  }
#if FSFW_HAL_SPI_WIRETAPPING == 1
  comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */

  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullHigh(gpioId);
  }

  transferStruct->tx_buf = origTx;
  transferStruct->rx_buf = origRx;
  if (gpioId != gpio::NO_GPIO) {
    mutex->unlockMutex();
  }
  return returnvalue::OK;
}

#endif