#include "SusAssembly.h"

#include <devices/powerSwitcherList.h>
#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/serviceinterface.h>

SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
                         SusAssHelper helper)
    : AssemblyBase(objectId, parentId), helper(helper), pwrSwitcher(pwrSwitcher) {}

ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = RETURN_OK;
  refreshHelperModes();
  return RETURN_OK;
}

ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
  return RETURN_OK;
}

ReturnValue_t SusAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
  if (submode != NOMINAL and submode != REDUNDANT and submode != DUAL_MODE) {
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SusAssembly::initialize() {
  ReturnValue_t result = RETURN_OK;
  for (const auto& id : helper.susIds) {
    result = registerChild(id);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      return result;
    }
  }
  return result;
}

bool SusAssembly::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;
}

void SusAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
  ReturnValue_t switchStateNom = RETURN_OK;
  ReturnValue_t switchStateRed = RETURN_OK;
  if (state == States::IDLE or state == States::SWITCHING_POWER) {
    switchStateNom = pwrSwitcher->getSwitchState(pcduSwitches::PDU1_CH4_SUS_NOMINAL_3V3);
    switchStateRed = pwrSwitcher->getSwitchState(pcduSwitches::PDU2_CH4_SUS_REDUNDANT_3V3);
    if (mode == MODE_OFF) {
      if (switchStateNom == PowerSwitchIF::SWITCH_OFF and
          switchStateRed == PowerSwitchIF::SWITCH_OFF) {
        state = States::MODE_COMMANDING;
        return;
      }
    } else {
      switch (submode) {
        case (NOMINAL): {
          if (switchStateNom == PowerSwitchIF::SWITCH_ON and
              switchStateRed == PowerSwitchIF::SWITCH_OFF) {
            state = States::MODE_COMMANDING;
            return;
          }
          break;
        }
        case (REDUNDANT): {
          if (switchStateNom == PowerSwitchIF::SWITCH_OFF and
              switchStateRed == PowerSwitchIF::SWITCH_ON) {
            state = States::MODE_COMMANDING;
            return;
          }
          break;
        }
        case (DUAL_MODE): {
          if (switchStateNom == PowerSwitchIF::SWITCH_ON and
              switchStateRed == PowerSwitchIF::SWITCH_ON) {
            state = States::MODE_COMMANDING;
            return;
          }
        }
      }
    }
  }
}

void SusAssembly::refreshHelperModes() {
  for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
    helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
  }
}