#include "DualLanePowerStateMachine.h"

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

DualLanePowerStateMachine::DualLanePowerStateMachine(pcduSwitches::Switches switchA,
                                                     pcduSwitches::Switches switchB,
                                                     PowerSwitchIF* pwrSwitcher,
                                                     duallane::PwrStates& state)
    : SWITCH_A(switchA), SWITCH_B(switchB), state(state), pwrSwitcher(pwrSwitcher) {}

duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  ReturnValue_t switchStateA = RETURN_OK;
  ReturnValue_t switchStateB = RETURN_OK;
  if (state == PwrStates::IDLE or state == PwrStates::SWITCHING_POWER or
      state == PwrStates::CHECKING_POWER) {
    switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
    switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
  } else {
    return OpCodes::NONE;
  }
  if (mode == HasModesIF::MODE_OFF) {
    if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
      return OpCodes::FINISH_OP;
    }
  } else {
    switch (submode) {
      case (A_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and
            switchStateB == PowerSwitchIF::SWITCH_OFF) {
          state = PwrStates::MODE_COMMANDING;
          return OpCodes::START_TRANSITION;
        }
        break;
      }
      case (B_SIDE): {
        if (switchStateA == PowerSwitchIF::SWITCH_OFF and
            switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = PwrStates::MODE_COMMANDING;
          return OpCodes::START_TRANSITION;
        }
        break;
      }
      case (DUAL_MODE): {
        if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
          state = PwrStates::MODE_COMMANDING;
          return OpCodes::START_TRANSITION;
        }
      }
    }
  }
  if (state == PwrStates::SWITCHING_POWER) {
    if (mode == HasModesIF::MODE_OFF) {
      if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
        pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
      }
      if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
        pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
      }
    } else {
      switch (submode) {
        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);
          }
          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_OFF);
          }
          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);
          }
          break;
        }
      }
    }
    state = PwrStates::CHECKING_POWER;
  }
  if (state == PwrStates::CHECKING_POWER) {
    // TODO: Could check for a timeout (temporal or cycles) here and resend command
  }
  return OpCodes::NONE;
}