#include "TcsBoardAssembly.h"

#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/ipc/QueueFactory.h>

TcsBoardAssembly::TcsBoardAssembly(object_id_t objectId, object_id_t parentId,
                                   PowerSwitchIF* pwrSwitcher, power::Switch_t theSwitch,
                                   TcsBoardHelper helper)
    : AssemblyBase(objectId, parentId, 24), switcher(pwrSwitcher, theSwitch), helper(helper) {
  eventQueue = QueueFactory::instance()->createMessageQueue(24);
  ModeListEntry entry;
  for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
    entry.setObject(helper.rtdInfos[idx].first);
    entry.setMode(MODE_OFF);
    entry.setSubmode(SUBMODE_NONE);
    entry.setInheritSubmode(false);
    modeTable.insert(entry);
  }
}

void TcsBoardAssembly::performChildOperation() {
  auto state = switcher.getState();
  if (state != PowerSwitcher::WAIT_OFF and state != PowerSwitcher::WAIT_ON) {
    AssemblyBase::performChildOperation();
    return;
  }
  switcher.doStateMachine();
  if (state == PowerSwitcher::WAIT_OFF and switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
    // Indicator that a transition to off is finished
    AssemblyBase::handleModeReached();
  } else if (state == PowerSwitcher::WAIT_ON and
             switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
    // Indicator that mode commanding can be performed now
    AssemblyBase::startTransition(targetMode, targetSubmode);
  }
}

ReturnValue_t TcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = RETURN_OK;
  // Initialize the mode table to ensure all devices are in a defined state
  for (uint8_t idx = 0; idx < NUMBER_RTDS; 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 TcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
  int devsInWrongMode = 0;
  try {
    for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
      if (childrenMap.at(helper.rtdInfos[idx].first).mode != wantedMode) {
        devsInWrongMode++;
      }
    }
  } catch (const std::out_of_range& e) {
    sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
  }
  if (devsInWrongMode >= 3) {
    if (warningSwitch) {
      sif::warning << "TcsBoardAssembly::checkChildrenStateOn: " << devsInWrongMode << " devices in"
                   << " wrong mode" << std::endl;
      warningSwitch = false;
    }
    return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
  }
  // TODO: Can't really do something other than power cycling if devices in wrong mode.
  // Might attempt one power-cycle. In any case, trigger an event
  if (devsInWrongMode > 0) {
    if (warningSwitch) {
      sif::warning << "TcsBoardAssembly::checkChildrenStateOn: " << devsInWrongMode << " devices in"
                   << " wrong mode" << std::endl;
      warningSwitch = false;
    }
  }
  return RETURN_OK;
}

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

ReturnValue_t TcsBoardAssembly::initialize() {
  ReturnValue_t result = RETURN_OK;
  for (const auto& obj : helper.rtdInfos) {
    result = registerChild(obj.first);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      return result;
    }
  }
  return SubsystemBase::initialize();
}

void TcsBoardAssembly::startTransition(Mode_t mode, Submode_t submode) {
  if (mode != MODE_OFF) {
    switcher.turnOn(true);
    switcher.doStateMachine();
    if (switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
      AssemblyBase::startTransition(mode, submode);
    } else {
      // Need to wait with mode commanding until power switcher is done
      targetMode = mode;
      targetSubmode = submode;
    }
  } else {
    // Perform regular mode commanding first
    AssemblyBase::startTransition(mode, submode);
  }
}

ReturnValue_t TcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = RETURN_OK;
  bool needsSecondStep = false;
  Mode_t devMode = 0;
  object_id_t objId = 0;
  try {
    for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
      devMode = childrenMap.at(helper.rtdInfos[idx].first).mode;
      objId = helper.rtdInfos[idx].first;
      if (mode == devMode) {
        modeTable[idx].setMode(mode);
      } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
        if (isUseable(objId, devMode)) {
          if (devMode == MODE_ON) {
            modeTable[idx].setMode(mode);
            modeTable[idx].setSubmode(SUBMODE_NONE);
          } else {
            modeTable[idx].setMode(MODE_ON);
            modeTable[idx].setSubmode(SUBMODE_NONE);
            if (internalState != STATE_SECOND_STEP) {
              needsSecondStep = true;
            }
          }
        }
      } else if (mode == MODE_ON) {
        if (isUseable(objId, devMode)) {
          modeTable[idx].setMode(MODE_ON);
          modeTable[idx].setSubmode(SUBMODE_NONE);
        }
      }
    }
  } catch (const std::out_of_range& e) {
    sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
  }
  if (needsSecondStep) {
    result = NEED_SECOND_STEP;
  }
  return result;
}

bool TcsBoardAssembly::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;
}

MessageQueueId_t TcsBoardAssembly::getEventReceptionQueue() { return eventQueue->getId(); }

void TcsBoardAssembly::handleModeReached() {
  if (targetMode == MODE_OFF) {
    switcher.turnOff(true);
    switcher.doStateMachine();
    // Need to wait with call to AssemblyBase::handleModeReached until power switcher is done
    if (switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
      AssemblyBase::handleModeReached();
    }
  } else {
    AssemblyBase::handleModeReached();
  }
}

void TcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
  // TODO: Maybe try a reboot once here?
  triggerEvent(CHILDREN_LOST_MODE, result);
  return;
}

void TcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
  if (targetMode == MODE_OFF) {
    AssemblyBase::handleModeTransitionFailed(result);
  } else {
    // To avoid transitioning back to off
    triggerEvent(MODE_TRANSITION_FAILED, result);
  }
}

ModeDefinitionHelper TcsBoardAssembly::getModeDefinitionHelper(){
  return ModeDefinitionHelper::create<DeviceHandlerIF::DeviceHandlerMode, HasModesIF::DefaultSubmode>();
}