#include "DualLanePowerStateMachine.h"

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

DualLanePowerStateMachine::DualLanePowerStateMachine(power::Switch_t switchA,
                                                     power::Switch_t switchB,
                                                     PowerSwitchIF* pwrSwitcher,
                                                     dur_millis_t checkTimeout)
    : PowerStateMachineBase(pwrSwitcher, checkTimeout), SWITCH_A(switchA), SWITCH_B(switchB) {}

power::OpCodes DualLanePowerStateMachine::fsm() {
  using namespace duallane;
  ReturnValue_t switchStateA = returnvalue::OK;
  ReturnValue_t switchStateB = returnvalue::OK;
  if (state == power::States::IDLE or state == power::States::MODE_COMMANDING) {
    return opResult;
  }
  if (state == power::States::SWITCHING_POWER or state == power::States::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 = power::States::IDLE;
      opResult = power::OpCodes::TO_OFF_DONE;
      return opResult;
    }
  } else {
    switch (targetSubmode) {
      case (A_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and
            switchStateB == PowerSwitchIF::SWITCH_OFF) {
          state = power::States::MODE_COMMANDING;
          opResult = power::OpCodes::TO_NOT_OFF_DONE;
          return opResult;
        }
        break;
      }
      case (B_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_OFF and
            switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = power::States::MODE_COMMANDING;
          opResult = power::OpCodes::TO_NOT_OFF_DONE;
          return opResult;
        }
        break;
      }
      case (DUAL_MODE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = power::States::MODE_COMMANDING;
          opResult = power::OpCodes::TO_NOT_OFF_DONE;
          return opResult;
        }
      }
    }
  }
  if (state == power::States::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 = power::States::CHECKING_POWER;
  }
  if (state == power::States::CHECKING_POWER) {
    if (checkTimeout.hasTimedOut()) {
      return power::OpCodes::TIMEOUT_OCCURED;
    }
  }
  return opResult;
}