Index: firmware/App/Modes/ModePreTreat.c
===================================================================
diff -u -rb8d74fc5b07d0e62d841b4c5a786b2be4e593c63 -rba7a576375ad7bbfa0d5a879d82b8783e5182899
--- firmware/App/Modes/ModePreTreat.c	(.../ModePreTreat.c)	(revision b8d74fc5b07d0e62d841b4c5a786b2be4e593c63)
+++ firmware/App/Modes/ModePreTreat.c	(.../ModePreTreat.c)	(revision ba7a576375ad7bbfa0d5a879d82b8783e5182899)
@@ -52,15 +52,15 @@
 /// States of the pre-treatment reservoir management state machine.
 typedef enum PreTreatmentReservoirMgmt_States
 {
-    PRE_TREATMENT_RESERVOIR_MGMT_START_STATE = 0,                       ///< Wait for signal to start drain and fill reservoirs.
-    PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_CMD_STATE,            ///< Command DG to switch active reservoir.
-    PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_RESP_STATE,           ///< Processing DG switch reservoir command response.
-    PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_STATE,                       ///< Command DG to start draining reservoir.
-    PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_RESP_STATE,                  ///< After sending drain command, process DG drain command response.
-    PRE_TREATMENT_RESERVOIR_MGMT_START_FILL_STATE,                      ///< Command DG to start filling reservoir.
-    PRE_TREATMENT_RESERVOIR_MGMT_FILL_CMD_RESP_STATE,                   ///< After sending fill command, process DG fill command response.
-    PRE_TREATMENT_RESERVOIR_MGMT_FILL_COMPLETE_STATE,                   ///< Reservoir fill has completed.
-    NUM_OF_PRE_TREATMENT_RESERVOIR_MGMT_STATES                          ///< Number of pre-treatments reservoir mgmt. states.
+    PRE_TREATMENT_RESERVOIR_MGMT_START_STATE = 0,                       ///< Wait for signal to start drain and fill reservoirs
+    PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_STATE,                       ///< Command DG to start draining reservoir
+    PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_RESP_STATE,                  ///< After sending drain command, process DG drain command response
+    PRE_TREATMENT_RESERVOIR_MGMT_START_FILL_STATE,                      ///< Command DG to start filling reservoir
+    PRE_TREATMENT_RESERVOIR_MGMT_FILL_CMD_RESP_STATE,                   ///< After sending fill command, process DG fill command response
+    PRE_TREATMENT_RESERVOIR_MGMT_FILL_COMPLETE_STATE,                   ///< Reservoir fill has completed
+    PRE_TREATMENT_RESERVOIR_MGMT_WAIT_FOR_RESERVOIR_SWITCH_STATE,       ///< Wait for prime operation to switch reservoir
+    PRE_TREATMENT_RESERVOIR_MGMT_COMPLETE_STATE,                        ///< Pre-treatment reservoir management complete state
+    NUM_OF_PRE_TREATMENT_RESERVOIR_MGMT_STATES                          ///< Number of pre-treatments reservoir mgmt. states
 } PRE_TREATMENT_RESERVOIR_MGMT_STATE_T;
 
 // ********** private data **********
@@ -99,13 +99,12 @@
 
 static void execPreTreatmentReservoirMgmt( void );
 static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtStartState( void );
-static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtSwitchReservoirCmdState( void );
-static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtSwitchReservoirRespState( void );
 static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtDrainCmdState( void );
 static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtDrainCmdRespState( void );
 static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtFillCmdState( void );
 static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtFillCmdRespState( void );
 static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtFillCompleteState( void );
+static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtWaitReservoirSwitchState( void );
 
 /*********************************************************************//**
  * @brief
@@ -762,14 +761,6 @@
             currentReservoirMgmtState = handlePreTreatmentReservoirMgmtStartState();
             break;
 
-        case PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_CMD_STATE:
-            currentReservoirMgmtState = handlePreTreatmentReservoirMgmtSwitchReservoirCmdState();
-            break;
-
-        case PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_RESP_STATE:
-            currentReservoirMgmtState = handlePreTreatmentReservoirMgmtSwitchReservoirRespState();
-            break;
-
         case PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_STATE:
             currentReservoirMgmtState = handlePreTreatmentReservoirMgmtDrainCmdState();
             break;
@@ -790,6 +781,13 @@
             currentReservoirMgmtState = handlePreTreatmentReservoirMgmtFillCompleteState();
             break;
 
+        case PRE_TREATMENT_RESERVOIR_MGMT_WAIT_FOR_RESERVOIR_SWITCH_STATE:
+            currentReservoirMgmtState = handlePreTreatmentReservoirMgmtWaitReservoirSwitchState();
+            break;
+
+        case PRE_TREATMENT_RESERVOIR_MGMT_COMPLETE_STATE:
+            break;
+
         default:
             currentReservoirMgmtState = PRE_TREATMENT_RESERVOIR_MGMT_START_STATE;
             SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_MODE_PRIME_RESERVOIR_MGMT_INVALID_STATE, (U32)currentReservoirMgmtState );
@@ -812,59 +810,10 @@
     if ( TRUE == fillReservoirOneStartRequested )
     {
         fillReservoirOneStartRequested = FALSE;
-        state = PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_CMD_STATE;
-    }
-
-    return state;
-}
-
-/*********************************************************************//**
- * @brief
- * The handlePreTreatmentReservoirMgmtSwitchReservoirRespState function sends
- * switch reservoir command to DG.
- * @details Inputs: reservoirFilledStatus[]
- * @details Outputs: sent switch reservoir command to DG
- * @return next state of pre-treatment reservoir management
- *************************************************************************/
-static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtSwitchReservoirCmdState( void )
-{
-    if ( FALSE == reservoirFilledStatus[ DG_RESERVOIR_1 ] )
-    {
+        state = PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_STATE;
         cmdSetDGActiveReservoir( DG_RESERVOIR_2 );
     }
-    else
-    {
-        cmdSetDGActiveReservoir( DG_RESERVOIR_1 );
-    }
 
-    return PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_RESP_STATE;
-}
-
-/*********************************************************************//**
- * @brief
- * The handlePreTreatmentReservoirMgmtSwitchReservoirRespState function waits
- * and processes DG switch reservoir command response.
- * @details Inputs: DG switch reservoir command response
- * @details Outputs: processed switch reservoir response
- * @return next state of pre-treatment reservoir management
- *************************************************************************/
-static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtSwitchReservoirRespState( void )
-{
-    PRE_TREATMENT_RESERVOIR_MGMT_STATE_T state = PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_RESP_STATE;
-    DG_CMD_RESPONSE_T dgCmdResp;
-
-    if ( TRUE == getDGCommandResponse( DG_CMD_SWITCH_RESERVOIR, &dgCmdResp ) )
-    {
-        if ( TRUE == dgCmdResp.rejected )
-        {
-            state = PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_CMD_STATE;
-        }
-        else
-        {
-            state = PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_STATE;
-        }
-    }
-
     return state;
 }
 
@@ -882,18 +831,20 @@
     DG_OP_MODE_T dgOpMode = getDGOpMode();
     U32 dgSubMode = getDGSubMode();
 
-    if ( DG_MODE_CIRC == dgOpMode )
+    // If DG has not started yet, start DG
+    if ( DG_MODE_STAN == dgOpMode )
     {
-        if ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == dgSubMode )
+        cmdStartDG();
+    }
+
+    if ( TRUE == hasDGCompletedReservoirSwitch() )
+    {
+        if ( ( DG_MODE_CIRC == dgOpMode ) && ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == dgSubMode ) )
         {
             state = PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_RESP_STATE;
             cmdStartDGDrain( DRAIN_RESERVOIR_TO_VOLUME_ML, TRUE );
         }
     }
-    else if ( DG_MODE_STAN == dgOpMode )
-    {
-        cmdStartDG();
-    }
 
     return state;
 }
@@ -998,21 +949,44 @@
 {
     PRE_TREATMENT_RESERVOIR_MGMT_STATE_T state = PRE_TREATMENT_RESERVOIR_MGMT_FILL_COMPLETE_STATE;
     DG_OP_MODE_T dgOpMode = getDGOpMode();
+    U32 dgSubMode = getDGSubMode();
 
-    if ( DG_MODE_CIRC == dgOpMode )
+    if ( ( DG_MODE_CIRC == dgOpMode ) && ( DG_RECIRCULATE_MODE_STATE_RECIRC_WATER == dgSubMode ) )
     {
         if ( FALSE == reservoirFilledStatus[ DG_RESERVOIR_1 ] )
         {
-            state = PRE_TREATMENT_RESERVOIR_MGMT_SWITCH_RESERVOIR_CMD_STATE;
+            state = PRE_TREATMENT_RESERVOIR_MGMT_WAIT_FOR_RESERVOIR_SWITCH_STATE;
             reservoirFilledStatus[ DG_RESERVOIR_1 ] = TRUE;
         }
         else if ( ( TRUE == reservoirFilledStatus[ DG_RESERVOIR_1 ] ) && ( FALSE == reservoirFilledStatus[ DG_RESERVOIR_2 ] ) )
         {
             reservoirFilledStatus[ DG_RESERVOIR_2 ] = TRUE;
+            state = PRE_TREATMENT_RESERVOIR_MGMT_COMPLETE_STATE;
         }
     }
 
     return state;
 }
 
+
+/*********************************************************************//**
+ * @brief
+ * The handlePreTreatmentReservoirMgmtWaitReservoirSwitchState function waits
+ * until prime operation switches the active reservoir before filling up next reservoir.
+ * @details Inputs: DG intactive reservoir
+ * @details Outputs: start filling next reservoir
+ * @return current state of pre-treatment reservoir management
+ *************************************************************************/
+static PRE_TREATMENT_RESERVOIR_MGMT_STATE_T handlePreTreatmentReservoirMgmtWaitReservoirSwitchState( void )
+{
+    PRE_TREATMENT_RESERVOIR_MGMT_STATE_T state = PRE_TREATMENT_RESERVOIR_MGMT_WAIT_FOR_RESERVOIR_SWITCH_STATE;
+
+    if ( DG_RESERVOIR_2 == getDGInactiveReservoir() )
+    {
+        state = PRE_TREATMENT_RESERVOIR_MGMT_DRAIN_CMD_STATE;
+    }
+
+    return state;
+}
+
 /**@}*/