#include "AcsBoardAssembly.h"

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

#include "OBSWConfig.h"

AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, PowerSwitchIF* switcher,
                                   AcsBoardHelper helper, GpioIF* gpioIF)
    : DualLaneAssemblyBase(objectId, switcher, SWITCH_A, SWITCH_B, POWER_STATE_MACHINE_TIMEOUT,
                           SIDE_SWITCH_TRANSITION_NOT_ALLOWED, TRANSITION_OTHER_SIDE_FAILED),
      helper(helper),
      gpioIF(gpioIF) {
  if (switcher == nullptr) {
    sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
                  "IF passed"
               << std::endl;
  }
  if (gpioIF == nullptr) {
    sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid GPIO IF passed" << std::endl;
  }
  ModeListEntry entry;
  initModeTableEntry(helper.mgm0Lis3IdSideA, entry, modeTable);
  initModeTableEntry(helper.mgm1Rm3100IdSideA, entry, modeTable);
  initModeTableEntry(helper.mgm2Lis3IdSideB, entry, modeTable);
  initModeTableEntry(helper.mgm3Rm3100IdSideB, entry, modeTable);
  initModeTableEntry(helper.gyro0AdisIdSideA, entry, modeTable);
  initModeTableEntry(helper.gyro1L3gIdSideA, entry, modeTable);
  initModeTableEntry(helper.gyro2AdisIdSideB, entry, modeTable);
  initModeTableEntry(helper.gyro3L3gIdSideB, entry, modeTable);
  initModeTableEntry(helper.gpsId, entry, modeTable);
}

ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  ReturnValue_t result = returnvalue::OK;
  refreshHelperModes();
  // Initialize the mode table to ensure all devices are in a defined state
  modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
  modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
  modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
  modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
  modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
  modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
  modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
  modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
  modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
  modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
  modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
  if (recoveryState == RecoveryState::RECOVERY_IDLE) {
    result = checkAndHandleHealthStates(mode, submode);
    if (result == NEED_TO_CHANGE_HEALTH) {
      return returnvalue::OK;
    }
  }
  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
      result = handleNormalOrOnModeCmd(mode, submode);
    }
  }
  HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
  executeTable(tableIter);
  return result;
}

ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
  using namespace duallane;
  refreshHelperModes();
  if (wantedSubmode == A_SIDE) {
    if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or
        (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or
        helper.gpsMode != MODE_ON) {
      return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
    }
    return returnvalue::OK;
  } else if (wantedSubmode == B_SIDE) {
    if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or
        (helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
        helper.gpsMode != MODE_ON) {
      return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
    }
    return returnvalue::OK;
  } else if (wantedSubmode == DUAL_MODE) {
    if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode and
         helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or
        (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode and
         helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
        helper.gpsMode != MODE_ON) {
      // Trigger event, but don't start any other transitions. This is the last fallback mode.
      if (dualModeErrorSwitch) {
        triggerEvent(NOT_ENOUGH_DEVICES_DUAL_MODE, 0, 0);
        dualModeErrorSwitch = false;
      }
      return returnvalue::OK;
    }
    return returnvalue::OK;
  }
  return returnvalue::OK;
}

ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
  using namespace duallane;
  ReturnValue_t result = returnvalue::OK;
  bool needsSecondStep = false;
  if (sideSwitchState == SideSwitchState::REQUESTED) {
    sideSwitchState = SideSwitchState::TO_DUAL;
  }
  // Switch to dual side first, and later switch back to the otherside
  if (sideSwitchState == SideSwitchState::TO_DUAL) {
    targetSubmodeForSideSwitch = static_cast<duallane::Submodes>(submode);
    submode = Submodes::DUAL_MODE;
    sideSwitchState = SideSwitchState::DISABLE_OTHER_SIDE;
    // TODO: Ugly hack. The base class should support arbitrary number of steps..
    needsSecondStep = true;
  } else if (sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
    submode = targetSubmodeForSideSwitch;
  }
  auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
    if (mode == devMode) {
      modeTable[tableIdx].setMode(mode);
    } else if (isUseable(objectId, devMode)) {
      modeTable[tableIdx].setMode(mode);
      modeTable[tableIdx].setSubmode(SUBMODE_NONE);
    }
  };
  bool gpsUsable = isUseable(helper.gpsId, helper.gpsMode);
  switch (submode) {
    case (A_SIDE): {
      modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
      modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
      modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
      modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
      modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
      modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
      modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
      modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
      cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
      cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
      cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
      cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
      if (gpsUsable) {
        gpioHandler(gpioIds::GNSS_0_NRESET, true,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
                    "of GNSS 0 high (used GNSS)");
        gpioHandler(gpioIds::GNSS_1_NRESET, false,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
                    "of GNSS 1 low (unused GNSS)");
        gpioHandler(gpioIds::GNSS_SELECT, false,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low");
      }
      break;
    }
    case (B_SIDE): {
      modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
      modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
      modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
      modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
      modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
      modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
      modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
      modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
      cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
      cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
      cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
      cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
      if (gpsUsable) {
        gpioHandler(gpioIds::GNSS_0_NRESET, false,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
                    "of GNSS 0 low (unused GNSS)");
        gpioHandler(gpioIds::GNSS_1_NRESET, true,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
                    "of GNSS 1 high (used GNSS)");
        gpioHandler(gpioIds::GNSS_SELECT, true,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high");
      }
      break;
    }
    case (DUAL_MODE): {
      cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
      cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
      cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
      cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
      cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
      cmdSeq(helper.gyro2AdisIdSideB, helper.gyro2SideBMode, ModeTableIdx::GYRO_2_B);
      cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
      cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
      cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
      ReturnValue_t status = returnvalue::OK;
      if (gpsUsable) {
        gpioHandler(gpioIds::GNSS_0_NRESET, true,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
                    "of GNSS 0 high (used GNSS)");
        gpioHandler(gpioIds::GNSS_1_NRESET, true,
                    "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull nReset pin"
                    "of GNSS 1 high (used GNSS)");
        if (defaultSubmode == Submodes::A_SIDE) {
          status = gpioIF->pullLow(gpioIds::GNSS_SELECT);
        } else {
          status = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
        }
        if (status != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
          sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
                        "default side for dual mode"
                     << std::endl;
#endif
        }
      }
      break;
    }
    default: {
      sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl;
    }
  }
  if (gpsUsable) {
    modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
    modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
  }
  if (needsSecondStep) {
    result = NEED_SECOND_STEP;
  }
  return result;
}

void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) {
  using namespace duallane;
  if (submode != Submodes::DUAL_MODE) {
    return;
  }
  ReturnValue_t result = returnvalue::OK;
  if (side == Submodes::A_SIDE) {
    result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
  } else {
    result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
  }
  if (result != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl;
#endif
  }
}

void AcsBoardAssembly::gpioHandler(gpioId_t gpio, bool high, std::string error) {
  ReturnValue_t result = returnvalue::OK;
  if (high) {
    result = gpioIF->pullHigh(gpio);
  } else {
    result = gpioIF->pullLow(gpio);
  }
  if (result != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::error << error << std::endl;
#endif
  }
}

void AcsBoardAssembly::refreshHelperModes() {
  try {
    helper.gyro0SideAMode = childrenMap.at(helper.gyro0AdisIdSideA).mode;
    helper.gyro1SideAMode = childrenMap.at(helper.gyro1L3gIdSideA).mode;
    helper.gyro2SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
    helper.gyro3SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
    helper.mgm0SideAMode = childrenMap.at(helper.mgm0Lis3IdSideA).mode;
    helper.mgm1SideAMode = childrenMap.at(helper.mgm1Rm3100IdSideA).mode;
    helper.mgm2SideBMode = childrenMap.at(helper.mgm2Lis3IdSideB).mode;
    helper.mgm3SideBMode = childrenMap.at(helper.mgm3Rm3100IdSideB).mode;
    helper.gpsMode = childrenMap.at(helper.gpsId).mode;
  } catch (const std::out_of_range& e) {
    sif::error << "AcsBoardAssembly::refreshHelperModes: Invalid map: " << e.what() << std::endl;
  }
}

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

ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
                                                           Submode_t deviceSubmode) {
  using namespace returnvalue;
  ReturnValue_t status = returnvalue::OK;
  auto checkAcsBoardSensorGroup = [&](object_id_t o0, object_id_t o1, object_id_t o2,
                                      object_id_t o3) {
    HealthState h0 = healthHelper.healthTable->getHealth(o0);
    HealthState h1 = healthHelper.healthTable->getHealth(o1);
    HealthState h2 = healthHelper.healthTable->getHealth(o2);
    HealthState h3 = healthHelper.healthTable->getHealth(o3);
    if ((h0 == FAULTY or h0 == PERMANENT_FAULTY) and (h1 == FAULTY or h1 == PERMANENT_FAULTY) and
        (h2 == FAULTY or h2 == PERMANENT_FAULTY) and (h3 == FAULTY or h3 == PERMANENT_FAULTY)) {
      overwriteDeviceHealth(o0, h0);
      overwriteDeviceHealth(o1, h1);
      overwriteDeviceHealth(o2, h2);
      overwriteDeviceHealth(o3, h3);
    }
    if (h0 == EXTERNAL_CONTROL or h1 == EXTERNAL_CONTROL or h2 == EXTERNAL_CONTROL or
        h3 == EXTERNAL_CONTROL) {
      modeHelper.setForced(true);
    }
  };
  if (deviceSubmode == duallane::DUAL_MODE) {
    checkAcsBoardSensorGroup(helper.mgm0Lis3IdSideA, helper.mgm1Rm3100IdSideA,
                             helper.mgm2Lis3IdSideB, helper.mgm3Rm3100IdSideB);
    checkAcsBoardSensorGroup(helper.gyro0AdisIdSideA, helper.gyro1L3gIdSideA,
                             helper.gyro2AdisIdSideB, helper.gyro3L3gIdSideB);
  }
  return status;
}