#include "comModeTree.h"

#include <fsfw/modes/HasModesIF.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/subsystem/Subsystem.h>

#include "eive/objects.h"
#include "mission/com/defs.h"
#include "mission/system/tree/util.h"

const auto check = subsystem::checkInsert;

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

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

auto COM_SEQUENCE_RX_ONLY =
    std::make_pair(::com::Submode::RX_ONLY, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TGT = std::make_pair(
    static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 1, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TRANS_0 = std::make_pair(
    static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TRANS_1 = std::make_pair(
    static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 3, FixedArrayList<ModeListEntry, 3>());

auto COM_SEQUENCE_RX_AND_TX_LOW_RATE =
    std::make_pair(::com::Submode::RX_AND_TX_LOW_DATARATE, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1,
                   FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2,
                   FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3,
                   FixedArrayList<ModeListEntry, 3>());

auto COM_SEQUENCE_RX_AND_TX_HIGH_RATE =
    std::make_pair(::com::Submode::RX_AND_TX_HIGH_DATARATE, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1,
                   FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2,
                   FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3,
                   FixedArrayList<ModeListEntry, 3>());

auto COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE =
    std::make_pair(::com::Submode::RX_AND_TX_DEFAULT_DATARATE, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1,
                   FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2,
                   FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 =
    std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3,
                   FixedArrayList<ModeListEntry, 3>());

namespace {

void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh);
void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh);
void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh);
void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh);

}  // namespace

Subsystem& satsystem::com::init() {
  ModeListEntry entry;
  buildRxOnlySequence(SUBSYSTEM, entry);
  buildTxAndRxLowRateSequence(SUBSYSTEM, entry);
  buildTxAndRxHighRateSequence(SUBSYSTEM, entry);
  buildTxAndRxDefaultRateSequence(SUBSYSTEM, entry);
  SUBSYSTEM.setInitialMode(COM_SEQUENCE_RX_ONLY.first);
  return SUBSYSTEM;
}

namespace {

void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::com::buildRxOnlySequence";
  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 RX Only table. We could track the state of the CCSDS IP core handler
  // as well but I do not think this is necessary because enabling that should
  // not interfere with the Syrlinks Handler.
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc);

  // Build RX Only transition 0
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TRANS_0.first, &COM_TABLE_RX_ONLY_TRANS_0.second)),
        ctxc);

  // Build RX Only transition 1
  iht(objects::CCSDS_HANDLER, OFF, 0, COM_TABLE_RX_ONLY_TRANS_1.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TRANS_1.first, &COM_TABLE_RX_ONLY_TRANS_1.second)),
        ctxc);

  // Build TX OFF sequence
  ihs(COM_SEQUENCE_RX_ONLY.second, COM_TABLE_RX_ONLY_TGT.first, 0, true);
  ihs(COM_SEQUENCE_RX_ONLY.second, COM_TABLE_RX_ONLY_TRANS_0.first, 0, false);
  ihs(COM_SEQUENCE_RX_ONLY.second, COM_TABLE_RX_ONLY_TRANS_1.first, 0, false);
  check(ss.addSequence(SequenceEntry(COM_SEQUENCE_RX_ONLY.first, &COM_SEQUENCE_RX_ONLY.second,
                                     COM_SEQUENCE_RX_ONLY.first)),
        ctxc);
}

void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::com::buildTxAndRxLowRateSequence";
  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 RX and TX low datarate table.
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
      COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
  iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_LOW),
      COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TGT.first,
                               &COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second)),
        ctxc);

  // Build TX and RX low datarate transition 0, switch CCSDS handler first
  iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_LOW),
      COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0.first,
                               &COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0.second)),
        ctxc);

  // Build TX and RX low transition 1
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
      COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.first,
                               &COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second)),
        ctxc);

  // Build TX and RX low datarate sequence
  ihs(COM_SEQUENCE_RX_AND_TX_LOW_RATE.second, COM_TABLE_RX_AND_TX_LOW_RATE_TGT.first, 0, true);
  ihs(COM_SEQUENCE_RX_AND_TX_LOW_RATE.second, COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0.first, 0, false);
  ihs(COM_SEQUENCE_RX_AND_TX_LOW_RATE.second, COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.first, 0, false);
  check(ss.addSequence(SequenceEntry(COM_SEQUENCE_RX_AND_TX_LOW_RATE.first,
                                     &COM_SEQUENCE_RX_AND_TX_LOW_RATE.second,
                                     COM_SEQUENCE_RX_ONLY.first)),
        ctxc);
}

void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::com::buildTxAndRxHighRateSequence";
  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 RX and TX high datarate table.
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
      COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
  iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_HIGH),
      COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.first,
                               &COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second)),
        ctxc);

  // Build TX and RX high datarate transition 0, switch CCSDS handler first
  iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_HIGH),
      COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0.first,
                               &COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0.second)),
        ctxc);

  // Build TX and RX high transition 1
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
      COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.first,
                               &COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second)),
        ctxc);

  // Build TX and RX low datarate sequence
  ihs(COM_SEQUENCE_RX_AND_TX_HIGH_RATE.second, COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.first, 0, true);
  ihs(COM_SEQUENCE_RX_AND_TX_HIGH_RATE.second, COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0.first, 0,
      false);
  ihs(COM_SEQUENCE_RX_AND_TX_HIGH_RATE.second, COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.first, 0,
      false);
  check(ss.addSequence(SequenceEntry(COM_SEQUENCE_RX_AND_TX_HIGH_RATE.first,
                                     &COM_SEQUENCE_RX_AND_TX_HIGH_RATE.second,
                                     COM_SEQUENCE_RX_ONLY.first)),
        ctxc);
}

void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) {
  std::string context = "satsystem::com::buildTxAndRxDefaultRateSequence";
  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 RX and TX default datarate table.
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
      COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
  iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_DEFAULT),
      COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.first,
                               &COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second)),
        ctxc);

  // Build TX and RX low datarate transition 0, switch CCSDS handler first
  iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_DEFAULT),
      COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0.first,
                               &COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0.second)),
        ctxc);

  // Build TX and RX default transition 1
  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
      COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second);
  check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.first,
                               &COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second)),
        ctxc);

  // Build TX and RX default datarate sequence
  ihs(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.second, COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.first, 0,
      true);
  ihs(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.second, COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0.first, 0,
      false);
  ihs(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.second, COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.first, 0,
      false);
  check(ss.addSequence(SequenceEntry(COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.first,
                                     &COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE.second,
                                     COM_SEQUENCE_RX_ONLY.first)),
        ctxc);
}

}  // namespace