#include "SusAssembly.h"

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

SusAssembly::SusAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, SusAssHelper helper)
    : DualLaneAssemblyBase(objectId, pwrSwitcher, SWITCH_NOM, SWITCH_RED,
                           POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
                           TRANSITION_OTHER_SIDE_FAILED),
      helper(helper),
      pwrSwitcher(pwrSwitcher) {
  ModeListEntry entry;
  for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
    initModeTableEntry(helper.susIds[idx], entry, modeTable);
  }
}

ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
  sif::debug << "commanding children to mode " << mode << " and submode " << (int) submode
      << std::endl;
  ReturnValue_t result = returnvalue::OK;
  refreshHelperModes();
  // Initialize the mode table to ensure all devices are in a defined state
  for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
    modeTable[idx].setMode(MODE_OFF);
    modeTable[idx].setSubmode(SUBMODE_NONE);
  }
  if (recoveryState == RecoveryState::RECOVERY_IDLE) {
    sif::debug << "checking health states, recovery not ongoing. Commanded submode: " <<
        (int) submode << std::endl;
    result = checkAndHandleHealthStates(mode, submode);
    if (result != returnvalue::OK) {
      return result;
    }
  }
  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
      sif::debug << "handling on or normal cmd. Submode: " << (int) submode << std::endl;
      result = handleNormalOrOnModeCmd(mode, submode);
    }
  }

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

ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  ReturnValue_t result = returnvalue::OK;
  bool needsSecondStep = false;
  handleSideSwitchStates(submode, needsSecondStep);
  auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) {
    if(isModeCommandable(objectId, devMode)) {
      modeTable[tableIdx].setMode(mode);
      modeTable[tableIdx].setSubmode(SUBMODE_NONE);
    }
//    if (mode == devMode) {
//      modeTable[tableIdx].setMode(mode);
//    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
//      if (isModeCommandable(objectId, devMode)) {
//        if (devMode == MODE_ON) {
//          modeTable[tableIdx].setMode(mode);
//          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
//        } else {
//          modeTable[tableIdx].setMode(MODE_ON);
//          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
//          if (internalState != STATE_SECOND_STEP) {
//            needsSecondStep = true;
//          }
//        }
//      }
//    } else if (mode == MODE_ON) {
//      if (isModeCommandable(objectId, devMode)) {
//        modeTable[tableIdx].setMode(MODE_ON);
//        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
//      }
//    }
  };
  switch (submode) {
    case (A_SIDE): {
      sif::debug << "commanding a side" << std::endl;
      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
        cmdSeq(helper.susIds[idx], helper.susModes[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);
      }
      break;
    }
    case (B_SIDE): {
      sif::debug << "commanding b side" << std::endl;
      for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
        cmdSeq(helper.susIds[idx], helper.susModes[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);
      }
      break;
    }
    case (DUAL_MODE): {
      sif::debug << "commanding dual mode for all sensors" << std::endl;
      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
        cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
      }
      break;
    }
  }
  if (needsSecondStep) {
    result = NEED_SECOND_STEP;
  }
  return result;
}

ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
  using namespace duallane;
  refreshHelperModes();
  if (wantedSubmode == A_SIDE) {
    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 returnvalue::OK;
  } else if (wantedSubmode == B_SIDE) {
    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 returnvalue::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 returnvalue::OK;
  }
  return returnvalue::OK;
}

ReturnValue_t SusAssembly::initialize() {
  for (const auto& child : childrenMap) {
    updateChildModeByObjId(child.first, MODE_OFF, 0);
  }
  return AssemblyBase::initialize();
}

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

ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t commandedMode, Submode_t commandedSubmode) {
  using namespace returnvalue;
  ReturnValue_t status = returnvalue::OK;
  bool needsHealthOverwritten = false;
  auto checkSusGroup = [&](object_id_t devNom, object_id_t devRed) {
    HealthState healthNom = healthHelper.healthTable->getHealth(devNom);
    HealthState healthRed = healthHelper.healthTable->getHealth(devRed);
    if ((healthNom == FAULTY or healthNom == PERMANENT_FAULTY) and
        (healthRed == FAULTY or healthRed == PERMANENT_FAULTY)) {
      overwriteDeviceHealth(devNom, healthNom);
      overwriteDeviceHealth(devRed, healthRed);
      sif::debug << "SUS module health was overwritten" << std::endl;
      needsHealthOverwritten = true;
    }
  };
  auto checkHealthForOneDev = [&](object_id_t dev) {
    HealthState health = healthHelper.healthTable->getHealth(dev);
    if (health == EXTERNAL_CONTROL) {
      modeHelper.setForced(true);
    }
  };
  if (commandedSubmode == duallane::DUAL_MODE) {
    uint8_t idx = 0;
    sif::debug << "doing dual mode health handling" << std::endl;
    for (idx = 0; idx < 6; idx++) {
      checkSusGroup(helper.susIds[idx], helper.susIds[idx + 6]);
      checkHealthForOneDev(helper.susIds[idx]);
    }
    for (idx = 6; idx < 12; idx++) {
      checkHealthForOneDev(helper.susIds[idx]);
    }
  }
  if(needsHealthOverwritten) {
    mode = commandedMode;
    submode = commandedSubmode;
    // We need second step instead of NEED_TO_CHANGE_HEALTH because we do not want recovery
    // handling.
    return NEED_TO_CHANGE_HEALTH;
  }
  return status;
}