#include "payloadModeTree.h"

#include <commonObjects.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/modes/HasModesIF.h>
#include <fsfw/returnvalues/HasReturnvaluesIF.h>
#include <fsfw/subsystem/Subsystem.h>

#include "mission/devices/devicedefinitions/payloadPcduDefinitions.h"
#include "mission/system/objects/PayloadSubsystem.h"
#include "mission/system/objects/definitions.h"

namespace {
void checkInsert(ReturnValue_t result, const char* ctx);
void initOffSequence(Subsystem* ss, ModeListEntry& eh);
void initPlMpsocStreamSequence(Subsystem* ss, ModeListEntry& eh);
void initPlCamStreamSequence(Subsystem* ss, ModeListEntry& eh);
void initPlSpvSequence(Subsystem* ss, ModeListEntry& eh);
void initEarthObsvSequence(Subsystem* ss, ModeListEntry& eh);
void initScexSequence(Subsystem* ss, ModeListEntry& eh);
}  // namespace

const auto CHK = checkInsert;
static const auto OFF = HasModesIF::MODE_OFF;
static const auto ON = HasModesIF::MODE_ON;
static const auto NML = DeviceHandlerIF::MODE_NORMAL;

auto PL_SEQUENCE_OFF = std::make_pair(OFF << 24, FixedArrayList<ModeListEntry, 2>());
auto PL_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList<ModeListEntry, 0>());
auto PL_TABLE_OFF_TRANS = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 5>());

auto PL_SEQUENCE_MPSOC_STREAM =
    std::make_pair(payload::Modes::MPSOC_STREAM << 24, FixedArrayList<ModeListEntry, 2>());
auto PL_TABLE_MPSOC_STREAM_TGT =
    std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 1, FixedArrayList<ModeListEntry, 4>());
auto PL_TABLE_MPSOC_STREAM_TRANS_0 =
    std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto PL_TABLE_MPSOC_STREAM_TRANS_1 =
    std::make_pair((payload::Modes::MPSOC_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 1>());

auto PL_SEQUENCE_CAM_STREAM =
    std::make_pair(payload::Modes::CAM_STREAM << 24, FixedArrayList<ModeListEntry, 2>());
auto PL_TABLE_CAM_STREAM_TGT =
    std::make_pair((payload::Modes::CAM_STREAM << 24) | 1, FixedArrayList<ModeListEntry, 1>());
auto PL_TABLE_CAM_STREAM_TRANS_0 =
    std::make_pair((payload::Modes::CAM_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto PL_TABLE_CAM_STREAM_TRANS_1 =
    std::make_pair((payload::Modes::CAM_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 1>());

auto PL_SEQUENCE_SUPV_ONLY =
    std::make_pair(payload::Modes::CAM_STREAM << 24, FixedArrayList<ModeListEntry, 2>());
auto PL_TABLE_SUPV_ONLY_TGT =
    std::make_pair((payload::Modes::CAM_STREAM << 24) | 1, FixedArrayList<ModeListEntry, 5>());
auto PL_TABLE_SUPV_ONLY_TRANS_0 =
    std::make_pair((payload::Modes::CAM_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto PL_TABLE_SUPV_ONLY_TRANS_1 =
    std::make_pair((payload::Modes::CAM_STREAM << 24) | 2, FixedArrayList<ModeListEntry, 5>());

void satsystem::pl::init() {
  ModeListEntry entry;
  Subsystem* plSubsystem = new Subsystem(objects::PL_SUBSYSTEM, objects::EIVE_SYSTEM, 12, 24);
  initOffSequence(plSubsystem, entry);
  initPlMpsocStreamSequence(plSubsystem, entry);
  initPlCamStreamSequence(plSubsystem, entry);
  initPlSpvSequence(plSubsystem, entry);
  initEarthObsvSequence(plSubsystem, entry);
  initScexSequence(plSubsystem, entry);
  plSubsystem->setInitialMode(OFF);
}

namespace {

void initOffSequence(Subsystem* ss, ModeListEntry& eh) {
  std::string context = "satsystem::payload::buildOffSequence";
  auto ctxc = context.c_str();
  // Insert Helper Table
  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
                 ArrayList<ModeListEntry>& sequence) {
    eh.setObject(obj);
    eh.setMode(mode);
    eh.setSubmode(submode);
    CHK(sequence.insert(eh), ctxc);
  };
  // Insert Helper Sequence
  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
                 bool checkSuccess) {
    eh.setTableId(tableId);
    eh.setWaitSeconds(waitSeconds);
    eh.setCheckSuccess(checkSuccess);
    CHK(sequence.insert(eh), ctxc);
  };

  // Build OFF target. Is empty
  ss->addTable(&PL_TABLE_OFF_TGT.second, PL_TABLE_OFF_TGT.first, false, true);

  // Build OFF transition 0
  iht(objects::PLOC_SWITCHER, OFF, 0, PL_TABLE_OFF_TRANS.second);
  iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_OFF_TRANS.second);
  iht(objects::SCEX_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS.second);
  iht(objects::PLPCDU_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS.second);
  iht(objects::PLOC_SUBSYSTEM, OFF, 0, PL_TABLE_OFF_TRANS.second);
  ss->addTable(&PL_TABLE_OFF_TRANS.second, PL_TABLE_OFF_TRANS.first, false, true);

  // Build OFF sequence
  ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TGT.first, 0, false);
  ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TRANS.first, 0, false);
  ss->addSequence(&PL_SEQUENCE_OFF.second, PL_SEQUENCE_OFF.first, PL_SEQUENCE_OFF.first, false,
                  true);
}

void initPlMpsocStreamSequence(Subsystem* ss, ModeListEntry& eh) {
  std::string context = "satsystem::payload::initPlMpsocStreamSequence";
  auto ctxc = context.c_str();
  // Insert Helper Table
  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
                 ArrayList<ModeListEntry>& sequence) {
    eh.setObject(obj);
    eh.setMode(mode);
    eh.setSubmode(submode);
    CHK(sequence.insert(eh), ctxc);
  };
  // Insert Helper Sequence
  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
                 bool checkSuccess) {
    eh.setTableId(tableId);
    eh.setWaitSeconds(waitSeconds);
    eh.setCheckSuccess(checkSuccess);
    CHK(sequence.insert(eh), ctxc);
  };

  // Build MPSoC stream target
  // Camera should always be off to prevent a conflict with the MPSoC streaming
  iht(objects::PLPCDU_HANDLER, NML, plpcdu::ALL_ON_SUBMODE, PL_TABLE_MPSOC_STREAM_TGT.second);
  iht(objects::PLOC_SWITCHER, ON, 0, PL_TABLE_MPSOC_STREAM_TGT.second);
  iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_MPSOC_STREAM_TGT.second);
  iht(objects::PLOC_SUBSYSTEM, NML, payload::ploc::MPSOC_ON, PL_TABLE_MPSOC_STREAM_TGT.second);
  ss->addTable(&PL_TABLE_MPSOC_STREAM_TGT.second, PL_TABLE_MPSOC_STREAM_TGT.first, false, true);

  // Build MPSoC stream transition 0
  iht(objects::PLOC_SWITCHER, ON, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  iht(objects::SCEX_HANDLER, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  ss->addTable(&PL_TABLE_MPSOC_STREAM_TRANS_0.second, PL_TABLE_MPSOC_STREAM_TRANS_0.first, false,
               true);

  // Build MPSoC stream transition 1
  iht(objects::PLOC_SUBSYSTEM, NML, payload::ploc::MPSOC_ON, PL_TABLE_MPSOC_STREAM_TRANS_1.second);
  ss->addTable(&PL_TABLE_MPSOC_STREAM_TRANS_1.second, PL_TABLE_MPSOC_STREAM_TRANS_1.first, false,
               true);

  // Build MPSoC stream sequence
  ihs(PL_SEQUENCE_MPSOC_STREAM.second, PL_TABLE_MPSOC_STREAM_TGT.first, 0, true);
  ihs(PL_SEQUENCE_MPSOC_STREAM.second, PL_TABLE_MPSOC_STREAM_TRANS_0.first, 0, true);
  ihs(PL_SEQUENCE_MPSOC_STREAM.second, PL_TABLE_MPSOC_STREAM_TRANS_1.first, 0, false);
  ss->addSequence(&PL_SEQUENCE_MPSOC_STREAM.second, PL_SEQUENCE_MPSOC_STREAM.first,
                  PL_SEQUENCE_OFF.first, false, true);
}

void initPlCamStreamSequence(Subsystem* ss, ModeListEntry& eh) {
  std::string context = "satsystem::payload::initPlCamSequence";
  auto ctxc = context.c_str();
  // Insert Helper Table
  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
                 ArrayList<ModeListEntry>& sequence) {
    eh.setObject(obj);
    eh.setMode(mode);
    eh.setSubmode(submode);
    CHK(sequence.insert(eh), ctxc);
  };
  // Insert Helper Sequence
  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
                 bool checkSuccess) {
    eh.setTableId(tableId);
    eh.setWaitSeconds(waitSeconds);
    eh.setCheckSuccess(checkSuccess);
    CHK(sequence.insert(eh), ctxc);
  };

  // Build CAM target
  // Only check that the PL PCDU is on for now. It might later become necessary to switch on
  // the PLOC, so we ignore its state.
  iht(objects::PLPCDU_HANDLER, NML, plpcdu::ALL_ON_SUBMODE, PL_TABLE_CAM_STREAM_TGT.second);
  ss->addTable(&PL_TABLE_CAM_STREAM_TGT.second, PL_TABLE_CAM_STREAM_TGT.first, false, true);

  // Build CAM transition 0
  // PLOC is actively commanded off here
  iht(objects::PLOC_SUBSYSTEM, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  iht(objects::CAM_SWITCHER, ON, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  iht(objects::SCEX_HANDLER, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  ss->addTable(&PL_TABLE_MPSOC_STREAM_TRANS_0.second, PL_TABLE_MPSOC_STREAM_TRANS_0.first, false,
               true);

  // Build CAM transition 1
  iht(objects::PLOC_SWITCHER, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_1.second);
  ss->addTable(&PL_TABLE_MPSOC_STREAM_TRANS_1.second, PL_TABLE_MPSOC_STREAM_TRANS_1.first, false,
               true);

  // Build CAM stream sequence
  ihs(PL_SEQUENCE_CAM_STREAM.second, PL_TABLE_CAM_STREAM_TGT.first, 0, true);
  ihs(PL_SEQUENCE_CAM_STREAM.second, PL_TABLE_CAM_STREAM_TRANS_0.first, 0, true);
  ihs(PL_SEQUENCE_CAM_STREAM.second, PL_TABLE_CAM_STREAM_TRANS_1.first, 0, false);
  ss->addSequence(&PL_SEQUENCE_CAM_STREAM.second, PL_SEQUENCE_CAM_STREAM.first,
                  PL_SEQUENCE_OFF.first, false, true);
}

void initPlSpvSequence(Subsystem* ss, ModeListEntry& eh) {
  std::string context = "satsystem::payload::initPlSupvSequence";
  auto ctxc = context.c_str();
  // Insert Helper Table
  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
                 ArrayList<ModeListEntry>& sequence) {
    eh.setObject(obj);
    eh.setMode(mode);
    eh.setSubmode(submode);
    CHK(sequence.insert(eh), ctxc);
  };
  // Insert Helper Sequence
  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
                 bool checkSuccess) {
    eh.setTableId(tableId);
    eh.setWaitSeconds(waitSeconds);
    eh.setCheckSuccess(checkSuccess);
    CHK(sequence.insert(eh), ctxc);
  };

  // Build Payload Supervisor Only target
  iht(objects::PLOC_SWITCHER, ON, 0, PL_TABLE_SUPV_ONLY_TGT.second);
  iht(objects::PLOC_SUBSYSTEM, NML, payload::ploc::SUPV_ONLY, PL_TABLE_SUPV_ONLY_TGT.second);
  ss->addTable(&PL_TABLE_SUPV_ONLY_TGT.second, PL_TABLE_SUPV_ONLY_TGT.first, false, true);

  // Build Payload Supervisor Only transition 0
  iht(objects::PLOC_SWITCHER, ON, 0, PL_TABLE_SUPV_ONLY_TRANS_0.second);
  ss->addTable(&PL_TABLE_SUPV_ONLY_TRANS_0.second, PL_TABLE_SUPV_ONLY_TRANS_0.first, inStore,
               preInit)

      // Build Payload Supervisor Only transition 1
      iht(objects::PLOC_SUBSYSTEM, NML, payload::ploc::SUPV_ONLY,
          PL_TABLE_SUPV_ONLY_TRANS_1.second);
}

void initEarthObsvSequence(Subsystem* ss, ModeListEntry& eh) {}

void initScexSequence(Subsystem* ss, ModeListEntry& eh) {}

void checkInsert(ReturnValue_t result, const char* ctx) {
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::warning << "satsystem::checkInsert: PL | Insertion failed at " << ctx << std::endl;
  }
}

}  // namespace