#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) {
  ModeListEntry entry;
  for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
    initModeTableEntry(helper.susIds[idx], entry);
  }
}

ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = RETURN_OK;
  refreshHelperModes();
  powerStateMachine(mode, submode);
  if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
    if (state == States::MODE_COMMANDING) {
      handleNormalOrOnModeCmd(mode, submode);
    }
  } else {
    for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
      modeTable[idx].setMode(MODE_OFF);
      modeTable[idx].setSubmode(SUBMODE_NONE);
    }
  }

  HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
  executeTable(tableIter);
  return result;
}

ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = RETURN_OK;
  auto cmdSeq = [&](object_id_t objectId, uint8_t tableIdx) {
    if (mode == DeviceHandlerIF::MODE_NORMAL) {
      if (isUseable(objectId, mode)) {
        if (helper.susModes[tableIdx] != MODE_OFF) {
          modeTable[tableIdx].setMode(mode);
          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
        } else {
          result = NEED_SECOND_STEP;
          modeTable[tableIdx].setMode(MODE_ON);
          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
        }
      }
    } else if (mode == MODE_ON) {
      if (isUseable(objectId, mode)) {
        modeTable[tableIdx].setMode(MODE_ON);
        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
      }
    }
  };
  switch (submode) {
    case (NOMINAL): {
      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
        cmdSeq(helper.susIds[idx], idx);
        // Switch off devices on redundant side
        modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
        modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
      }
      return result;
    }
    case (REDUNDANT): {
      for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
        cmdSeq(helper.susIds[idx], idx);
        // Switch devices on nominal side
        modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
        modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
      }
      return result;
    }
    case (DUAL_MODE): {
      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
        cmdSeq(helper.susIds[idx], idx);
      }
      return result;
    }
  }
  return RETURN_OK;
}

ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
  refreshHelperModes();
  if (wantedSubmode == NOMINAL) {
    for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
      if (helper.susModes[idx] != wantedMode) {
        return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
      }
    }
    return RETURN_OK;
  } else if (wantedSubmode == REDUNDANT) {
    for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
      if (helper.susModes[idx] != wantedMode) {
        return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
      }
    }
    return RETURN_OK;
  } else {
    // Trigger event if devices are faulty? This is the last fallback mode, returning
    // a failure here would trigger a transition to MODE_OFF unless handleModeTransitionFailed
    // is overriden
    return RETURN_OK;
  }
  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(SWITCH_NOM);
    switchStateRed = pwrSwitcher->getSwitchState(SWITCH_RED);
  } else {
    return;
  }
  if (mode == MODE_OFF) {
    if (switchStateNom == PowerSwitchIF::SWITCH_OFF and
        switchStateRed == PowerSwitchIF::SWITCH_OFF) {
      state = States::MODE_COMMANDING;
      return;
    }
  } else {
    if (state == States::IDLE) {
      if (mode == MODE_OFF) {
        if (switchStateNom != PowerSwitchIF::SWITCH_OFF) {
          pwrSwitcher->sendSwitchCommand(SWITCH_NOM, false);
        }
        if (switchStateRed != PowerSwitchIF::SWITCH_OFF) {
          pwrSwitcher->sendSwitchCommand(SWITCH_RED, false);
        }
      } else {
        switch (submode) {
          case (NOMINAL): {
            if (switchStateNom != PowerSwitchIF::SWITCH_ON) {
              pwrSwitcher->sendSwitchCommand(SWITCH_NOM, true);
            }
            if (switchStateRed != PowerSwitchIF::SWITCH_OFF) {
              pwrSwitcher->sendSwitchCommand(SWITCH_RED, false);
            }
            break;
          }
          case (REDUNDANT): {
            if (switchStateRed != PowerSwitchIF::SWITCH_OFF) {
              pwrSwitcher->sendSwitchCommand(SWITCH_RED, false);
            }
            if (switchStateNom != PowerSwitchIF::SWITCH_ON) {
              pwrSwitcher->sendSwitchCommand(SWITCH_NOM, true);
            }
            break;
          }
          case (DUAL_MODE): {
            if (switchStateNom != PowerSwitchIF::SWITCH_ON) {
              pwrSwitcher->sendSwitchCommand(SWITCH_NOM, true);
            }
            if (switchStateRed != PowerSwitchIF::SWITCH_ON) {
              pwrSwitcher->sendSwitchCommand(SWITCH_RED, true);
            }
            break;
          }
        }
      }
      state = States::SWITCHING_POWER;
    }
    if (state == States::SWITCHING_POWER) {
      // TODO: Could check for a timeout (temporal or cycles) here and resend command
    }
  }
}

void SusAssembly::handleModeReached() {
  AssemblyBase::handleModeReached();
  state = States::IDLE;
}

void SusAssembly::handleModeTransitionFailed(ReturnValue_t result) {
  // The sun-sensors are required for the Safe-Mode. It would be good if the software
  // transitions from nominal side to redundant side and from redundant side to dual mode
  // autonomously to ensure that that enough sensors are available witout an operators intervention.
  // Therefore, the failure handler is overriden to perform these steps.
  // TODO: Implement transitions mentioned above
}

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

void SusAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
  entry.setObject(id);
  entry.setMode(MODE_OFF);
  entry.setSubmode(SUBMODE_NONE);
  entry.setInheritSubmode(false);
  modeTable.insert(entry);
}