/**************************************************************************
*
* Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved.
*
* THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
* WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
*
* @file ModeHeatDisinfect.c
*
* @author (last) Quang Nguyen
* @date (last) 01-Sep-2020
*
* @author (original) Sean
* @date (original) 20-Apr-2020
*
***************************************************************************/

#include "ConductivitySensors.h"
#include "DrainPump.h"
#include "Heaters.h"
#include "LoadCell.h"
#include "ModeHeatDisinfect.h"
#include "OperationModes.h"
#include "Pressures.h"
#include "Reservoirs.h"
#include "ROPump.h"
#include "SystemCommMessages.h"
#include "TaskGeneral.h"
#include "TemperatureSensors.h"
#include "Timers.h"
#include "UVReactors.h"
#include "Valves.h"

/**
 *  @addtogroup DGHeatDisinfectMode
 *  @{
 */

// ********** private definitions **********

// General defines
#define MAX_ALLOWED_STATE_TRIALS                    1                                      ///< Max allowed trials on a state. This is general among all the states.
#define HEAT_DISINFECT_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )         ///< Mode Heat Disinfect data publish interval in counts.

// Start state defines
#define MIN_INLET_PRESSURE_PSI                   30.0                                      ///< Minimum water inlet pressure in psi.
#define MAX_START_STATE_TEMP_SENSORS_DIFF_C       1.0                                      ///< Max start state TDi and TRo difference tolerance in C.

// Drain R1 & R2 states defines
#define DRAIN_PUMP_TARGET_RPM                    1800                                      ///< Drain pump target RPM during drain.
#define RSRVRS_INITIAL_DRAIN_TIME_OUT_MS  ( 60 * MS_PER_SECOND )                           ///< Reservoirs 1 & 2 initial drain time out in milliseconds.
#define DRAIN_WEIGHT_UNCHANGE_TIMEOUT    ( 6 * MS_PER_SECOND )                             ///< Time period of unchanged weight during draining before timeout.

// Flush drain path state defines
#define FLUSH_DRAIN_WAIT_TIME_MS      ( 10 * MS_PER_SECOND )                                ///< Flush Drain path wait time in milliseconds. TODo original time was 60 seconds
#define MIN_INLET_TEMPERATURE_C                  15.0                                      ///< Minimum water inlet temperature in C.      TODO original temperature was 25 C
#define MIN_INLET_CONDUCTIVITY_US_PER_CM          0.0                                      ///< Minimum water inlet conductivity in uS/cm
#define MAX_INLET_CONDUCTIVITY_US_PER_CM       2000.0                                      ///< Maximum water inlet conductivity in us/cm

// Flush circulation path state defines
#define RO_PUMP_TARGET_FLUSH_FILL_FLOW_RATE_LPM   0.8                                      ///< RO pump target flow rate during flush/fill in L/min. TODO original flow was 0.8
#define MAX_RO_PUMP_FLUSH_FILL_PRESSURE_PSI       130                                      ///< Maximum RO pump pressure during flush/fill states in psi.
#define FLUSH_CICRCULATION_WAIT_TIME_MS   ( 30 * MS_PER_SECOND )                           ///< Flush/rinse circulation path wait time in milliseconds. TODO original time was 30 seconds
#define MAX_FLUSH_CIRC_TEMP_SENSOR_DIFF_C         50.0                                     ///< Maximum flush circulation temperature difference tolerance in C. TODO original difference was 3.0 degrees
#define NUM_OF_TEMP_SENSORS_TO_AVG                4.0                                      ///< Number of temperature sensors to average to check the difference.

// Flush and drain R1 and R2
#define RSRVRS_FULL_VOL_ML                     1750.0                                      ///< Reservoirs 1 & 2 full volume in mL. TODo original value was 1900
#define RSRVRS_PARTIAL_FILL_VOL_ML              500.0                                      ///< Reservoirs 1 & 2 partial volume in mL.
#define RSRVRS_FULL_STABLE_TIME_COUNT    ( ( 4 * MS_PER_SECOND ) / TASK_GENERAL_INTERVAL ) ///< Reservoirs 1 & 2 full stable time in counts.
#define RSRVRS_FILL_UP_TIMEOUT_MS        ( 5 * 60 * MS_PER_SECOND )                        ///< Reservoirs 1 & 2 full fill up timeout in ms. TODO original value was 5 mins
#define RSRVRS_500ML_FILL_UP_TIMEOUT_MS  ( 2 * 60 * MS_PER_SECOND )                        ///< Reservoirs 1 & 2 partial fill up timeout in ms.
#define RSRVRS_DRAIN_TIMEOUT_MS          ( 2 * 60 * MS_PER_SECOND )                        ///< Reservoirs 1 & 2 drain timeout in ms.

// Fill and heat water
#define HEAT_DISINFECT_TARGET_TEMPERATURE_C     85.0                                       ///< Heat disinfect target water temperature in C. TODO original temperature was 85.0
#define HEAT_DISINFECT_START_TEMPERATURE_C      81.0                                       ///< Heat disinfect minimum acceptable temperature in C. TODO original temperature was 81.0

// R1 to R2 & R2 to R1 heat disinfect circulation
#define HEAT_DISINFECT_TARGET_RO_FLOW_LPM        0.9                                       ///< Heat disinfect target RO flow rate in L/min. TODO original value was 0.8
#define HEAT_DISINFECT_MAX_RO_PRESSURE_PSI        30                                       ///< Heat disinfect maximum RO pressure in psi.
#define HEAT_DISINFECT_TARGET_DRAIN_PRES_PSI    10.0                                       ///< Heat disinfect target drain outlet pressure in psi.
#define HEAT_DISINFECT_TIME_MS              ( 5 * 60 * MS_PER_SECOND )                     ///< Heat disinfect time for each section in milliseconds.  TODO original time was 10 minutes
#define HEAT_DISINFECT_START_TEMP_TIMOUT_MS ( 4 * 60 * 60 * MS_PER_SECOND )                ///< Heat disinfect reaching to minimum temperature timeout in milliseconds. TODO figure out this timeout
#define RSRVRS_TARGET_VOL_OUT_TIMEOUT_MS    ( 0.5 * 60 * MS_PER_SECOND )                   ///< Reservoirs 1 & 2 maximum volume out of range timeout during heat disnfect. TODO change this to 5 seconds
#define RSRVRS_MAX_TARGET_VOL_CHANGE_ML        500.0                                       ///< Reservoirs 1 & 2 maximum allowed volume change when full during heat disinfect. TODO original value is 100 mL
#define POST_HEAT_DISINFECT_WAIT_TIME_MS    ( 3 * 60 * MS_PER_SECOND )                     ///< Heat disinfect final wait time before flushing the system in milliseconds.

// Mix drain R1 and R2
#define RSRVRS_MIX_DRAIN_TIMEOUT_MS           ( 20 * 60 * MS_PER_SECOND )                  ///< Reservoirs 1 & 2 mix drain timeout in ms.
#define DRAIN_PUMP_START_TIME_IN_MIX_DRAIN_MS  ( 5 * MS_PER_SECOND )                       ///< Time to start the drain pump at mix drain after directing the flow to drain in ms.
#define DRAIN_PUMP_RPM_IN_MIX_DRAIN            600                                         ///< The RPM that the drain pump should be run during mix drain.

// Rinse R1 to R2
#define ROF_MIN_LOW_PRESSURE_TEMPERATURE_C      45.0                                       ///< RO filter minimum temperature that the pressure must be no more than 30psi in C. TODO the actual value is 45.0

// Cancellation paths
#define MIX_DRAIN_TEMPERATURE_THRESHOLD_C       60.0                                       ///< Temperature threshold for performing mix drain or normal drain.

/// Cancellation paths
typedef enum Cancellation_modes
{
    CANCELLATION_MODE_NONE = 0,                                                            ///< Cancellation mode none.
    CANCELLATION_MODE_BASIC,                                                               ///< Cancellation mode basic.
    CANCELLATION_MODE_HOT,                                                                 ///< Cancellation mode hot.
    CANCELLATION_MODE_COLD,                                                                ///< Cancellation mode cold.
    NUM_OF_CANCELLATION_MODES                                                              ///< Number of cancellation modes.
} CANCELLATION_MODES_T;

/// Reservoirs status
typedef enum Reservoirs_status
{
    RESERVOIR_STATUS_EMPTY = 0,                                                            ///< Reservoir empty.
    RESERVOIR_STATUS_FULL,                                                                 ///< Reservoir full.
    RESERVOIR_STATUS_UNKNOWN,                                                              ///< Reservoir timeout.
    NUM_OF_RESERVOIR_STATUS                                                                ///< Number of reservoirs status.
} RESERVOIRS_STATUS_T;

/// Heat disinfect status
typedef enum Heat_disinfect_status
{
    HEAT_DISINFECT_IN_PROGRESS = 0,                                                        ///< Heat disinfect in progress.
    HEAT_DISINFECT_RSRVRS_LEAK_TIMEOUT,                                                    ///< Heat disinfect reservoirs leak timeout.
    HEAT_DISINFECT_HEAT_UP_TIMEOUT,                                                        ///< Heat disinfect heat up timeout.
    HEAT_DISINFECT_COMPLETE,                                                               ///< Heat disinfect complete.
    NUM_OF_HEAT_DISINFECT_STATUS                                                           ///< Number of heat disinfect status.
} HEAT_DISINFECT_STATUS_T;

// ********** private data **********

static DG_HEAT_DISINFECT_STATE_T heatDisinfectState = DG_HEAT_DISINFECT_STATE_START;       ///< Current active heat disinfect state.
static DG_HEAT_DISINFECT_STATE_T prevHeatDisinfectState = DG_HEAT_DISINFECT_STATE_START;   ///< Previous active heat disinfect state before alarm.
static U32 overallHeatDisinfectTimer = 0;                                                  ///< Heat disinfect cycle total timer.
static U32 stateTimer = 0;                                                                 ///< Heat disinfect state timer to be used in different states.
static U32 stateTrialCounter = 0;                                                          ///< Heat disinfect state trial counter to be used for retries in different states.
static BOOL areTempSensorsInRange = FALSE;                                                 ///< Heat disinfect temperature sensors in/out range flag.
/// Boolean flag to check whether draining R1 and R2 is at the end of the heat disinfect cycle or in the beginning. So the drain states can be reused.
static BOOL isThisLastDrain = FALSE;
static RESERVOIRS_STATUS_T rsrvr1Status = RESERVOIR_STATUS_UNKNOWN;                        ///< Reservoir 1 status.
static RESERVOIRS_STATUS_T rsrvr2Status = RESERVOIR_STATUS_UNKNOWN;                        ///< Reservoir 2 status.
static F32 R1HeatDisinfectVol = 0.0;                                                       ///< Reservoir 1 full volume during heat disinfect.
static F32 R2HeatDisinfectVol = 0.0;                                                       ///< Reservoir 2 full volume during heat disinfect.
static U32 heatDisinfectTimer = 0;                                                         ///< Heat disinfect timer.
static BOOL isPartialDisinfectInProgress = FALSE;                                          ///< Heat disinfect partial complete/in progess flag.
static U32 rsrvrsVolMonitorTimer = 0;                                                      ///< Reservoir 1 & 2 volume monitor timers during heat disinfect.
static BOOL areRsrvrsLeaking = FALSE;                                                      ///< Reservoir 1 & 2 leak check flag during heat disinfect.
static U32 dataPublishCounter = 0;                                                         ///< Heat Disinfect data publish counter.
static CANCELLATION_MODES_T cancellationMode = CANCELLATION_MODE_NONE;                     ///< Cancellation mode.
static U32 rsrvrFillStableTimeCounter = 0;                                                 ///< Reservoirs fill stable time counter.
static ALARM_ID_T alarm;                                                                   ///< Heat disinfect alarm to raise.
static BOOL isDrainPumpInMixDrainOn = FALSE;                                               ///< Flag to show the drain pump is on during mix drain.

// ********** private function prototypes **********

static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectStartState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDrainR1State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDrainR2State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushDrainState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushCirculationState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushR1AndR2State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushR2AndDrainR1State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushDrainR2State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushDrainR1State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFillWithWaterState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDisinfectR1ToR2State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFillR2WithHotWaterState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDisinfectR2ToR1State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCoolDownHeatersState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectMixDrainR1State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectMixDrainR2State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCoolDownROFilterState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectRinseR1ToR2State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectRinseR2ToR1AndDrainR1State( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectRinseCirculationState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCancelModeBasicPathState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCancelModeWaterPathState( void );
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCompleteState( void );

static void resetActuators( void );
static void setModeToFailed( void );
static RESERVOIRS_STATUS_T getRsrvrFillStatus( DG_RESERVOIR_ID_T r, F32 targetVol, U32 timeout );
static RESERVOIRS_STATUS_T getRsrvrDrainStatus( DG_RESERVOIR_ID_T r, U32 drainSteadyStateTimeout, U32 timeout );
static HEAT_DISINFECT_STATUS_T getHeatDisinfectStatus( void );
static void publishHeatDisinfectData( void );

/*********************************************************************//**
 * @brief
 * The initHeatDisinfectMode function initializes the heat disinfect mode
 * module.
 * @details Inputs: none
 * @details Outputs: heatDisinfectState, stateTimer, isThisLastDrain,
 * stateTrialCounter, areTempSensorsInRange, rsrvr1Status, rsrvr2Status,
 * R1HeatDisinfectVol, R2HeatDisinfectVol, overallHeatDisinfectTimer,
 * cancellationMode, rsrvrFillStableTimeCounter, isPartialDisinfectInProgress,
 * isDrainPumpOnInMixDrain
 * @return none
 *************************************************************************/
void initHeatDisinfectMode( void )
{
    heatDisinfectState           = DG_HEAT_DISINFECT_STATE_START;
    prevHeatDisinfectState       = DG_HEAT_DISINFECT_STATE_START;
    stateTimer                   = 0;
    isThisLastDrain              = FALSE;
    stateTrialCounter            = 0;
    areTempSensorsInRange        = FALSE;
    rsrvr1Status                 = RESERVOIR_STATUS_UNKNOWN;
    rsrvr2Status                 = RESERVOIR_STATUS_UNKNOWN;
    R1HeatDisinfectVol           = 0.0;
    R2HeatDisinfectVol           = 0.0;
    overallHeatDisinfectTimer    = 0;
    cancellationMode             = CANCELLATION_MODE_NONE;
    rsrvrFillStableTimeCounter   = 0;
    isPartialDisinfectInProgress = FALSE;
    isDrainPumpInMixDrainOn      = FALSE;
}

/*********************************************************************//**
 * @brief
 * The transitionToHeatDisinfectMode function prepares for transition to
 * heat disinfect mode.
 * @details Inputs: none
 * @details Outputs: none
 * @return none
 *************************************************************************/
void transitionToHeatDisinfectMode( void )
{
    initHeatDisinfectMode();
}

/*********************************************************************//**
 * @brief
 * The execHeatDisinfectMode function executes the heat disinfect mode
 * state machine.
 * @details Inputs: heatDisinfectState
 * @details Outputs: heatDisinfectState
 * @return current state
 *************************************************************************/
U32 execHeatDisinfectMode( void )
{
    switch ( heatDisinfectState )
    {
        case DG_HEAT_DISINFECT_STATE_START:
            heatDisinfectState = handleHeatDisinfectStartState();
            break;

        case DG_HEAT_DISINFECT_STATE_DRAIN_R1:
            heatDisinfectState = handleHeatDisinfectDrainR1State();
            break;

        case DG_HEAT_DISINFECT_STATE_DRAIN_R2:
            heatDisinfectState = handleHeatDisinfectDrainR2State();
            break;

        case DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN:
            heatDisinfectState = handleHeatDisinfectFlushDrainState();
            break;

        case DG_HEAT_DISINFECT_STATE_FLUSH_CIRCULATION:
            heatDisinfectState = handleHeatDisinfectFlushCirculationState();
            break;

        case DG_HEAT_DISINFECT_STATE_FLUSH_R1_AND_R2:
            heatDisinfectState = handleHeatDisinfectFlushR1AndR2State();
            break;

        case DG_HEAT_DISINFECT_STATE_FLUSH_R2_AND_DRAIN_R1:
            heatDisinfectState = handleHeatDisinfectFlushR2AndDrainR1State();
            break;

        case DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN_R2:
            heatDisinfectState = handleHeatDisinfectFlushDrainR2State();
            break;

        case DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN_R1:
            heatDisinfectState = handleHeatDisinfectFlushDrainR1State();
            break;

        case DG_HEAT_DISINFECT_STATE_FILL_WITH_WATER:
            heatDisinfectState = handleHeatDisinfectFillWithWaterState();
            break;

        case DG_HEAT_DISINFECT_STATE_DISINFECT_R1_TO_R2:
            heatDisinfectState = handleHeatDisinfectDisinfectR1ToR2State();
            break;

        case DG_HEAT_DISINFECT_STATE_FILL_R2_WITH_HOT_WATER:
            heatDisinfectState = handleHeatDisinfectFillR2WithHotWaterState();
            break;

        case DG_HEAT_DISINFECT_STATE_DISINFECT_R2_TO_R1:
            heatDisinfectState = handleHeatDisinfectDisinfectR2ToR1State();
            break;

        case DG_HEAT_DISINFECT_STATE_COOL_DOWN_HEATERS:
            heatDisinfectState = handleHeatDisinfectCoolDownHeatersState();
            break;

        case DG_HEAT_DISINFECT_STATE_MIX_DRAIN_R1:
            heatDisinfectState = handleHeatDisinfectMixDrainR1State();
            break;

        case DG_HEAT_DISINFECT_STATE_MIX_DRAIN_R2:
            heatDisinfectState = handleHeatDisinfectMixDrainR2State();
            break;

        case DG_HEAT_DISINFECT_STATE_COOL_DOWN_RO_FILTER:
            heatDisinfectState = handleHeatDisinfectCoolDownROFilterState();
            break;

        case DG_HEAT_DISINFECT_STATE_RINSE_R1_TO_R2:
            heatDisinfectState = handleHeatDisinfectRinseR1ToR2State();
            break;

        case DG_HEAT_DISINFECT_STATE_RINSE_R2_TO_R1_AND_DRAIN_R1:
            heatDisinfectState = handleHeatDisinfectRinseR2ToR1AndDrainR1State();
            break;

        case DG_HEAT_DISINFECT_STATE_RINSE_CIRCULATION:
            heatDisinfectState = handleHeatDisinfectRinseCirculationState();
            break;

        case DG_HEAT_DISINFECT_STATE_CANCEL_BASIC_PATH:
            heatDisinfectState = handleHeatDisinfectCancelModeBasicPathState();
            break;

        case DG_HEAT_DISINFECT_STATE_CANCEL_WATER_PATH:
            heatDisinfectState = handleHeatDisinfectCancelModeWaterPathState();
            break;

        case DG_HEAT_DISINFECT_STATE_COMPLETE:
            heatDisinfectState = handleHeatDisinfectCompleteState();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_HEAT_DISINFECT_INVALID_EXEC_STATE, heatDisinfectState )
            heatDisinfectState = DG_HEAT_DISINFECT_STATE_START;
            break;
    }

    publishHeatDisinfectData();

    return heatDisinfectState;
}

/*********************************************************************//**
 * @brief
 * The getCurrentHeatDisinfectState function returns the current state of
 * the heat disinfect mode.
 * @details Inputs: heatDisinfectState
 * @details Outputs: none
 * @return the current state of heat disinfect mode.
 *************************************************************************/
DG_HEAT_DISINFECT_STATE_T getCurrentHeatDisinfectState( void )
{
    return heatDisinfectState;
}

/*********************************************************************//**
 * @brief
 * The stopDGHeatDisinfect function stops heat disinfect mode.
 * @details Inputs: heatDisinfectionState
 * @details Outputs: heatDisinfectionState
 * @return none
 *************************************************************************/
void stopDGHeatDisinfect( void )
{
    // Reset all the actuators
    resetActuators();

    // Transition to mode standby
    requestNewOperationMode( DG_MODE_STAN );
}

// ********** private function prototypes **********

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectStartState function handles the heat disinfect
 * start state. The state checks the inlet pressure and the difference in
 * between TDi and TRo sensors and if they are not in range, it transitions
 * to basic cancellation path. Otherwise, it transitions to the next
 * state.
 * @details Inputs: alarm, rsrvrFillStableTimeCounter, rsrvr1Status,
 * stateTimer
 * @details Outputs: alarm, rsrvrFillStableTimeCounter, rsrvr1Status,
 * stateTimer
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectStartState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_DRAIN_R1;

    // Start overall heat disinfect timer
    overallHeatDisinfectTimer = getMSTimerCount();

    // Set all the actuators to reset and de-energized state
    resetActuators();

    // Reset the load cells lowest weight prior to starting the run
    resetReservoirsLowestWeight();

    F32 ppiPressure = getMeasuredDGPressure( PRESSURE_SENSOR_RO_PUMP_INLET );
    F32 TDiTemp     = getTemperatureValue( TEMPSENSORS_OUTLET_REDUNDANT );  //TODo change to TDi
    F32 TRoTemp     = getTemperatureValue( TEMPSENSORS_OUTLET_REDUNDANT );

    // If the inlet pressure is less than the threshold or TDi and TRo difference is greater than 1 C, the cycle
    // should be canceled
    if ( ppiPressure < MIN_INLET_PRESSURE_PSI && fabs( TDiTemp - TRoTemp ) > MAX_START_STATE_TEMP_SENSORS_DIFF_C )
    {
        prevHeatDisinfectState = state;
        alarm = ALARM_ID_DG_HEAT_DISINFECT_INLET_PRES_AND_TEMP_SNSRS_OUT;
        state = DG_HEAT_DISINFECT_STATE_CANCEL_BASIC_PATH;
    }
    else
    {
        // Close VPi to prevent wasting water
        setValveState( VPI, VALVE_STATE_CLOSED );
        // Request a tare for reservoir 1
        tareReservoir();
        // Set the actuators to drain R1
        setValveState( VRD, VALVE_STATE_R1_C_TO_NC );
        setDrainPumpTargetRPM( DRAIN_PUMP_TARGET_RPM );

        rsrvrFillStableTimeCounter = 0;
        // Assume reservoir 1 is full and drain it
        rsrvr1Status = RESERVOIR_STATUS_FULL;
        stateTimer   = getMSTimerCount();
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectDrainR1State function handles the heat disinfect
 * drain R1 state. The state drains reservoir 1. If the transition is
 * finished within the time, it transitions to the next state, otherwise,
 * it transitions to basic cancellation path.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status, isThisLastDrain
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDrainR1State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_DRAIN_R1;

    if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        if ( isThisLastDrain )
        {
            // Done with draining
            signalDrainPumpHardStop();

            // Set the valves to flush the recirculation line
            setValveState( VPI, VALVE_STATE_OPEN );
            setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
            setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO );
            setValveState( VDR, VALVE_STATE_DRAIN_C_TO_NO );
            setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO );

            // TODO turn on the concentrate pumps

            // Set the RO pump to run at full pressure
            setROPumpTargetFlowRate( RO_PUMP_TARGET_FLUSH_FILL_FLOW_RATE_LPM, MAX_RO_PUMP_FLUSH_FILL_PRESSURE_PSI );

            // Done with final draining
            isThisLastDrain = FALSE;
            state = DG_HEAT_DISINFECT_STATE_RINSE_CIRCULATION;
        }
        else
        {
            // Assume reservoir 2 is full and drain it
            rsrvr2Status = RESERVOIR_STATUS_FULL;
            // Set the actuators to drain R2.
            // Request a tare for reservoir 2
            tareReservoir();
            // NOTE: Drain pump is already on and VDr is already on drain state
            setValveState( VRD, VALVE_STATE_R2_C_TO_NO );
            state = DG_HEAT_DISINFECT_STATE_DRAIN_R2;
        }

        // Start the timer
        stateTimer = getMSTimerCount();
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectDrainR2State function handles the heat disinfect
 * drain R2 state. The state drains reservoir 2. If the transition is
 * finished within the time, it transitions to the next state, otherwise,
 * it transitions to basic cancellation path.
 * @details Inputs: stateTimer, rsrvr2Status, isThisLastDrain,
 * stateTrialCounter
 * @details Outputs: stateTimer, rsrvr2Status, stateTrialCounter
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDrainR2State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_DRAIN_R2;

    if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr2Status = getRsrvrDrainStatus( DG_RESERVOIR_2, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr2Status == RESERVOIR_STATUS_EMPTY )
    {
        if ( isThisLastDrain )
        {
            setValveState( VRD, VALVE_STATE_R1_C_TO_NC );
            rsrvr1Status = RESERVOIR_STATUS_FULL;
            state = DG_HEAT_DISINFECT_STATE_DRAIN_R1;
        }
        else
        {
            signalDrainPumpHardStop();
            setValveState( VPI, VALVE_STATE_OPEN );
            stateTrialCounter = 0;
            stateTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFlushDrainState function handles the heat disinfect
 * flush drain state. The state flushes the drain line for a period of time
 * and then measure the temperature and conductivity of water. If they are
 * not within the range, it transitions to basic cancellation path, otherwise
 * it transitions to the next state.
 * @details Inputs: stateTimer, stateTrialCounter, alarm,
 * prevHeatDisinfectState
 * @details Outputs: stateTimer, stateTrialCounter, alarm,
 * prevHeatDisinfectState
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushDrainState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN;

    // Check if flush time has elapsed
    if ( didTimeout( stateTimer, FLUSH_DRAIN_WAIT_TIME_MS ) )
    {
        // If the inlet temperature and conductivity are in range, move onto the next state
        if ( getTemperatureValue( TEMPSENSORS_INLET_PRIMARY_HEATER ) > MIN_INLET_TEMPERATURE_C &&
             getConductivityValue( CONDUCTIVITYSENSORS_CPI_SENSOR ) >  MIN_INLET_CONDUCTIVITY_US_PER_CM )
        {
            setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
            setROPumpTargetFlowRate( RO_PUMP_TARGET_FLUSH_FILL_FLOW_RATE_LPM, MAX_RO_PUMP_FLUSH_FILL_PRESSURE_PSI );
            stateTimer = getMSTimerCount();
            stateTrialCounter = 0;
            state = DG_HEAT_DISINFECT_STATE_FLUSH_CIRCULATION;
        }
        // If the failure is still in range, reset the timer and start over
        else if ( ++stateTrialCounter < MAX_ALLOWED_STATE_TRIALS )
        {
            stateTimer = getMSTimerCount();
        }
        // Couldn't get a good water sample after a couple of trials and the disinfect cycle failed
        else
        {
            alarm = ALARM_ID_DG_HEAT_DISINFECT_INLET_COND_AND_TEMP_OUT;
            prevHeatDisinfectState = state;
            state = DG_HEAT_DISINFECT_STATE_CANCEL_BASIC_PATH;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFlushCirculationState function handles the heat
 * disinfect flush circulation state. The state flushes the circulation
 * line for a period of time. After the flush if the temperature sensors
 * are not within a certain degrees from each other, the state transitions
 * to basic cancellation path, otherwise, it transitions to the next state.
 * @details Inputs: stateTimer, stateTrialCounter, prevHeatDisinfectState
 * alarm, areTempSensorsInRange, rsrvr1Status, rsrvr2Status
 * @details Outputs: stateTimer, stateTrialCounter, prevHeatDisinfectState,
 * alarm, areTempSensorsInRange, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushCirculationState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FLUSH_CIRCULATION;

    // Check if the flush circulation time has elapsed and the temperature sensors are not in range yet
    if ( didTimeout( stateTimer, FLUSH_CICRCULATION_WAIT_TIME_MS ) && FALSE == areTempSensorsInRange )
    {
        F32 ThdTemp = getTemperatureValue( TEMPSENSORS_OUTLET_REDUNDANT ); // TODO add THd later. This is the new temp sensor of the coldest spot.
        F32 TPoTemp = getTemperatureValue( TEMPSENSORS_OUTLET_PRIMARY_HEATER );
        F32 TD1Temp = getTemperatureValue( TEMPSENSORS_CONDUCTIVITY_SENSOR_1 );
        F32 TD2Temp = getTemperatureValue( TEMPSENSORS_CONDUCTIVITY_SENSOR_2 );
        F32 avgTemp = ( ThdTemp + TPoTemp + TD1Temp + TD2Temp ) / NUM_OF_TEMP_SENSORS_TO_AVG;

        BOOL isThdOut = fabs( ThdTemp - avgTemp ) > MAX_FLUSH_CIRC_TEMP_SENSOR_DIFF_C;
        BOOL isTPoOut = fabs( TPoTemp - avgTemp ) > MAX_FLUSH_CIRC_TEMP_SENSOR_DIFF_C;
        BOOL isTD1Out = fabs( TD1Temp - avgTemp ) > MAX_FLUSH_CIRC_TEMP_SENSOR_DIFF_C;
        BOOL isTD2Out = fabs( TD2Temp - avgTemp ) > MAX_FLUSH_CIRC_TEMP_SENSOR_DIFF_C;

        // Check if any of the temperature sensors are out of tolerance
        if( isThdOut || isTPoOut || isTD1Out || isTD2Out )
        {
            // Check if we have exceeded the number of trials. If not, try another time
            if ( ++stateTrialCounter < MAX_ALLOWED_STATE_TRIALS )
            {
                stateTimer = getMSTimerCount();
            }
            // State failed. Cancel heat disinfect mode
            else
            {
                alarm = ALARM_ID_DG_TEMP_SENSORS_DIFF_OUT_OF_RANGE;
                prevHeatDisinfectState = state;
                state = DG_HEAT_DISINFECT_STATE_CANCEL_BASIC_PATH;
            }
        }
        else
        {
            areTempSensorsInRange = TRUE;
            stateTimer = getMSTimerCount();
            // TODO Turn on the concentrate pumps and wait for 30 seconds
        }
    }

    // Only start the concentrate pumps if the temperature sensors are in range
    if ( areTempSensorsInRange )
    {
        // TODO: enable the timeout once the concentrate pumps are available.
        //if ( didTimeout( stateTimer, FLUSH_CICRCULATION_WAIT_TIME_MS ) )
        if ( TRUE )
        {
            rsrvr1Status = RESERVOIR_STATUS_EMPTY;
            rsrvr2Status = RESERVOIR_STATUS_EMPTY;

            setValveState( VPO, VALVE_STATE_FILL_C_TO_NC );
            setValveState( VRF, VALVE_STATE_R1_C_TO_NC );
            setValveState( VRI, VALVE_STATE_R2_C_TO_NC );
            stateTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_FLUSH_R1_AND_R2;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFlushR1AndR2State function handles the heat
 * disinfect flush reservoir 1 and reservoir 2 state. If the reservoirs
 * did not flush within a period time, the state transitions to water
 * cancellation path.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status,
 * prevHeatDisinfectState
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status,
 * prevHeatDisinfectState
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushR1AndR2State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FLUSH_R1_AND_R2;

    // If R1 is not full, keep monitoring for R1 level and timeout
    if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_FULL_VOL_ML, RSRVRS_FILL_UP_TIMEOUT_MS );
    }
    // Once R1 is full, keep monitoring for R2 level and timeout
    else if( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr2Status = getRsrvrFillStatus( DG_RESERVOIR_2, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_500ML_FILL_UP_TIMEOUT_MS );

        // Once R2 is full (to 500mL in this case), transition to the next state
        if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
        {
            // Set the actuators to flush R2 and drain R1 state
            setValveState( VRF, VALVE_STATE_R2_C_TO_NO );
            setValveState( VRI, VALVE_STATE_R1_C_TO_NO );
            setValveState( VRO, VALVE_STATE_R2_C_TO_NC );
            setValveState( VRD, VALVE_STATE_R1_C_TO_NC );
            setDrainPumpTargetRPM( DRAIN_PUMP_TARGET_RPM );
            stateTimer = getMSTimerCount();

            // Set both reservoirs status
            rsrvr1Status = RESERVOIR_STATUS_FULL;
            rsrvr2Status = RESERVOIR_STATUS_EMPTY;
            state = DG_HEAT_DISINFECT_STATE_FLUSH_R2_AND_DRAIN_R1;
        }
    }
    else if ( rsrvr1Status == RESERVOIR_STATUS_UNKNOWN || rsrvr2Status == RESERVOIR_STATUS_UNKNOWN )
    {
        prevHeatDisinfectState = state;
        state = DG_HEAT_DISINFECT_STATE_CANCEL_WATER_PATH;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFlushR2AndDrainR1State function handles the heat
 * disinfect flush reservoir 2 and drain reservoir 1 state. The state
 * drains reservoir 1 and flushes reservoir 2 at the same time until the
 * water in reservoir 2 overflows to reservoir 1. If this process is done
 * within a certain period of time, it transitions to the next state.
 * If the drain process times out, it transitions to basic cancellation and
 * if the flush process times out, it transitions to water cancellation.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushR2AndDrainR1State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FLUSH_R2_AND_DRAIN_R1;

    // If reservoir 1 is empty, turn off the drain pump
    if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        // Done with draining R1
        signalDrainPumpHardStop();
    }

    // First reservoir 2 must be completely full
    if ( rsrvr2Status == RESERVOIR_STATUS_EMPTY )
    {
        rsrvr2Status = getRsrvrFillStatus( DG_RESERVOIR_2, RSRVRS_FULL_VOL_ML, RSRVRS_FILL_UP_TIMEOUT_MS );
    }
    // Once R2 is full, R1 must be partially full
    else if( rsrvr2Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_500ML_FILL_UP_TIMEOUT_MS );

        // Once R1 is partially full, transition to the next state
        if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
        {
            // Done with filing turn off the RO pump
            signalROPumpHardStop();

            // Set the valves to drain R2 and no fill
            setValveState( VPI, VALVE_STATE_CLOSED );
            setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
            setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO );
            setValveState( VRO, VALVE_STATE_R1_C_TO_NO );
            setValveState( VRD, VALVE_STATE_R2_C_TO_NO );
            setDrainPumpTargetRPM( DRAIN_PUMP_TARGET_RPM );
            // Start the timer for drain timeout
            stateTimer = getMSTimerCount();
            rsrvr2Status = RESERVOIR_STATUS_FULL;
            state = DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN_R2;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFlushDrainR2State function handles the heat
 * disinfect flush drain reservoir 2 state. The state drains reservoir 2
 * and if the drain times out, it transitions to basic cancellation. If the
 * drain is finished within a certain period of time, it transitions to the
 * next state.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushDrainR2State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN_R2;

    // If reservoir 2 is empty, set the drain valve to drain R1
    if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr2Status = getRsrvrDrainStatus( DG_RESERVOIR_2, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr2Status == RESERVOIR_STATUS_EMPTY )
    {
        setValveState( VRD, VALVE_STATE_R1_C_TO_NC );
        // Start the timer for drain timeout
        stateTimer = getMSTimerCount();
        rsrvr1Status = RESERVOIR_STATUS_FULL;
        state = DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN_R1;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFlushDrainR1State function handles the heat
 * disinfect flush drain reservoir 1 state. The state drains reservoir 1
 * and if the drain times out, it transitions to basic cancellation. If the
 * drain is finished within a certain period of time, it transitions to the
 * next state.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFlushDrainR1State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FLUSH_DRAIN_R1;

    // If reservoir 1 is empty, set the state to fill water state
    if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        // Done with draining the reservoirs
        signalDrainPumpHardStop();

        // Prepare for filling the reservoirs and heating the water
        setValveState( VPI, VALVE_STATE_OPEN );
        setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
        setValveState( VPO, VALVE_STATE_FILL_C_TO_NC );
        setValveState( VRO, VALVE_STATE_R1_C_TO_NO );
        setValveState( VRI, VALVE_STATE_R2_C_TO_NC );
        setValveState( VRF, VALVE_STATE_R1_C_TO_NC );
        setValveState( VRD, VALVE_STATE_R2_C_TO_NO );

        // Turn on the RO pump
        setROPumpTargetFlowRate( RO_PUMP_TARGET_FLUSH_FILL_FLOW_RATE_LPM, MAX_RO_PUMP_FLUSH_FILL_PRESSURE_PSI );

        // Start heating the water while we are filling up the rsrvrs
        setPrimaryHeaterTargetTemperature( HEAT_DISINFECT_TARGET_TEMPERATURE_C );
        startPrimaryHeater();

        // Start the timer for drain timeout
        stateTimer = getMSTimerCount();
        state = DG_HEAT_DISINFECT_STATE_FILL_WITH_WATER;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFillWithWaterState function handles the heat
 * disinfect fill with water state. The state fills reservoir 1 until it
 * overflows to reservoir 2. If the filling process times out, it
 * transitions to water cancellation state, otherwise, it transitions to
 * next state. The levels of the reservoirs are recorded to be monitored
 * during heat disinfect.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status, R1HeatDisinfectVol
 * R2HeatDisinfectVol, rsrvrsVolMonitorTimer
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status R1HeatDisinfectVol
 * R2HeatDisinfectVol, rsrvrsVolMonitorTimer
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFillWithWaterState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FILL_WITH_WATER;

    // First reservoir 1 must be full
    if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_FULL_VOL_ML, RSRVRS_FILL_UP_TIMEOUT_MS );
    }
    // Once reservoir 1 is full, check the status of reservoir 2 since the water overflows to reservoir 2
    else if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr2Status = getRsrvrFillStatus( DG_RESERVOIR_2, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_500ML_FILL_UP_TIMEOUT_MS );

        // Once reservoir 2 is full, set the actuators for recirculation
        if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
        {
            // Set the valves to drain R2 and no fill
            setValveState( VPI, VALVE_STATE_CLOSED );
            setValveState( VBF, VALVE_STATE_OPEN );
            setValveState( VPD, VALVE_STATE_DRAIN_C_TO_NC );
            setValveState( VDR, VALVE_STATE_RECIRC_C_TO_NC );
            setValveState( VRC, VALVE_STATE_RECIRC_C_TO_NC );

            // Set the drain pump to control mode
            setDrainPumpTargetOutletPressure( HEAT_DISINFECT_TARGET_DRAIN_PRES_PSI );

            // Set the RO flow to maximum pressure of 25psi since it is the maximum pressure on the RO filter
            // at inlet temperature > 45 C
            setROPumpTargetFlowRate( HEAT_DISINFECT_TARGET_RO_FLOW_LPM, HEAT_DISINFECT_MAX_RO_PRESSURE_PSI );

            // Start the trimmer heater since we are recirculating water
            //setTrimmerHeaterTargetTemperature( HEAT_DISINFECT_TARGET_TEMPERATURE_C );
            //startTrimmerHeater(); TODO turn the trimmer heater on when THD is implemented

            // Get the current volumes of R1 & R2. These values will be used to make sure the reservoirs'
            // volume does not change more than a certain amount during the actual heat disinfect cycle
            R1HeatDisinfectVol = getLoadCellLargeFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY );
            R2HeatDisinfectVol = getLoadCellLargeFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY );

            stateTimer = getMSTimerCount();
            rsrvrsVolMonitorTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_DISINFECT_R1_TO_R2;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectDisinfectR1ToR2State function handles the heat
 * disinfect R1 to R2 state. The state runs reservoir 1 to reservoir 2
 * heat disinfect. If the reservoirs leak or it cannot reach to temperature
 * within a certain period of time, it transitions to water cancellation.
 * If heat disinfect reservoir 1 to reservoir 2 is completed, it transitions
 * to the next state.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDisinfectR1ToR2State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_DISINFECT_R1_TO_R2;
    HEAT_DISINFECT_STATUS_T status  = getHeatDisinfectStatus();

    switch ( status )
    {
        case HEAT_DISINFECT_RSRVRS_LEAK_TIMEOUT:
        case HEAT_DISINFECT_HEAT_UP_TIMEOUT:
            state = DG_HEAT_DISINFECT_STATE_CANCEL_WATER_PATH;
            break;

        case HEAT_DISINFECT_COMPLETE:

            //TODO turn off CP1 and CP2
            // Set the valves to transfer hot water from R1 to R2 and fill up R2.
            setValveState( VRO, VALVE_STATE_R2_C_TO_NC );
            setValveState( VRD, VALVE_STATE_R1_C_TO_NC );
            setValveState( VRI, VALVE_STATE_R1_C_TO_NO );
            setValveState( VRF, VALVE_STATE_R2_C_TO_NO );
            // Although there is fluid in both reservoirs, but they are set to empty
            // to begin the transition of hot water from R1 to R2.
            rsrvr2Status = RESERVOIR_STATUS_EMPTY;
            rsrvr1Status = RESERVOIR_STATUS_EMPTY;
            stateTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_FILL_R2_WITH_HOT_WATER;
            break;

        default:
            // Do nothing, heat disinfect in progress
            break;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectFillR2WithHotWaterState function handles fill R2
 * with water state. The state transfers hot water from reservoir 1 to
 * reservoir 2 until hot water overflows from reservoir 2 to reservoir 1.
 * If the fill times out, it transitions to water cancellation state,
 * otherwise, it transitions to the next state.
 * @details Inputs: rsrvr1Status, rsrvr2Status, R1HeatDisinfectVol,
 * R2HeatDisinfectVol
 * @details Outputs: rsrvr1Status, rsrvr2Status, R1HeatDisinfectVol,
 * R2HeatDisinfectVol
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectFillR2WithHotWaterState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_FILL_R2_WITH_HOT_WATER;

    // First reservoir 1 must be partially full
    if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_500ML_FILL_UP_TIMEOUT_MS );
    }
    else if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr2Status = getRsrvrFillStatus( DG_RESERVOIR_2, RSRVRS_FULL_VOL_ML, RSRVRS_FILL_UP_TIMEOUT_MS );

        if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
        {
            // Get the current volumes to be monitored during R2 to R1 heat disinfect state
            R1HeatDisinfectVol = getLoadCellLargeFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY );
            R2HeatDisinfectVol = getLoadCellLargeFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY );

            //TODO turn on CP1 and CP2

            state = DG_HEAT_DISINFECT_STATE_DISINFECT_R2_TO_R1;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectDisinfectR2ToR1State function handles the heat
 * disinfect R2 to R1 state. If the reservoirs leak or it cannot reach to
 * temperature within a certain period of time, it transitions to water
 * cancellation state. If heat disinfect reservoir 1 to reservoir 2 is
 * completed, it transitions to the next state.
 * @details Inputs: hasPostHeatDisinfectWaitStarted, stateTimer, rsrvr1Status
 * @details Outputs: hasPostHeatDisinfectWaitStarted, stateTimer, rsrvr1Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectDisinfectR2ToR1State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_DISINFECT_R2_TO_R1;
    HEAT_DISINFECT_STATUS_T status  = getHeatDisinfectStatus();

    switch ( status )
    {
        case HEAT_DISINFECT_RSRVRS_LEAK_TIMEOUT:
        case HEAT_DISINFECT_HEAT_UP_TIMEOUT:
            state = DG_HEAT_DISINFECT_STATE_CANCEL_WATER_PATH;
            break;

        case HEAT_DISINFECT_COMPLETE:
            // Turn off the pumps and heaters
            // TODO turn off CP1 and CP2
            stopPrimaryHeater();
            stopTrimmerHeater();
            stateTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_COOL_DOWN_HEATERS;
            break;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectCoolDownHeatersState function handles the heat
 * disinfect cool down heaters state. The state continues running the fluid
 * while the heaters are off for a certain period of time.
 * @details Inputs: stateTimer
 * @details Outputs: stateTimer
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCoolDownHeatersState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_COOL_DOWN_HEATERS;

    if ( didTimeout( stateTimer, POST_HEAT_DISINFECT_WAIT_TIME_MS ) )
    {
        // Stop the drain pump and the RO pump to exit the closed loop
        signalDrainPumpHardStop();
        signalROPumpHardStop();

        // De-energize all the valves that are not in the path anymore
        // and wait for the RO membrane to be cooled down.
        // In this state, VPi and VPd must still be kept energized to make sure fresh
        // water does not enter the circulation path the membrane is cooling down
        setValveState( VPI, VALVE_STATE_CLOSED );
        setValveState( VBF, VALVE_STATE_CLOSED );
        setValveState( VPD, VALVE_STATE_DRAIN_C_TO_NC );

        stateTimer = getMSTimerCount();
        state = DG_HEAT_DISINFECT_STATE_COOL_DOWN_RO_FILTER;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectCoolDownROFilterState function handles the heat
 * disinfect cool down RO filter state. The state monitors the temperature
 * at THd and if it is less than 45 C, it transitions to the next state.
 * @details Inputs: stateTimer, rsrvr1Status
 * @details Outputs: stateTimer, rsrvr1Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCoolDownROFilterState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_COOL_DOWN_RO_FILTER;

    F32 ThdTemp = getTemperatureValue( TEMPSENSORS_OUTLET_REDUNDANT ); //TODO change this to actual THd sensor later

    // Check if the coldest spot temperature is less than 45 C so the RO filter
    // can safely run fluid through
    if ( ThdTemp < ROF_MIN_LOW_PRESSURE_TEMPERATURE_C )
    {
        setValveState( VPI, VALVE_STATE_OPEN );
        setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO );
        setValveState( VDR, VALVE_STATE_DRAIN_C_TO_NO );
        setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO );
        setValveState( VRO, VALVE_STATE_R1_C_TO_NO );
        setValveState( VRI, VALVE_STATE_R1_C_TO_NO );
        setValveState( VRF, VALVE_STATE_R2_C_TO_NO );

        rsrvr1Status = RESERVOIR_STATUS_FULL;
        stateTimer = getMSTimerCount();
        state = DG_HEAT_DISINFECT_STATE_MIX_DRAIN_R1;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectMixDrainR1State function handles the heat
 * disinfect mix drain R1 state. The state drains reservoir 1 and if it
 * times out, it transitions to basic cancellation state. Otherwise, it
 * transitions to the next state.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status,
 * isDrainPumpOnInMixDrain
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status,
 * isDrainPumpOnInMixDrain
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectMixDrainR1State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_MIX_DRAIN_R1;

    if ( didTimeout( stateTimer, DRAIN_PUMP_START_TIME_IN_MIX_DRAIN_MS ) && isDrainPumpInMixDrainOn == FALSE )
    {
        isDrainPumpInMixDrainOn = TRUE;

        // Turn on the drain pump to drain the reservoirs in open loop mode
        //setDrainPumpTargetRPM( DRAIN_PUMP_RPM_IN_MIX_DRAIN ); TODO if this is not needed remove the #define
        setDrainPumpTargetOutletPressure( 5.0 ); //TODO if this is the method, add a #define
    }
    else if ( isDrainPumpInMixDrainOn == TRUE )
    {
        if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
        {
            rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_MIX_DRAIN_TIMEOUT_MS );
        }
        else if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
        {
            // Set the drain valve to reservoir 2
            setValveState( VRD, VALVE_STATE_R2_C_TO_NO );

            rsrvr2Status = RESERVOIR_STATUS_FULL;
            stateTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_MIX_DRAIN_R2;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectMixDrainR2State function handles the heat
 * disinfect mix drain R2 state. The state drains reservoir 1 and if it
 * times out, it transitions to basic cancellation state. Otherwise, it
 * transitions to the next state.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectMixDrainR2State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_MIX_DRAIN_R2;

    if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr2Status = getRsrvrDrainStatus( DG_RESERVOIR_2, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_MIX_DRAIN_TIMEOUT_MS );
    }
    else if ( rsrvr2Status == RESERVOIR_STATUS_EMPTY )
    {
        // Done with draining the reservoirs
        signalDrainPumpHardStop();

        // Set the valves to fill up R1 and overflow to R2
        setValveState( VPI, VALVE_STATE_OPEN );
        setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
        setValveState( VPO, VALVE_STATE_FILL_C_TO_NC );
        setValveState( VDR, VALVE_STATE_DRAIN_C_TO_NO );
        setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO );
        setValveState( VRF, VALVE_STATE_R1_C_TO_NC );
        setValveState( VRO, VALVE_STATE_R1_C_TO_NO );
        setValveState( VRI, VALVE_STATE_R2_C_TO_NC );
        setROPumpTargetFlowRate( RO_PUMP_TARGET_FLUSH_FILL_FLOW_RATE_LPM, MAX_RO_PUMP_FLUSH_FILL_PRESSURE_PSI );

        rsrvr1Status = RESERVOIR_STATUS_EMPTY;
        rsrvr2Status = RESERVOIR_STATUS_EMPTY;

        stateTimer = getMSTimerCount();
        state = DG_HEAT_DISINFECT_STATE_RINSE_R1_TO_R2;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectRinseR1ToR2State function handles the heat
 * disinfect rinse R1 to R2 state. The state rinses reservoir 1 to reservoir
 * 2. If the rinse process times out, it transitions to water cancellation
 * state, otherwise, it transitions to the next state.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectRinseR1ToR2State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_RINSE_R1_TO_R2;

    if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_FULL_VOL_ML, RSRVRS_FILL_UP_TIMEOUT_MS );
    }
    else if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr2Status = getRsrvrFillStatus( DG_RESERVOIR_2, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_500ML_FILL_UP_TIMEOUT_MS );

        if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
        {
            // Set the valves to rinse R2 to R1 and drain R1
            setValveState( VRF, VALVE_STATE_R2_C_TO_NO );
            setValveState( VRO, VALVE_STATE_R2_C_TO_NC );
            setValveState( VRD, VALVE_STATE_R1_C_TO_NC );
            setValveState( VRI, VALVE_STATE_R1_C_TO_NO );
            setDrainPumpTargetRPM( DRAIN_PUMP_TARGET_RPM );

            rsrvr1Status = RESERVOIR_STATUS_FULL;
            rsrvr2Status = RESERVOIR_STATUS_EMPTY;
            stateTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_RINSE_R2_TO_R1_AND_DRAIN_R1;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectRinseR2ToR1AndDrainR1State function handles the
 * heat disinfect rinse R2 to R1 and drain R1 state. The state rinses
 * reservoir 2 and drains reservoir 1 at the same time. If the drain
 * process times out, it transitions to basic cancellation state, and
 * if the rinse times out, it transitions to water cancellation state.
 * If the drain and rinse are completed within the define time, it
 * transitions to the next state.
 * @details Inputs: stateTimer, rsrvr1Status, rsrvr2Status, isThisLastDrain
 * @details Outputs: stateTimer, rsrvr1Status, rsrvr2Status, isThisLastDrain
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectRinseR2ToR1AndDrainR1State( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_RINSE_R2_TO_R1_AND_DRAIN_R1;

    if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
    {
        // Done with draining R1
        signalDrainPumpHardStop();
    }

    // First reservoir 2 must be completely full
    if ( rsrvr2Status == RESERVOIR_STATUS_EMPTY )
    {
        rsrvr2Status = getRsrvrFillStatus( DG_RESERVOIR_2, RSRVRS_FULL_VOL_ML, RSRVRS_FILL_UP_TIMEOUT_MS );
    }
    // Once reservoir 2 is completely full, monitor reservoir 1
    else if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
    {
        rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_500ML_FILL_UP_TIMEOUT_MS );

        if ( rsrvr1Status == RESERVOIR_STATUS_FULL )
        {
            // Done with filling, turn off the RO pump
            signalROPumpHardStop();

            // De-energize all the valves and set the VDr to drain R2
            setValveState( VPI, VALVE_STATE_CLOSED );
            setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
            setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO );
            setValveState( VRO, VALVE_STATE_R1_C_TO_NO );
            setValveState( VRD, VALVE_STATE_R2_C_TO_NO );
            setValveState( VRI, VALVE_STATE_R1_C_TO_NO );
            setValveState( VRF, VALVE_STATE_R2_C_TO_NO );

            // Turn on the drain pump to drain R2
            setDrainPumpTargetRPM( DRAIN_PUMP_TARGET_RPM );

            // This is the last drain of heat disinfect cycle
            isThisLastDrain = TRUE;
            stateTimer = getMSTimerCount();
            state = DG_HEAT_DISINFECT_STATE_DRAIN_R2;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectRinseCirculationState function handles the
 * heat disinfect rinse RO circulation and concentrate pumps state. Once
 * the defined flush circulation time has elapsed, it transitions to the next
 * state.
 * @details Inputs: none
 * @details Outputs: none
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectRinseCirculationState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_RINSE_CIRCULATION;

    if ( didTimeout( stateTimer, FLUSH_CICRCULATION_WAIT_TIME_MS ) )
    {
        // Done with heat disinfect cycle
        resetActuators();

        state = DG_HEAT_DISINFECT_STATE_COMPLETE;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectCancelModeBasicPathState function handles the
 * heat disinfect cancel mode basic path state. The state sets the state
 * to complete and raises an alarm.
 * @details Inputs: cancellationMode
 * @details Outputs: cancellationMode
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCancelModeBasicPathState( void )
{
    // Go to state complete, we are done
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_CANCEL_BASIC_PATH;

    // Set the cancellation mode
    cancellationMode = CANCELLATION_MODE_BASIC;

    setModeToFailed();

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectCancelModeWaterPathState function handles the
 * heat disinfect cancel mode cold water path state. The state resets all
 * the actuators. If THd > 60 C, it runs a hot water cancellation to make sure
 * the water that is drained is below 60 C. Otherwise, it runs a cold water
 * drain. If the drain times out, it transitions to basic cancellation state.
 * If the drain is completed within the define time, it transitions to the
 * complete state.
 * @details Inputs: rsrvr1Status, rsrvr2Status, cancellationMode, stateTimer
 * @details Outputs: rsrvr1Status, rsrvr2Status, cancellationMode, stateTimer
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCancelModeWaterPathState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_CANCEL_WATER_PATH;

    if ( cancellationMode == CANCELLATION_MODE_NONE )
    {
        // Stop all the actuators first then decide who should run next
        resetActuators();

        // Check inlet dialysate and redundant outlet temperature sensors
        F32 TDi = getTemperatureValue( TEMPSENSORS_OUTLET_REDUNDANT ); //TODO change to TDi
        F32 TRo = getTemperatureValue( TEMPSENSORS_OUTLET_REDUNDANT );

        // The two sensors must be less than a threshold to decide if mix drain is needed to normal drain
        if ( TDi < MIX_DRAIN_TEMPERATURE_THRESHOLD_C && TRo < MIX_DRAIN_TEMPERATURE_THRESHOLD_C )
        {
            rsrvr1Status = RESERVOIR_STATUS_FULL;
            rsrvr2Status = RESERVOIR_STATUS_FULL;
            cancellationMode = CANCELLATION_MODE_COLD;
        }
        else
        {
            // The fluid is hot so this is a mix drain. Set the VPd to direct the cold inlet fluid to drain
            setValveState( VPI, VALVE_STATE_OPEN );
            setValveState( VPD, VALVE_STATE_DRAIN_C_TO_NC );

            rsrvr1Status = RESERVOIR_STATUS_FULL;
            rsrvr2Status = RESERVOIR_STATUS_FULL;
            cancellationMode = CANCELLATION_MODE_HOT;
        }

        // The drain is set to start from reservoir 2 since all the actuators have been de-energized
        // Start the drain pump
        setDrainPumpTargetRPM( DRAIN_PUMP_TARGET_RPM );

        // Start the timer for drain timeout
        stateTimer = getMSTimerCount();
    }

    // If reservoir 2 is empty, set to drain reservoir 1
    if ( rsrvr2Status == RESERVOIR_STATUS_FULL )
    {
        // If the cancellation water path cannot be done, got to basic cancellation path
        rsrvr2Status = getRsrvrDrainStatus( DG_RESERVOIR_2, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );

        if ( rsrvr2Status == RESERVOIR_STATUS_EMPTY )
        {
            // Set the drain valve to reservoir 1
            setValveState( VRD, VALVE_STATE_R1_C_TO_NC );
        }
    }

    // If reservoir 2 has already been drained and reservoir 1 is empty, reset and switch to complete
    if ( ( rsrvr2Status == RESERVOIR_STATUS_EMPTY ) && ( rsrvr1Status == RESERVOIR_STATUS_FULL ) )
    {
        // If the cancellation water path cannot be done, got to basic cancellation path
        rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_INITIAL_DRAIN_TIME_OUT_MS );

        if ( rsrvr1Status == RESERVOIR_STATUS_EMPTY )
        {
            setModeToFailed();
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleHeatDisinfectCompleteState function handles the
 * heat disinfect complete state. The state stops heat disinfect and
 * requests transition to mode standby.
 * @details Inputs: none
 * @details Outputs: none
 * @return next state of the heat disinfect state machine
 *************************************************************************/
static DG_HEAT_DISINFECT_STATE_T handleHeatDisinfectCompleteState( void )
{
    DG_HEAT_DISINFECT_STATE_T state = DG_HEAT_DISINFECT_STATE_COMPLETE;

    stopDGHeatDisinfect();

    return state;
}

/*********************************************************************//**
 * @brief
 * The resetActuators function sets all the actuators to reset and
 * de-energized state.
 * @details Inputs: none
 * @details Outputs: none
 * @return none
 *************************************************************************/
static void resetActuators( void )
{
    // UV reactors will not be used in the heat disinfection since their operating temperature
    // range is below 85C and they might be damaged by the high temperature.
    turnOffUVReactor( INLET_UV_REACTOR );
    turnOffUVReactor( OUTLET_UV_REACTOR );

    // De-energize all the valves
    setValveState( VPI, VALVE_STATE_OPEN );
    setValveState( VBF, VALVE_STATE_CLOSED );
    setValveState( VSP, VALVE_STATE_CLOSED );
    setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
    setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO );
    setValveState( VDR, VALVE_STATE_DRAIN_C_TO_NO );
    setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO );
    setValveState( VRO, VALVE_STATE_R1_C_TO_NO );
    setValveState( VRD, VALVE_STATE_R2_C_TO_NO );
    setValveState( VRI, VALVE_STATE_R1_C_TO_NO );
    setValveState( VRF, VALVE_STATE_R2_C_TO_NO );

    //TODO add the composition pumps
    signalROPumpHardStop();
    signalDrainPumpHardStop();
    stopPrimaryHeater();
    stopTrimmerHeater();
}

/*********************************************************************//**
 * @brief
 * The setModeToFailed function sets the heat disinfect mode to failed
 * by changing the state to complete.
 * @details Inputs: alarm, prevHeatDisinfectState
 * @details Outputs: none
 * @return none
 *************************************************************************/
static void setModeToFailed( void )
{
    SET_ALARM_WITH_1_U32_DATA( alarm, prevHeatDisinfectState )
}

/*********************************************************************//**
 * @brief
 * The isRsrvrFull function checks whether the target reservoir is full or
 * not. If the fill times out, it sets the state machine to complete and
 * exits the heat disinfect mode.
 * @details Inputs: rsrvrFillStableTimeCounter, alarm, stateTimer
 * @details Outputs: none
 * @param r is DG_RESERVOIR_1 or DG_RESERVOIR_2
 * @param targetVol is the target fill volume
 * @param timeout is the fill up timeout
 * @param cancellationState is the cancellation state to be called in case
 * the operation timed out.
 * @return the status of the reservoirs during filling
 *************************************************************************/
static RESERVOIRS_STATUS_T getRsrvrFillStatus( DG_RESERVOIR_ID_T r, F32 targetVol, U32 timeout )
{
    RESERVOIRS_STATUS_T status = RESERVOIR_STATUS_EMPTY;
    F32 volume = 0.0;

    if ( r == DG_RESERVOIR_1 )
    {
        volume = getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY );
    }
    else if ( r == DG_RESERVOIR_2 )
    {
        volume = getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY );
    }

    // Check the volume of the reservoir against the target volume
    if ( volume >= targetVol )
    {
        if ( ++rsrvrFillStableTimeCounter >= RSRVRS_FULL_STABLE_TIME_COUNT )
        {
            status = RESERVOIR_STATUS_FULL;
            rsrvrFillStableTimeCounter = 0;
            // Set the state timer in case it needs to be used for another timeout check
            stateTimer = getMSTimerCount();
        }
    }
    else if ( didTimeout( stateTimer, timeout ) )
    {
        // Failed to fill ontime. Update the previous heat disinfect state and transition to basic cancellation
        alarm = ALARM_ID_DG_RESERVOIR_FILL_TIMEOUT;
        status = RESERVOIR_STATUS_UNKNOWN;
    }

    return status;
}

/*********************************************************************//**
 * @brief
 * The getRsrvrDrainStatus function returns the status of draining a
 * reservoir.
 * @details Inputs: stateTimer, prevHeatDisinfectState, heatDisinfectState,
 * alarm
 * @details Outputs: stateTimer, heatDisinfectState heatDisinfectState,
 * alarm
 * @param r is DG_RESERVOIR_1 or DG_RESERVOIR_2
 * @param cancellationState is the cancellation state to be called in case
 * the operation timed out.
 * @return the status of the reservoirs during draining
 *************************************************************************/
static RESERVOIRS_STATUS_T getRsrvrDrainStatus( DG_RESERVOIR_ID_T r, U32 drainSteadyStateTimeout, U32 timeout )
{
    RESERVOIRS_STATUS_T status = RESERVOIR_STATUS_FULL;

    BOOL isDrainComplete = hasTargetDrainVolumeBeenReached( r, drainSteadyStateTimeout );

    if ( TRUE == isDrainComplete )
    {
        // Set the state timer in case it needs to be used for another timeout check
        stateTimer = getMSTimerCount();
        status = RESERVOIR_STATUS_EMPTY;
    }
    else if ( didTimeout( stateTimer, timeout ) )
    {
        // Failed to drain ontime. Update the previous heat disinfect state and transition to basic cancellation
        //prevHeatDisinfectState = heatDisinfectState;
        //alarm = ALARM_ID_DG_RESERVOIR_DRAIN_TIMEOUT;
        //status = RESERVOIR_STATUS_UNKNOWN;
    }

    return status;
}

/*********************************************************************//**
 * @brief
 * The getHeatDisinfectStatus function monitors and returns the current
 * stage of heat disinfect cycle. If the level of the reservoirs is drifted
 * consecutively for the define period of time, it sets the reservoir leak
 * time out alarm. If the target temperature is not reached within the
 * defined period of time, it set the temperature out of range alarm.
 * If the heat disinfect has started or has elapsed, it set the status of
 * heat disinfect accordingly.
 * @details Inputs: areRsrvrsLeaking, areRsrvrsLeaking
 * @details Outputs: areRsrvrsLeaking, areRsrvrsLeaking
 * @return status of the heat disinfect (i.e in progress, complete)
 *************************************************************************/
static HEAT_DISINFECT_STATUS_T getHeatDisinfectStatus( void )
{
    HEAT_DISINFECT_STATUS_T status = HEAT_DISINFECT_IN_PROGRESS;

    F32 TPoTemp = getTemperatureValue( TEMPSENSORS_OUTLET_PRIMARY_HEATER );
    F32 ThdTemp = getTemperatureValue( TEMPSENSORS_OUTLET_REDUNDANT ); //TODO change this to actual TPm sensor later

    BOOL isR1OutOfRange = fabs( getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_1_PRIMARY ) - R1HeatDisinfectVol ) > RSRVRS_MAX_TARGET_VOL_CHANGE_ML;
    BOOL isR2OutOfRange = fabs( getLoadCellSmallFilteredWeight( LOAD_CELL_RESERVOIR_2_PRIMARY ) - R2HeatDisinfectVol ) > RSRVRS_MAX_TARGET_VOL_CHANGE_ML;

    // Check if either reservoir 1 or reservoir 2 are losing volume more than allowed volume
    if ( isR1OutOfRange || isR2OutOfRange )
    {
        // If the leak is the first time after a while, set the flag and start the timer
        if ( FALSE == areRsrvrsLeaking )
        {
            areRsrvrsLeaking      = TRUE;
            rsrvrsVolMonitorTimer = getMSTimerCount();
        }
        // If the volume is out of range and it has timed out, exit
        else if ( didTimeout( rsrvrsVolMonitorTimer, RSRVRS_TARGET_VOL_OUT_TIMEOUT_MS ) )
        {
            areRsrvrsLeaking = FALSE;
            alarm  = ALARM_ID_DG_RESERVOIR_LEAK_TIMEOUT;
            status = HEAT_DISINFECT_RSRVRS_LEAK_TIMEOUT;
        }
    }
    // Reservoirs are in range
    else
    {
        areRsrvrsLeaking = FALSE;
    }

    // If the coldest spot which is TPm is less than minimum heat disinfect temperature,
    // reset the heat disinfect timers and check whether heating up has timed out
    if ( ThdTemp < HEAT_DISINFECT_START_TEMPERATURE_C )
    {
        // Keep reseting the disinfect timer so the elapsed time is always 0 until disinfect truly starts
        heatDisinfectTimer           = getMSTimerCount();
        isPartialDisinfectInProgress = FALSE;

        if ( didTimeout( stateTimer, HEAT_DISINFECT_START_TEMP_TIMOUT_MS ) )
        {
            // Heating up to minimum temperature for heat disinfect failed
            alarm  = ALARM_ID_DG_HEAT_DISINFECT_TARGET_TEMP_TIMEOUT;
            status = HEAT_DISINFECT_HEAT_UP_TIMEOUT;
        }
    }
    else if ( isPartialDisinfectInProgress != TRUE && ThdTemp > HEAT_DISINFECT_START_TEMPERATURE_C )
    {
        // The temperature of the coldest spot is in range to start the disinfect timer
        heatDisinfectTimer           = getMSTimerCount();
        isPartialDisinfectInProgress = TRUE;
    }

    // If the flag is TRUE, check if this stage of heat disinfect is done
    if ( isPartialDisinfectInProgress && didTimeout( heatDisinfectTimer, HEAT_DISINFECT_TIME_MS ) )
    {
        // Done with this stage of heat disnfect. Reset the variables
        status = HEAT_DISINFECT_COMPLETE;
        isPartialDisinfectInProgress = FALSE;
    }

    return status;
}

/*********************************************************************//**
 * @brief
 * The publishHeatDisinfectData function publishes heat disinfect data at
 * the set interval.
 * @details Inputs: dataPublishCounter
 * @details Outputs: dataPublishCounter
 * @return: none
 *************************************************************************/
static void publishHeatDisinfectData( void )
{
    if ( ++dataPublishCounter > HEAT_DISINFECT_DATA_PUB_INTERVAL )
    {
        MODE_HEAT_DISINFECT_DATA_T data;

        data.heatDisinfectState = (U32)heatDisinfectState;
        data.overallElapsedTime = calcTimeSince( overallHeatDisinfectTimer );
        data.stateElapsedTime   = calcTimeSince( stateTimer );
        data.cancellationMode   = (U32)cancellationMode;
        data.R1FillLevel        = R1HeatDisinfectVol;
        data.R2FillLevel        = R2HeatDisinfectVol;

        // If the mode is in the actual heat disinfect states, publish the elapsed time, otherwise publish 0 to avoid confusion
        if ( heatDisinfectState == DG_HEAT_DISINFECT_STATE_DISINFECT_R1_TO_R2 || heatDisinfectState == DG_HEAT_DISINFECT_STATE_DISINFECT_R2_TO_R1 )
        {
            data.heatDisinfectElapsedTime = calcTimeSince( heatDisinfectTimer );
        }
        else
        {
            data.heatDisinfectElapsedTime = 0;
        }

        broadcastHeatDisinfectData( &data );

        dataPublishCounter = 0;
    }
}

/**@}*/