#include "SharedPowerAssemblyBase.h"

SharedPowerAssemblyBase::SharedPowerAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
                                                 power::Switch_t switchId, uint16_t cmdQueueDepth)
    : AssemblyBase(objectId, cmdQueueDepth), switcher(pwrSwitcher, switchId) {}

void SharedPowerAssemblyBase::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);
  }
}

void SharedPowerAssemblyBase::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);
  }
}

void SharedPowerAssemblyBase::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();
  }
}

bool SharedPowerAssemblyBase::checkAndHandleRecovery() {
  using namespace power;
  if (recoveryState == RECOVERY_IDLE) {
    return AssemblyBase::checkAndHandleRecovery();
  }
  if (customRecoveryStates == IDLE) {
    switcher.turnOff();
    customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_OFF;
  }
  if (customRecoveryStates == POWER_SWITCHING_OFF) {
    switcher.doStateMachine();
    if (switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
      customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON;
      switcher.turnOn();
    }
  }
  if (customRecoveryStates == POWER_SWITCHING_ON) {
    switcher.doStateMachine();
    if (switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
      customRecoveryStates = RecoveryCustomStates::DONE;
    }
  }
  if (customRecoveryStates == DONE) {
    bool pendingRecovery = AssemblyBase::checkAndHandleRecovery();
    if (not pendingRecovery) {
      customRecoveryStates = RecoveryCustomStates::IDLE;
    }
    // For a recovery on one side, only do the recovery once
    for (auto& child : childrenMap) {
      if (healthHelper.healthTable->getHealth(child.first) == HasHealthIF::NEEDS_RECOVERY) {
        sendHealthCommand(child.second.commandQueue, HEALTHY);
        child.second.healthChanged = false;
      }
    }
    return pendingRecovery;
  }
  return true;
}