From 73c594a930e342a1eab53d587fd54487f32158d5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Mar 2023 20:12:48 +0100
Subject: [PATCH] this is going to be tricky

---
 mission/system/objects/AcsBoardAssembly.cpp   | 31 +++++++++----------
 .../system/objects/DualLaneAssemblyBase.cpp   |  7 ++---
 mission/system/objects/DualLaneAssemblyBase.h |  9 ++++++
 3 files changed, 25 insertions(+), 22 deletions(-)

diff --git a/mission/system/objects/AcsBoardAssembly.cpp b/mission/system/objects/AcsBoardAssembly.cpp
index 01dd76b1..469ff4fb 100644
--- a/mission/system/objects/AcsBoardAssembly.cpp
+++ b/mission/system/objects/AcsBoardAssembly.cpp
@@ -104,28 +104,25 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
   using namespace duallane;
   ReturnValue_t result = returnvalue::OK;
   bool needsSecondStep = false;
+  if(sideSwitchTransition(mode, submode) and sideSwitchState == SideSwitchState::NONE) {
+    sideSwitchState = SideSwitchState::TO_DUAL;
+  }
+  // Switch to dual side first, and later switch back to the otherside
+  if(sideSwitchState == SideSwitchState::TO_DUAL) {
+    submode = Submodes::DUAL_MODE;
+    targetSubmodeForSideSwitch = static_cast<duallane::Submodes>(submode);
+    // TODO: Ugly hack. The base class should support arbitrary number of steps..
+    needsSecondStep = true;
+  } else if(sideSwitchState == SideSwitchState::DISABLE_OTHER_SIDE) {
+    submode = targetSubmodeForSideSwitch;
+  }
   auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
     if (mode == devMode) {
       modeTable[tableIdx].setMode(mode);
-    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
-      if (isUseable(objectId, devMode)) {
-        if (devMode == MODE_ON) {
-          modeTable[tableIdx].setMode(mode);
-          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-        } else {
-          modeTable[tableIdx].setMode(MODE_ON);
-          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-          if (internalState != STATE_SECOND_STEP) {
-            needsSecondStep = true;
-          }
-        }
-      }
-    } else if (mode == MODE_ON) {
-      if (isUseable(objectId, devMode)) {
-        modeTable[tableIdx].setMode(MODE_ON);
+    } else if (isUseable(objectId, devMode)) {
+        modeTable[tableIdx].setMode(mode);
         modeTable[tableIdx].setSubmode(SUBMODE_NONE);
       }
-    }
   };
   bool gpsUsable = isUseable(helper.gpsId, helper.gpsMode);
   switch (submode) {
diff --git a/mission/system/objects/DualLaneAssemblyBase.cpp b/mission/system/objects/DualLaneAssemblyBase.cpp
index a55553a0..d6a5ad2c 100644
--- a/mission/system/objects/DualLaneAssemblyBase.cpp
+++ b/mission/system/objects/DualLaneAssemblyBase.cpp
@@ -97,11 +97,7 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_
     return returnvalue::FAILED;
   }
   if (sideSwitchTransition(mode, submode)) {
-    // I could implement this but this would increase the already high complexity. This is not
-    // necessary. The operator should can send a command to switch the assembly off first and
-    // then send a command to turn on the other side, either to ON or to NORMAL
-    triggerEvent(SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID, 0, 0);
-    return TRANS_NOT_ALLOWED;
+    //inSideSwitchTransition = true;
   }
   return returnvalue::OK;
 }
@@ -225,6 +221,7 @@ void DualLaneAssemblyBase::finishModeOp() {
   pwrStateMachine.reset();
   powerRetryCounter = 0;
   tryingOtherSide = false;
+  sideSwitchState = SideSwitchState::NONE;
   dualModeErrorSwitch = true;
 }
 
diff --git a/mission/system/objects/DualLaneAssemblyBase.h b/mission/system/objects/DualLaneAssemblyBase.h
index eadfb77f..eecc6478 100644
--- a/mission/system/objects/DualLaneAssemblyBase.h
+++ b/mission/system/objects/DualLaneAssemblyBase.h
@@ -33,6 +33,15 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
   bool dualModeErrorSwitch = true;
   duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE;
 
+  enum SideSwitchState {
+    NONE,
+    TO_DUAL,
+    DISABLE_OTHER_SIDE
+  };
+
+  SideSwitchState sideSwitchState = SideSwitchState::NONE;
+  duallane::Submodes targetSubmodeForSideSwitch = duallane::Submodes::B_SIDE;
+
   enum RecoveryCustomStates {
     IDLE,
     POWER_SWITCHING_OFF,