Index: firmware/App/Modes/ModePostTreat.c
===================================================================
diff -u -r38e8e85e6add1c4f798af6ada4a45cc7db547f3e -r94a95302c5d48c3632a7c2136d35c257fe64a0bf
--- firmware/App/Modes/ModePostTreat.c	(.../ModePostTreat.c)	(revision 38e8e85e6add1c4f798af6ada4a45cc7db547f3e)
+++ firmware/App/Modes/ModePostTreat.c	(.../ModePostTreat.c)	(revision 94a95302c5d48c3632a7c2136d35c257fe64a0bf)
@@ -51,8 +51,10 @@
 /// Post-Treatment drain reservoirs state machine.
 typedef enum Drain_States
 {
-    DRAIN_RESERVOIR_ONE_STATE = 0,         ///< Drain reservoir one state.
-    DRAIN_RESERVOIR_TWO_STATE,             ///< Drain reservoir two state.
+    DRAIN_FIRST_RESERVOIR_START_STATE = 0,         ///< Drain first reservoir state.
+    DRAIN_FIRST_RESERVOIR_STATE,         ///< Drain first reservoir state.
+    DRAIN_SWITCH_RESERVOIR_STATE,
+    DRAIN_SECOND_RESERVOIR_STATE,             ///< Drain reservoir two state.
     DRAIN_COMPLETE_STATE,                  ///< Drain complete state.
     NUM_OF_DRAIN_STATES                    ///< Number of drain states.
 } DRAIN_STATE_T;
@@ -77,10 +79,10 @@
 static HD_POST_TREATMENT_STATE_T handlePostTreatmentVerifyState( void );
 
 static void execDrainReservoirs( void );
-static BOOL switchReservoirCompleted( DG_RESERVOIR_ID_T activeRes );
-static BOOL startDrainCompleted( void );
-static DRAIN_STATE_T handleDrainReservoirOneState( void );
-static DRAIN_STATE_T handleDrainReservoirTwoState( void );
+static DRAIN_STATE_T handleDrainFirstReservoirStartState( void );
+static DRAIN_STATE_T handleDrainFirstReservoirState( void );
+static DRAIN_STATE_T handleDrainSwitchReservoirState( void );
+static DRAIN_STATE_T handleDrainSecondReservoirState( void );
 
 /*********************************************************************//**
  * @brief
@@ -95,7 +97,7 @@
     disposableRemovalConfirmed = FALSE;
     isDrainStarted = FALSE;
     currentPostTreatmentState = HD_POST_TREATMENT_PATIENT_DISCONNECTION_STATE;
-    currentDrainReservoirState = DRAIN_RESERVOIR_ONE_STATE;
+    currentDrainReservoirState = DRAIN_FIRST_RESERVOIR_START_STATE;
     postTreatmentPublishTimerCounter = 0;
 
     memset( &treatmentLogData, 0x0, sizeof( TREATMENT_LOG_DATA_PAYLOAD_T ) );
@@ -328,10 +330,15 @@
 {
     BOOL const isReservoirOneEmpty = fabs( getReservoirWeightLargeFilter( DG_RESERVOIR_1 ) ) <= LOAD_CELL_VOLUME_NOISE_TOLERANCE_GRAMS;
     BOOL const isReservoirTwoEmpty = fabs( getReservoirWeightLargeFilter( DG_RESERVOIR_2 ) ) <= LOAD_CELL_VOLUME_NOISE_TOLERANCE_GRAMS;
+#ifndef SKIP_CARTRIDGE_REMOVAL
+    BOOL isCartridgeRemoved = isCartridgeUnloaded();
+#else
+    BOOL isCartridgeRemoved = TRUE;
+#endif
 
     if ( STATE_CLOSED == getFPGADoorState() )
     {
-        if ( FALSE == isCartridgeUnloaded() )
+        if ( FALSE == isCartridgeRemoved )
         {
             activateAlarmNoData( ALARM_ID_CARTRIDGE_REMOVAL_FAILURE );
         }
@@ -342,7 +349,7 @@
         }
 
         if ( ( TRUE == isReservoirOneEmpty ) && ( TRUE == isReservoirTwoEmpty ) &&
-             ( TRUE == isCartridgeUnloaded() ) && ( FALSE == isSyringeDetected() ) )
+             ( TRUE == isCartridgeRemoved ) && ( FALSE == isSyringeDetected() ) )
         {
             cmdStopDG();
             requestNewOperationMode( MODE_STAN );
@@ -367,14 +374,22 @@
 {
     switch ( currentDrainReservoirState )
     {
-        case DRAIN_RESERVOIR_ONE_STATE:
-            currentDrainReservoirState = handleDrainReservoirOneState();
+        case DRAIN_FIRST_RESERVOIR_START_STATE:
+            currentDrainReservoirState = handleDrainFirstReservoirStartState();
             break;
 
-        case DRAIN_RESERVOIR_TWO_STATE:
-            currentDrainReservoirState = handleDrainReservoirTwoState();
+        case DRAIN_FIRST_RESERVOIR_STATE:
+            currentDrainReservoirState = handleDrainFirstReservoirState();
             break;
 
+        case DRAIN_SWITCH_RESERVOIR_STATE:
+            currentDrainReservoirState = handleDrainSwitchReservoirState();
+            break;
+
+        case DRAIN_SECOND_RESERVOIR_STATE:
+            currentDrainReservoirState = handleDrainSecondReservoirState();
+            break;
+
         case DRAIN_COMPLETE_STATE:
             break;
 
@@ -385,94 +400,64 @@
 
 /*********************************************************************//**
  * @brief
- * The switchReservoirCompleted function waits for switch reservoir response
- * from DG and re-send switch reservoir command if DG rejected previous one.
+ * The handleDrainReservoirOneState function sends command to DG to
+ * drain reservoir one.
  * @details Inputs: none
- * @details Outputs: processed DG switch reservoir command response
- * @return TRUE if switch reservoir completed, otherwise FALSE
+ * @details Outputs: drained reservoir one
+ * @return current state (sub-mode)
  *************************************************************************/
-static BOOL switchReservoirCompleted( DG_RESERVOIR_ID_T activeRes )
+static DRAIN_STATE_T handleDrainFirstReservoirStartState( void )
 {
-    BOOL result = FALSE;
-    DG_CMD_RESPONSE_T dgCmdResp;
+    DRAIN_STATE_T state = DRAIN_FIRST_RESERVOIR_START_STATE;
 
-    if ( TRUE == getDGCommandResponse( DG_CMD_SWITCH_RESERVOIR, &dgCmdResp ) )
+    if ( ( DG_MODE_CIRC == getDGOpMode() ) && ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == getDGSubMode() ) )
     {
-        if ( TRUE == dgCmdResp.rejected )
-        {
-            cmdSetDGActiveReservoir( activeRes );
-        }
-        else
-        {
-            result = TRUE;
-        }
+        state = DRAIN_FIRST_RESERVOIR_STATE;
+        cmdStartDGDrain( EMPTY_RESERVOIR_VOLUME_ML, FALSE );
     }
 
-    return result;
+    return state;
 }
 
-/*********************************************************************//**
- * @brief
- * The startDrainCompleted function waits for drain command response from DG
- * and re-send drain command if DG rejected previous one.
- * @details Inputs: none
- * @details Outputs: processed DG drain command response
- * @return TRUE if start reservoir drain completed, otherwise FALSE
- *************************************************************************/
-static BOOL startDrainCompleted( void )
+static DRAIN_STATE_T handleDrainFirstReservoirState( void )
 {
-    BOOL result = FALSE;
     DG_CMD_RESPONSE_T dgCmdResp;
+    DRAIN_STATE_T state = DRAIN_FIRST_RESERVOIR_STATE;
 
-    if ( TRUE == getDGCommandResponse( DG_CMD_START_DRAIN, &dgCmdResp ) )
+    if ( ( TRUE == getDGCommandResponse( DG_CMD_START_DRAIN, &dgCmdResp ) ) && ( DG_CMD_REQUEST_REJECT_REASON_INVALID_MODE == dgCmdResp.rejectCode ) )
     {
-        if ( TRUE == dgCmdResp.rejected )
+        state = DRAIN_FIRST_RESERVOIR_START_STATE;
+    }
+
+    if ( DG_MODE_DRAI == getDGOpMode() )
+    {
+        isDrainStarted = TRUE;
+    }
+
+    // Drain has started and DG goes to re-circ mode means drain completed
+    if ( TRUE == isDrainStarted )
+    {
+        if ( ( DG_MODE_CIRC == getDGOpMode() ) && ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == getDGSubMode() ) )
         {
-            cmdStartDGDrain( EMPTY_RESERVOIR_VOLUME_ML, FALSE );
+            isDrainStarted = FALSE;
+            state = DRAIN_SWITCH_RESERVOIR_STATE;
+            cmdSetDGActiveReservoir( getDGInactiveReservoir() );
         }
-        else
-        {
-            result = TRUE;
-        }
     }
 
-    return result;
+    return state;
 }
 
-/*********************************************************************//**
- * @brief
- * The handleDrainReservoirOneState function sends command to DG to
- * drain reservoir one.
- * @details Inputs: none
- * @details Outputs: drained reservoir one
- * @return current state (sub-mode)
- *************************************************************************/
-static DRAIN_STATE_T handleDrainReservoirOneState( void )
+static DRAIN_STATE_T handleDrainSwitchReservoirState( void )
 {
-    DRAIN_STATE_T state = DRAIN_RESERVOIR_ONE_STATE;
+    DRAIN_STATE_T state = DRAIN_SWITCH_RESERVOIR_STATE;
 
-    if ( ( DG_MODE_CIRC == getDGOpMode() ) && ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == getDGSubMode() ) )
+    if ( TRUE == hasDGCompletedReservoirSwitch() )
     {
-        cmdSetDGActiveReservoir( DG_RESERVOIR_2 );
-    }
-
-    if ( TRUE == switchReservoirCompleted( DG_RESERVOIR_2 ) )
-    {
+        state = DRAIN_SECOND_RESERVOIR_STATE;
         cmdStartDGDrain( EMPTY_RESERVOIR_VOLUME_ML, FALSE );
     }
 
-    if ( TRUE == startDrainCompleted() )
-    {
-        isDrainStarted = TRUE;
-    }
-
-    // Drain has started and DG goes to re-circ mode means drain completed
-    if ( ( TRUE == isDrainStarted ) && ( DG_MODE_CIRC == getDGOpMode() ) )
-    {
-        isDrainStarted = FALSE;
-        state = DRAIN_RESERVOIR_TWO_STATE;
-    }
-
     return state;
 }
 
@@ -484,30 +469,29 @@
  * @details Outputs: drained reservoir two
  * @return current state (sub-mode)
  *************************************************************************/
-static DRAIN_STATE_T handleDrainReservoirTwoState( void )
+static DRAIN_STATE_T handleDrainSecondReservoirState( void )
 {
-    DRAIN_STATE_T state = DRAIN_RESERVOIR_TWO_STATE;
+    DG_CMD_RESPONSE_T dgCmdResp;
+    DRAIN_STATE_T state = DRAIN_SECOND_RESERVOIR_STATE;
 
-    if ( ( DG_MODE_CIRC == getDGOpMode() ) && ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == getDGSubMode() ) )
+    if ( ( TRUE == getDGCommandResponse( DG_CMD_START_DRAIN, &dgCmdResp ) ) && ( DG_CMD_REQUEST_REJECT_REASON_INVALID_MODE == dgCmdResp.rejectCode ) )
     {
-        cmdSetDGActiveReservoir( DG_RESERVOIR_1 );
-    }
-
-    if ( TRUE == switchReservoirCompleted( DG_RESERVOIR_1 ) )
-    {
         cmdStartDGDrain( EMPTY_RESERVOIR_VOLUME_ML, FALSE );
     }
 
-    if ( TRUE == startDrainCompleted() )
+    if ( DG_MODE_DRAI == getDGOpMode() )
     {
         isDrainStarted = TRUE;
     }
 
     // Drain has started and DG goes to re-circ mode means drain completed
-    if ( ( TRUE == isDrainStarted ) && ( DG_MODE_CIRC == getDGOpMode() ) )
+    if ( TRUE == isDrainStarted )
     {
-        isDrainStarted = FALSE;
-        state = DRAIN_COMPLETE_STATE;
+        if ( ( DG_MODE_CIRC == getDGOpMode() ) && ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == getDGSubMode() ) )
+        {
+            isDrainStarted = FALSE;
+            state = DRAIN_COMPLETE_STATE;
+        }
     }
 
     return state;