#include "DualLaneAssemblyBase.h"

DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId,
                                           PowerSwitchIF* pwrSwitcher,
                                           pcduSwitches::Switches switch1,
                                           pcduSwitches::Switches switch2, Event pwrTimeoutEvent)
    : AssemblyBase(objectId, parentId),
      pwrStateMachine(switch1, switch2, pwrSwitcher),
      pwrTimeoutEvent(pwrTimeoutEvent) {}

void DualLaneAssemblyBase::performChildOperation() {
  using namespace duallane;
  if (pwrStateMachine.active()) {
    pwrStateMachineWrapper();
    // This state is the indicator that the power state machine is done
  }
  if (not pwrStateMachine.active()) {
    AssemblyBase::performChildOperation();
  }
}

void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  pwrStateMachine.reset();
  // If anything other than MODE_OFF is commanded, perform power state machine first
  if (mode != MODE_OFF) {
    // Cache the target modes, required by power state machine
    pwrStateMachine.start(mode, submode);
    // Cache these for later after the power state machine has finished
    targetMode = mode;
    targetSubmode = submode;
    // Perform power state machine first, then start mode transition. The power state machine will
    // start the transition after it has finished
    pwrStateMachineWrapper();
  } else {
    // Command the devices to off first before switching off the power. The handleModeReached
    // custom implementation will take care of starting the power state machine.
    AssemblyBase::startTransition(mode, submode);
  }
}

bool DualLaneAssemblyBase::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 DualLaneAssemblyBase::pwrStateMachineWrapper() {
  using namespace duallane;
  OpCodes opCode = pwrStateMachine.powerStateMachine();
  if (opCode == OpCodes::NONE) {
    return RETURN_OK;
  } else if (opCode == OpCodes::FINISH_OP) {
    finishModeOp();
  } else if (opCode == OpCodes::START_TRANSITION) {
    AssemblyBase::startTransition(targetMode, targetSubmode);
  } else if (opCode == OpCodes::TIMEOUT_OCCURED) {
    if (powerRetryCounter == 0) {
      powerRetryCounter++;
      pwrStateMachine.reset();
    } else {
#if OBSW_VERBOSE_LEVEL >= 1
      sif::warning << "Timeout occured in power state machine" << std::endl;
#endif
      triggerEvent(pwrTimeoutEvent, 0, 0);
      return RETURN_FAILED;
    }
  }
  return RETURN_OK;
}

void DualLaneAssemblyBase::handleModeReached() {
  using namespace duallane;
  if (targetMode == MODE_OFF) {
    pwrStateMachine.start(targetMode, targetSubmode);
    // Now we can switch off the power. After that, the AssemblyBase::handleModeReached function
    // will be called
    pwrStateMachineWrapper();
  } else {
    finishModeOp();
  }
}