From d08546e5fe7abdc8400b7d8586656f142b57641a Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 14 Nov 2022 17:17:34 +0100
Subject: [PATCH] added TCS subsystem normal sequence

---
 mission/system/tree/tcsModeTree.cpp | 57 +++++++++++++++++++++++++----
 1 file changed, 49 insertions(+), 8 deletions(-)

diff --git a/mission/system/tree/tcsModeTree.cpp b/mission/system/tree/tcsModeTree.cpp
index 6af8129b..e06def5d 100644
--- a/mission/system/tree/tcsModeTree.cpp
+++ b/mission/system/tree/tcsModeTree.cpp
@@ -18,14 +18,14 @@ static const auto OFF = HasModesIF::MODE_OFF;
 static const auto NML = DeviceHandlerIF::MODE_NORMAL;
 
 auto TCS_SEQUENCE_OFF = std::make_pair(OFF << 24, FixedArrayList<ModeListEntry, 3>());
-auto TCS_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
+auto TCS_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList<ModeListEntry, 2>());
 auto TCS_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 7>());
-auto TCS_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 1>());
+auto TCS_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 2>());
 
-auto TCS_SEQUENCE_NORMAL = std::make_pair(NML << 24, FixedArrayList<ModeListEntry, 2>());
-auto TCS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeListEntry, 4>());
-auto TCS_TABLE_NORMAL_TRANS_0 = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 4>());
-auto TCS_TABLE_NORMAL_TRANS_1 = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 4>());
+auto TCS_SEQUENCE_NORMAL = std::make_pair(NML << 24, FixedArrayList<ModeListEntry, 3>());
+auto TCS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeListEntry, 2>());
+auto TCS_TABLE_NORMAL_TRANS_0 = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 7>());
+auto TCS_TABLE_NORMAL_TRANS_1 = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 2>());
 
 void satsystem::tcs::init() {
   ModeListEntry entry;
@@ -77,6 +77,47 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 }
 
-void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) {}
-  // TODO: Implement
+void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) {
+  std::string context = "satsystem::tcs::buildNormalSequence";
+  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);
+  };
+
+  // OFF target table is empty
+  check(ss.addTable(&TCS_TABLE_NORMAL_TGT.second, TCS_TABLE_NORMAL_TGT.first, false, true), ctxc);
+
+  iht(objects::TCS_BOARD_ASS, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
+  iht(objects::TMP1075_HANDLER_TCS_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
+  iht(objects::TMP1075_HANDLER_TCS_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
+  iht(objects::TMP1075_HANDLER_PLPCDU_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
+  iht(objects::TMP1075_HANDLER_PLPCDU_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
+  iht(objects::TMP1075_HANDLER_IF_BOARD, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
+  check(ss.addTable(&TCS_TABLE_NORMAL_TRANS_0.second, TCS_TABLE_NORMAL_TRANS_0.first, false, true),
+        ctxc);
+
+  iht(objects::THERMAL_CONTROLLER, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
+  check(ss.addTable(&TCS_TABLE_NORMAL_TRANS_1.second, TCS_TABLE_NORMAL_TRANS_1.first, false, true),
+        ctxc);
+
+  ihs(TCS_SEQUENCE_NORMAL.second, TCS_TABLE_NORMAL_TGT.first, 0, false);
+  ihs(TCS_SEQUENCE_NORMAL.second, TCS_TABLE_NORMAL_TRANS_0.first, 0, false);
+  ihs(TCS_SEQUENCE_NORMAL.second, TCS_TABLE_NORMAL_TRANS_1.first, 0, false);
+  check(ss.addSequence(&TCS_SEQUENCE_NORMAL.second, TCS_SEQUENCE_NORMAL.first,
+                       TCS_SEQUENCE_NORMAL.first, false, true),
+        ctxc);
+}
+
 }  // namespace