#include "comModeTree.h"

#include "eive/objects.h"
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/modes/HasModesIF.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/subsystem/Subsystem.h>

#include "util.h"

const auto check = subsystem::checkInsert;

Subsystem satsystem::com::SUBSYSTEM = Subsystem(objects::COM_SUBSYSTEM, 12, 24);

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

auto COM_SEQUENCE_TX_OFF = std::make_pair(NML << 24, FixedArrayList<ModeListEntry, 2>());
auto COM_TABLE_TX_OFF_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeListEntry, 1>());
auto COM_TABLE_TX_OFF_TRANS = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 6>());

auto COM_SEQUENCE_TX_ON = std::make_pair(NML << 24, FixedArrayList<ModeListEntry, 2>());
auto COM_TABLE_TX_ON_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeListEntry, 1>());
auto COM_TABLE_TX_ON_TRANS = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 6>());

namespace {
void buildTxOffSequence(Subsystem& ss, ModeListEntry& eh);
void buildTxOnSequence(Subsystem& ss, ModeListEntry& eh);
}  // namespace

void satsystem::com::init() {
  ModeListEntry entry;
  buildTxOffSequence(SUBSYSTEM, entry);
  buildTxOnSequence(SUBSYSTEM, entry);
  SUBSYSTEM.setInitialMode(NML, 0 /* TODO: Which submode? */);
}

namespace {

void buildTxOffSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::com::buildTxOffSequence";
  auto ctxc = context.c_str();
  // Insert Helper Table
  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
    eh.setObject(obj);
    eh.setMode(mode);
    eh.setSubmode(submode);
    check(table.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 TX OFF table
  iht(objects::SYRLINKS_HK_HANDLER, NML, 0/* TODO */, COM_TABLE_TX_OFF_TGT.second);
  check(ss.addTable(TableEntry(COM_SEQUENCE_TX_OFF.first, &COM_TABLE_TX_OFF_TGT.second)), ctxc);

  // Build TX OFF transition
  iht(objects::SYRLINKS_HK_HANDLER, NML, 0/* TODO */, COM_TABLE_TX_OFF_TRANS.second);
  check(ss.addTable(TableEntry(COM_SEQUENCE_TX_OFF.first, &COM_TABLE_TX_OFF_TRANS.second)), ctxc);

  // Build TX OFF sequence
  ihs(COM_SEQUENCE_TX_OFF.second, COM_TABLE_TX_OFF_TGT.first, 0, false);
  ihs(COM_SEQUENCE_TX_OFF.second, COM_TABLE_TX_OFF_TRANS.first, 0, false);
  check(ss.addSequence(SequenceEntry(COM_TABLE_TX_OFF_TRANS.first, &COM_TABLE_TX_OFF_TRANS.second,
                                      COM_TABLE_TX_OFF_TRANS.first)),
        ctxc);
}

void buildTxOnSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::com::buildTxOnSequence";
  auto ctxc = context.c_str();
  // Insert Helper Table
  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
    eh.setObject(obj);
    eh.setMode(mode);
    eh.setSubmode(submode);
    check(table.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 TX ON table
  iht(objects::SYRLINKS_HK_HANDLER, NML, /*TODO*/0, COM_TABLE_TX_ON_TGT.second);
  check(ss.addTable(TableEntry(COM_SEQUENCE_TX_ON.first, &COM_TABLE_TX_ON_TGT.second)), ctxc);

  // Build TX ON transition
  iht(objects::SYRLINKS_HK_HANDLER, NML, /*TODO*/0, COM_TABLE_TX_ON_TRANS.second);
  check(ss.addTable(TableEntry(COM_SEQUENCE_TX_ON.first, &COM_TABLE_TX_ON_TRANS.second)), ctxc);

  // Build TX ON sequence
  ihs(COM_SEQUENCE_TX_ON.second, COM_TABLE_TX_ON_TGT.first, 0, false);
  ihs(COM_SEQUENCE_TX_ON.second, COM_TABLE_TX_ON_TRANS.first, 0, false);
  check(ss.addSequence(SequenceEntry(COM_TABLE_TX_ON_TRANS.first, &COM_TABLE_TX_ON_TRANS.second,
                                      COM_TABLE_TX_ON_TRANS.first)),
        ctxc);
}

}  // namespace