#include "RwAssembly.h"

RwAssembly::RwAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::Switch_t switchId,
                       RwHelper helper)
    : SharedPowerAssemblyBase(objectId, pwrSwitcher, switchId), helper(helper) {
  ModeListEntry entry;
  for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
    entry.setObject(helper.rwIds[idx]);
    entry.setMode(MODE_OFF);
    entry.setSubmode(SUBMODE_NONE);
    modeTable.insert(entry);
  }
}

ReturnValue_t RwAssembly::commandChildren(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = returnvalue::OK;
  // Initialize the mode table to ensure all devices are in a defined state
  for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
    modeTable[idx].setMode(MODE_OFF);
    modeTable[idx].setSubmode(SUBMODE_NONE);
  }
  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
      result = handleNormalOrOnModeCmd(mode, submode);
    }
  }
  HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
  executeTable(tableIter);
  return result;
}

ReturnValue_t RwAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
  int devsInCorrectMode = 0;
  try {
    for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
      if (childrenMap.at(helper.rwIds[idx]).mode == wantedMode) {
        devsInCorrectMode++;
      }
    }
  } catch (const std::out_of_range& e) {
    sif::error << "RW ASSY: Invalid children map: " << e.what() << std::endl;
  }
  if (devsInCorrectMode < 3) {
    if (warningSwitch) {
      sif::warning << "RW ASSY: Only " << devsInCorrectMode << " devices in correct mode"
                   << std::endl;
      warningSwitch = false;
    }
    return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
  }
  return returnvalue::OK;
}

ReturnValue_t RwAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
  if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) {
    return returnvalue::OK;
  }
  return HasModesIF::INVALID_MODE;
}

ReturnValue_t RwAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = returnvalue::OK;
  object_id_t objId = 0;
  try {
    for (uint8_t idx = 0; idx < NUMBER_RWS; idx++) {
      if (isUseable(objId, mode)) {
        modeTable[idx].setMode(mode);
        modeTable[idx].setSubmode(submode);
      }
    }
  } catch (const std::out_of_range& e) {
    sif::error << "RW ASSY: Invalid children map: " << e.what() << std::endl;
  }
  return result;
}

bool RwAssembly::isUseable(object_id_t object, Mode_t mode) {
  if (healthHelper.healthTable->isFaulty(object)) {
    return false;
  }

  // Check if device is already in target mode
  if (childrenMap[object].mode == mode) {
    return true;
  }

  if (healthHelper.healthTable->isCommandable(object)) {
    return true;
  }
  return false;
}

ReturnValue_t RwAssembly::initialize() {
  for (auto objId : helper.rwIds) {
    updateChildModeByObjId(objId, MODE_OFF, SUBMODE_NONE);
  }
  return AssemblyBase::initialize();
}