#include "PowerStateMachineBase.h"
#include "fsfw/serviceinterface.h"

PowerStateMachineBase::PowerStateMachineBase(PowerSwitchIF *pwrSwitcher, dur_millis_t checkTimeout)
    : pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {}

void PowerStateMachineBase::reset() {
  state = power::States::IDLE;
  opResult = power::OpCodes::NONE;
  targetMode = HasModesIF::MODE_OFF;
  targetSubmode = 0;
  checkTimeout.resetTimer();
}

void PowerStateMachineBase::setCheckTimeout(dur_millis_t timeout) {
  checkTimeout.setTimeout(timeout);
}

void PowerStateMachineBase::start(Mode_t mode, Submode_t submode) {
  reset();
  checkTimeout.resetTimer();
  targetMode = mode;
  targetSubmode = submode;
  sif::debug << "starting power fsm with mode " << mode << " and submode " << (int) submode
      << std::endl;
  state = power::States::SWITCHING_POWER;
}

power::States PowerStateMachineBase::getState() const { return state; }

bool PowerStateMachineBase::active() {
  if (state == power::States::IDLE or state == power::States::MODE_COMMANDING) {
    return false;
  }
  return true;
}