#include "AcsBoardAssembly.h"

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

AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
                                   PowerSwitchIF* switcher, AcsBoardHelper helper)
    : AssemblyBase(objectId, parentId), switcher(switcher), helper(helper) {
  if (switcher == nullptr) {
    sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
                  "IF passed"
               << std::endl;
  }
  ModeListEntry entry;
  initModeTableEntry(helper.mgm0Lis3IdSideA, entry);
  initModeTableEntry(helper.mgm1Rm3100IdSideA, entry);
  initModeTableEntry(helper.mgm2Lis3IdSideB, entry);
  initModeTableEntry(helper.mgm3Rm3100IdSideB, entry);
  initModeTableEntry(helper.gyro0AdisIdSideA, entry);
  initModeTableEntry(helper.gyro1L3gIdSideA, entry);
  initModeTableEntry(helper.gyro2AdisIdSideB, entry);
  initModeTableEntry(helper.gyro3L3gIdSideB, entry);
  initModeTableEntry(helper.gpsId, entry);
}

ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = RETURN_OK;
  if (currentMode == mode and submode == currentSubmode) {
    return result;
  }
  helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode;
  helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode;
  helper.gyro2SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode;
  helper.gyro3SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode;
  helper.mgm0SideAMode = childrenMap[helper.mgm0Lis3IdSideA].mode;
  helper.mgm1SideAMode = childrenMap[helper.mgm1Rm3100IdSideA].mode;
  helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode;
  helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode;
  helper.gpsMode = childrenMap[helper.gpsId].mode;
  if (state == States::MODE_COMMANDING) {
    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
      powerStateMachine(submode);
      handleNormalOrOnModeCmd(mode, submode);
    } else {
      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);
    }
  }

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

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

ReturnValue_t AcsBoardAssembly::initialize() {
  ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = registerChild(helper.gyro1L3gIdSideA);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = registerChild(helper.gyro2AdisIdSideB);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = registerChild(helper.gyro3L3gIdSideB);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = registerChild(helper.mgm0Lis3IdSideA);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = registerChild(helper.mgm1Rm3100IdSideA);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = registerChild(helper.mgm2Lis3IdSideB);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = registerChild(helper.mgm3Rm3100IdSideB);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  return result;
}

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

ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
  return HasReturnvaluesIF::RETURN_OK;
}

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

ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
  ReturnValue_t result = RETURN_OK;
  Mode_t tgtMode = DeviceHandlerIF::MODE_NORMAL;
  auto cmdSeq = [&](object_id_t objectId, ModeTableIdx tableIdx) {
    if(tgtMode == DeviceHandlerIF::MODE_NORMAL) {
      if (isUseable(objectId, mode)) {
        if (helper.gyro0SideAMode != MODE_OFF) {
          modeTable[tableIdx].setMode(tgtMode);
          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
        } else {
          result = NEED_SECOND_STEP;
          modeTable[tableIdx].setMode(MODE_ON);
          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
        }
      }
    } else if(tgtMode == MODE_ON) {
      if (isUseable(objectId, mode)) {
        modeTable[tableIdx].setMode(MODE_ON);
        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
      }
    }
  };
  switch (submode) {
    case (A_SIDE): {
      cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
      cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
      cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
      cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
      cmdSeq(helper.gpsId, ModeTableIdx::GPS);
      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);
      return result;
    }
    case (B_SIDE): {
      cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
      cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
      cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
      cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
      cmdSeq(helper.gpsId, ModeTableIdx::GPS);
      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);
      return result;
    }
    case (DUAL_MODE): {
      cmdSeq(helper.gpsId, ModeTableIdx::GPS);
      cmdSeq(helper.gyro0AdisIdSideA, ModeTableIdx::GYRO_0_A);
      cmdSeq(helper.gyro1L3gIdSideA, ModeTableIdx::GYRO_1_A);
      cmdSeq(helper.gyro2AdisIdSideB, ModeTableIdx::GYRO_2_B);
      cmdSeq(helper.gyro3L3gIdSideB, ModeTableIdx::GYRO_3_B);
      cmdSeq(helper.mgm0Lis3IdSideA, ModeTableIdx::MGM_0_A);
      cmdSeq(helper.mgm1Rm3100IdSideA, ModeTableIdx::MGM_1_A);
      cmdSeq(helper.mgm2Lis3IdSideB, ModeTableIdx::MGM_2_B);
      cmdSeq(helper.mgm3Rm3100IdSideB, ModeTableIdx::MGM_3_B);
      return result;
    }
    default: {
      sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl;
    }
  }
  return result;
}

void AcsBoardAssembly::powerStateMachine(Submode_t submode) {
  ReturnValue_t switchStateA = switcher->getSwitchState(pcduSwitches::ACS_BOARD_SIDE_A);
  ReturnValue_t switchStateB = switcher->getSwitchState(pcduSwitches::ACS_BOARD_SIDE_B);
  switch (submode) {
    case (A_SIDE): {
      if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_OFF) {
        state = States::MODE_COMMANDING;
        return;
      }
      break;
    }
    case (B_SIDE): {
      if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_ON) {
        state = States::MODE_COMMANDING;
        return;
      }
      break;
    }
    case (DUAL_MODE): {
      if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
        state = States::MODE_COMMANDING;
        return;
      }
    }
  }
  if (state == States::IDLE) {
    switch (submode) {
      case (A_SIDE): {
        if (switchStateA != PowerSwitchIF::SWITCH_ON) {
          // Set A side on first in power switcher IF
          switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_A, true);
        }
        if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
          switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_B, false);
        }
        break;
      }
      case (B_SIDE): {
        if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
          // Set A side on first in power switcher IF
          switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_A, false);
        }
        if (switchStateB != PowerSwitchIF::SWITCH_ON) {
          switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_B, true);
        }
        break;
      }
      case (DUAL_MODE): {
        if (switchStateA != PowerSwitchIF::SWITCH_ON) {
          // Set A side on first in power switcher IF
          switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_A, true);
        }
        if (switchStateB != PowerSwitchIF::SWITCH_ON) {
          switcher->sendSwitchCommand(pcduSwitches::ACS_BOARD_SIDE_B, true);
        }
        break;
      }
    }
    state = States::SWITCHING_POWER;
  }
  if (state == States::SWITCHING_POWER) {
    // TODO: Could check for a timeout (temporal or cycles) here and resend command
  }
}