#include "payloadModeTree.h"

#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/modes/HasModesIF.h>
#include <fsfw/retval.h>
#include <fsfw/subsystem/Subsystem.h>

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

namespace {
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

Subsystem satsystem::pl::SUBSYSTEM = Subsystem(objects::PL_SUBSYSTEM, 12, 24);

const auto check = subsystem::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(payload::Mode::OFF, FixedArrayList<ModeListEntry, 3>());
auto PL_TABLE_OFF_TGT =
    std::make_pair((payload::Mode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
auto PL_TABLE_OFF_TRANS_0 =
    std::make_pair((payload::Mode::OFF << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto PL_TABLE_OFF_TRANS_1 =
    std::make_pair((payload::Mode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 1>());

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

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

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

auto PL_SEQUENCE_EARTH_OBSV =
    std::make_pair(payload::Mode::EARTH_OBSV, FixedArrayList<ModeListEntry, 3>());
auto PL_TABLE_EARTH_OBSV_TGT =
    std::make_pair((payload::Mode::EARTH_OBSV << 24) | 1, FixedArrayList<ModeListEntry, 5>());
auto PL_TABLE_EARTH_OBSV_TRANS_0 =
    std::make_pair((payload::Mode::EARTH_OBSV << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto PL_TABLE_EARTH_OBSV_TRANS_1 =
    std::make_pair((payload::Mode::EARTH_OBSV << 24) | 3, FixedArrayList<ModeListEntry, 2>());

auto PL_SEQUENCE_SCEX = std::make_pair(payload::Mode::SCEX, FixedArrayList<ModeListEntry, 2>());
auto PL_TABLE_SCEX_TGT =
    std::make_pair((payload::Mode::SCEX << 24) | 1, FixedArrayList<ModeListEntry, 1>());
auto PL_TABLE_SCEX_TRANS_0 =
    std::make_pair((payload::Mode::SCEX << 24) | 2, FixedArrayList<ModeListEntry, 1>());

void satsystem::pl::init() {
  ModeListEntry entry;
  initOffSequence(SUBSYSTEM, entry);
  initPlMpsocStreamSequence(SUBSYSTEM, entry);
  initPlCamStreamSequence(SUBSYSTEM, entry);
  initPlSpvSequence(SUBSYSTEM, entry);
  initEarthObsvSequence(SUBSYSTEM, entry);
  initScexSequence(SUBSYSTEM, entry);
  SUBSYSTEM.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);
    check(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);
    check(sequence.insert(eh), ctxc);
  };

  // Build OFF target. Is empty
  check(ss.addTable(TableEntry(PL_TABLE_OFF_TGT.first, &PL_TABLE_OFF_TGT.second)), ctxc);

  // Build OFF transition 0
  iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_OFF_TRANS_0.second);
  iht(objects::SCEX, OFF, 0, PL_TABLE_OFF_TRANS_0.second);
  iht(objects::PLPCDU_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS_0.second);
  iht(objects::PLOC_MPSOC_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS_0.second);
  check(ss.addTable(TableEntry(PL_TABLE_OFF_TRANS_0.first, &PL_TABLE_OFF_TRANS_0.second)), ctxc);

  // Build OFF transition 1
  iht(objects::PLOC_SUPERVISOR_HANDLER, OFF, 0, PL_TABLE_OFF_TRANS_1.second);
  check(ss.addTable(TableEntry(PL_TABLE_OFF_TRANS_1.first, &PL_TABLE_OFF_TRANS_1.second)), ctxc);

  // Build OFF sequence
  ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TGT.first, 0, false);
  ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TRANS_0.first, 0, false);
  ihs(PL_SEQUENCE_OFF.second, PL_TABLE_OFF_TRANS_1.first, 0, false);
  check(ss.addSequence(
            SequenceEntry(PL_SEQUENCE_OFF.first, &PL_SEQUENCE_OFF.second, PL_SEQUENCE_OFF.first)),
        ctxc);
}

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);
    check(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);
    check(sequence.insert(eh), ctxc);
  };

  // Build MPSoC stream target
  // Camera should always be off to prevent a conflict with the MPSoC streaming
  // PL PCDU must be on and in normal mode, but this is commanded separately because of the
  // number of commands invovled
  iht(objects::PLPCDU_HANDLER, NML, plpcdu::ALL_ON_SUBMODE, PL_TABLE_MPSOC_STREAM_TGT.second);
  iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_MPSOC_STREAM_TGT.second);
  iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TGT.second);
  iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TGT.second);
  check(ss.addTable(TableEntry(PL_TABLE_MPSOC_STREAM_TGT.first, &PL_TABLE_MPSOC_STREAM_TGT.second)),
        ctxc);

  // Build MPSoC stream transition 0
  iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  iht(objects::SCEX, OFF, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TRANS_0.second);
  check(ss.addTable(
            TableEntry(PL_TABLE_MPSOC_STREAM_TRANS_0.first, &PL_TABLE_MPSOC_STREAM_TRANS_0.second)),
        ctxc);

  // Build MPSoC stream transition 1
  iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_MPSOC_STREAM_TRANS_1.second);
  check(ss.addTable(
            TableEntry(PL_TABLE_MPSOC_STREAM_TRANS_1.first, &PL_TABLE_MPSOC_STREAM_TRANS_1.second)),
        ctxc);

  // 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);
  check(ss.addSequence(SequenceEntry(PL_SEQUENCE_MPSOC_STREAM.first,
                                     &PL_SEQUENCE_MPSOC_STREAM.second, PL_SEQUENCE_OFF.first)),
        ctxc);
}

void initPlCamStreamSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::payload::initPlCamStreamSequence";
  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);
    check(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);
    check(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);
  check(ss.addTable(TableEntry(PL_TABLE_CAM_STREAM_TGT.first, &PL_TABLE_CAM_STREAM_TGT.second)),
        ctxc);

  // Build CAM transition 0
  // PLOC is actively commanded off here
  iht(objects::PLOC_MPSOC_HANDLER, OFF, 0, PL_TABLE_CAM_STREAM_TRANS_0.second);
  iht(objects::CAM_SWITCHER, ON, 0, PL_TABLE_CAM_STREAM_TRANS_0.second);
  iht(objects::SCEX, OFF, 0, PL_TABLE_CAM_STREAM_TRANS_0.second);
  check(ss.addTable(
            TableEntry(PL_TABLE_CAM_STREAM_TRANS_0.first, &PL_TABLE_CAM_STREAM_TRANS_0.second)),
        ctxc);

  // Build CAM transition 1
  iht(objects::PLOC_SUPERVISOR_HANDLER, OFF, 0, PL_TABLE_CAM_STREAM_TRANS_1.second);
  check(ss.addTable(
            TableEntry(PL_TABLE_CAM_STREAM_TRANS_1.first, &PL_TABLE_CAM_STREAM_TRANS_1.second)),
        ctxc);

  // 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);
  check(ss.addSequence(SequenceEntry(PL_SEQUENCE_CAM_STREAM.first, &PL_SEQUENCE_CAM_STREAM.second,
                                     PL_SEQUENCE_OFF.first)),
        ctxc);
}

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);
    check(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);
    check(sequence.insert(eh), ctxc);
  };

  // Build Payload Supervisor Only target
  iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_SUPV_ONLY_TGT.second);
  check(ss.addTable(TableEntry(PL_TABLE_SUPV_ONLY_TGT.first, &PL_TABLE_SUPV_ONLY_TGT.second)),
        ctxc);

  // Build Payload Supervisor Only transition 0
  iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_SUPV_ONLY_TRANS_0.second);
  iht(objects::CAM_SWITCHER, OFF, 0, PL_TABLE_SUPV_ONLY_TRANS_0.second);
  check(
      ss.addTable(TableEntry(PL_TABLE_SUPV_ONLY_TRANS_0.first, &PL_TABLE_SUPV_ONLY_TRANS_0.second)),
      ctxc);

  // Build Payload Supervisor Only transition 1
  iht(objects::PLOC_MPSOC_HANDLER, OFF, 0, PL_TABLE_SUPV_ONLY_TRANS_1.second);
  check(
      ss.addTable(TableEntry(PL_TABLE_SUPV_ONLY_TRANS_1.first, &PL_TABLE_SUPV_ONLY_TRANS_1.second)),
      ctxc);

  // Build Payload Supervisor Only Sequence
  ihs(PL_SEQUENCE_SUPV_ONLY.second, PL_TABLE_SUPV_ONLY_TGT.first, 0, true);
  ihs(PL_SEQUENCE_SUPV_ONLY.second, PL_TABLE_SUPV_ONLY_TRANS_0.first, 0, true);
  ihs(PL_SEQUENCE_SUPV_ONLY.second, PL_TABLE_SUPV_ONLY_TRANS_1.first, 0, false);
  check(ss.addSequence(SequenceEntry(PL_SEQUENCE_SUPV_ONLY.first, &PL_SEQUENCE_SUPV_ONLY.second,
                                     PL_SEQUENCE_OFF.first)),
        ctxc);
}

void initEarthObsvSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::payload::initEarthObsvSequence";
  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);
    check(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);
    check(sequence.insert(eh), ctxc);
  };

  // Build Earth Observation target
  iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_EARTH_OBSV_TGT.second);
  iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_EARTH_OBSV_TGT.second);
  iht(objects::CAM_SWITCHER, ON, 0, PL_TABLE_EARTH_OBSV_TGT.second);
  iht(objects::PLPCDU_HANDLER, OFF, 0, PL_TABLE_EARTH_OBSV_TGT.second);
  check(ss.addTable(TableEntry(PL_TABLE_EARTH_OBSV_TGT.first, &PL_TABLE_EARTH_OBSV_TGT.second)),
        ctxc);

  // Build Earth Observation transition 0
  iht(objects::PLOC_SUPERVISOR_HANDLER, NML, 0, PL_TABLE_EARTH_OBSV_TRANS_0.second);
  iht(objects::CAM_SWITCHER, ON, 0, PL_TABLE_EARTH_OBSV_TRANS_0.second);
  iht(objects::PLPCDU_HANDLER, OFF, 0, PL_TABLE_EARTH_OBSV_TRANS_0.second);
  check(ss.addTable(
            TableEntry(PL_TABLE_EARTH_OBSV_TRANS_0.first, &PL_TABLE_EARTH_OBSV_TRANS_0.second)),
        ctxc);

  // Build Earth Observation transition 1
  iht(objects::PLOC_MPSOC_HANDLER, NML, 0, PL_TABLE_CAM_STREAM_TRANS_1.second);
  check(ss.addTable(
            TableEntry(PL_TABLE_EARTH_OBSV_TRANS_1.first, &PL_TABLE_EARTH_OBSV_TRANS_1.second)),
        ctxc);

  ihs(PL_SEQUENCE_EARTH_OBSV.second, PL_TABLE_EARTH_OBSV_TGT.first, 0, true);
  ihs(PL_SEQUENCE_EARTH_OBSV.second, PL_TABLE_EARTH_OBSV_TRANS_0.first, 0, true);
  ihs(PL_SEQUENCE_EARTH_OBSV.second, PL_TABLE_EARTH_OBSV_TRANS_1.first, 0, false);
  check(ss.addSequence(SequenceEntry(PL_SEQUENCE_EARTH_OBSV.first, &PL_SEQUENCE_EARTH_OBSV.second,
                                     PL_SEQUENCE_OFF.first)),
        ctxc);
}

void initScexSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::payload::initScexSequence";
  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);
    check(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);
    check(sequence.insert(eh), ctxc);
  };

  // Build SCEX target
  iht(objects::SCEX, NML, 0, PL_TABLE_SCEX_TGT.second);
  check(ss.addTable(TableEntry(PL_TABLE_SCEX_TGT.first, &PL_TABLE_SCEX_TGT.second)), ctxc);

  // Build SCEX transition 0
  iht(objects::SCEX, NML, 0, PL_TABLE_SCEX_TRANS_0.second);
  check(ss.addTable(TableEntry(PL_TABLE_SCEX_TRANS_0.first, &PL_TABLE_SCEX_TRANS_0.second)), ctxc);

  // Build SCEX sequence
  ihs(PL_SEQUENCE_SCEX.second, PL_TABLE_SCEX_TGT.first, 0, true);
  ihs(PL_SEQUENCE_SCEX.second, PL_TABLE_SCEX_TRANS_0.first, 0, false);
  check(ss.addSequence(
            SequenceEntry(PL_SEQUENCE_SCEX.first, &PL_SEQUENCE_SCEX.second, PL_SEQUENCE_OFF.first)),
        ctxc);
}

}  // namespace