#include <commonObjects.h>

#include "acsModeTree.h"
#include <fsfw/subsystem/modes/ModeDefinitions.h>
#include <fsfw/subsystem/Subsystem.h>
#include <fsfw/modes/HasModesIF.h>

FixedArrayList<ModeListEntry, 2> ACS_SEQUENCE_OFF = FixedArrayList<ModeListEntry, 2>();
FixedArrayList<ModeListEntry, 0> ACS_TABLE_OFF_TGT = FixedArrayList<ModeListEntry, 0>();
FixedArrayList<ModeListEntry, 6> ACS_TABLE_OFF_TRANS = FixedArrayList<ModeListEntry, 6>();

static constexpr Mode_t ACS_SEQUENCE_OFF_ID = HasModesIF::MODE_OFF;
static constexpr Mode_t ACS_TABLE_OFF_TGT_ID = 0x61000000;
static constexpr Mode_t ACS_TABLE_OFF_TRANS_ID = 0x61000001;

void satsystem::acsSubsystemInit() {
  ModeListEntry entry;
  Subsystem *acsSubsystem = new Subsystem(objects::ACS_SUBSYSTEM, objects::EIVE_SYSTEM, 20, 20);
  buildOffSequence(acsSubsystem, entry);
}

void satsystem::buildOffSequence(Subsystem* ss, ModeListEntry& eh) {
  // Build OFF target
  ss->addTable(&ACS_TABLE_OFF_TGT, ACS_TABLE_OFF_TGT_ID, false, true);

  // Build OFF transition
  eh.setObject(objects::ACS_CONTROLLER);
  eh.setMode(HasModesIF::MODE_OFF);
  eh.setSubmode(0);
  ACS_TABLE_OFF_TRANS.insert(eh);

  eh.setObject(objects::IMTQ_HANDLER);
  eh.setMode(HasModesIF::MODE_OFF);
  eh.setSubmode(0);
  ACS_TABLE_OFF_TRANS.insert(eh);

  eh.setObject(objects::STAR_TRACKER);
  eh.setMode(HasModesIF::MODE_OFF);
  eh.setSubmode(0);
  ACS_TABLE_OFF_TRANS.insert(eh);

  eh.setObject(objects::ACS_BOARD_ASS);
  eh.setMode(HasModesIF::MODE_OFF);
  eh.setSubmode(0);
  ACS_TABLE_OFF_TRANS.insert(eh);

  eh.setObject(objects::SUS_BOARD_ASS);
  eh.setMode(HasModesIF::MODE_OFF);
  eh.setSubmode(0);
  ACS_TABLE_OFF_TRANS.insert(eh);

  eh.setObject(objects::RW_ASS);
  eh.setMode(HasModesIF::MODE_OFF);
  eh.setSubmode(0);
  ACS_TABLE_OFF_TRANS.insert(eh);

  ss->addTable(&ACS_TABLE_OFF_TRANS, ACS_TABLE_OFF_TRANS_ID, false, true);

  // Build OFF sequence
  eh.setTableId(ACS_TABLE_OFF_TGT_ID);
  eh.setWaitSeconds(0);
  eh.setCheckSuccess(false);
  ACS_SEQUENCE_OFF.insert(eh);

  eh.setTableId(ACS_TABLE_OFF_TRANS_ID);
  eh.setWaitSeconds(0);
  eh.setCheckSuccess(false);
  ACS_SEQUENCE_OFF.insert(eh);

  ss->addSequence(&ACS_SEQUENCE_OFF, ACS_SEQUENCE_OFF_ID, ACS_SEQUENCE_OFF_ID, false, true);
  ss->setInitialMode(HasModesIF::MODE_OFF);
}