#include "DualLanePowerStateMachine.h"

#include <fsfw/devicehandlers/AssemblyBase.h>
#include <fsfw/power/PowerSwitchIF.h>

DualLanePowerStateMachine::DualLanePowerStateMachine(pcduSwitches::Switches switchA,
                                                     pcduSwitches::Switches switchB,
                                                     PowerSwitchIF* pwrSwitcher,
                                                     dur_millis_t checkTimeout)
    : SWITCH_A(switchA), SWITCH_B(switchB), pwrSwitcher(pwrSwitcher), checkTimeout(checkTimeout) {}

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

void DualLanePowerStateMachine::start(Mode_t mode, Submode_t submode) {
  reset();
  checkTimeout.resetTimer();
  targetMode = mode;
  targetSubmode = submode;
  state = duallane::PwrStates::SWITCHING_POWER;
}

duallane::PwrStates DualLanePowerStateMachine::getState() const { return state; }

bool DualLanePowerStateMachine::active() {
  if (state == duallane::PwrStates::IDLE or state == duallane::PwrStates::MODE_COMMANDING) {
    return false;
  }
  return true;
}

duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() {
  using namespace duallane;
  ReturnValue_t switchStateA = RETURN_OK;
  ReturnValue_t switchStateB = RETURN_OK;
  if (state == PwrStates::IDLE or state == PwrStates::MODE_COMMANDING) {
    return opResult;
  }
  if (state == PwrStates::SWITCHING_POWER or state == PwrStates::CHECKING_POWER) {
    switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
    switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
  } else {
    return opResult;
  }
  if (targetMode == HasModesIF::MODE_OFF) {
    if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
      state = PwrStates::IDLE;
      opResult = OpCodes::TO_OFF_DONE;
      return opResult;
    }
  } else {
    switch (targetSubmode) {
      case (A_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and
            switchStateB == PowerSwitchIF::SWITCH_OFF) {
          state = PwrStates::MODE_COMMANDING;
          opResult = OpCodes::TO_NOT_OFF_DONE;
          return opResult;
        }
        break;
      }
      case (B_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_OFF and
            switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = PwrStates::MODE_COMMANDING;
          opResult = OpCodes::TO_NOT_OFF_DONE;
          return opResult;
        }
        break;
      }
      case (DUAL_MODE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = PwrStates::MODE_COMMANDING;
          opResult = OpCodes::TO_NOT_OFF_DONE;
          return opResult;
        }
      }
    }
  }
  if (state == PwrStates::SWITCHING_POWER) {
    if (targetMode == HasModesIF::MODE_OFF) {
      if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
        pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
      }
      if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
        pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
      }
      checkTimeout.resetTimer();
    } else {
      switch (targetSubmode) {
        case (A_SIDE): {
          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
          }
          if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
          }
          checkTimeout.resetTimer();
          break;
        }
        case (B_SIDE): {
          if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
          }
          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
          }
          checkTimeout.resetTimer();
          break;
        }
        case (DUAL_MODE): {
          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
          }
          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
          }
          checkTimeout.resetTimer();
          break;
        }
      }
    }
    state = PwrStates::CHECKING_POWER;
  }
  if (state == PwrStates::CHECKING_POWER) {
    if (checkTimeout.hasTimedOut()) {
      return OpCodes::TIMEOUT_OCCURED;
    }
  }
  return opResult;
}

void DualLanePowerStateMachine::reset() {
  state = duallane::PwrStates::IDLE;
  opResult = duallane::OpCodes::NONE;
  targetMode = HasModesIF::MODE_OFF;
  targetSubmode = 0;
  checkTimeout.resetTimer();
}