/**************************************************************************
*
* 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 ModeFlush.c
*
* @author (last) Quang Nguyen
* @date (last) 01-Sep-2020
*
* @author (original) Leonardo Baloa
* @date (original) 20-Dec-2019
*
***************************************************************************/

#include "ConductivitySensors.h"
#include "DrainPump.h"
#include "Heaters.h"
#include "LoadCell.h"
#include "ModeFlush.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 DGFlushMode
 *  @{
 */

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

// General defines
#define FLUSH_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )                  ///< Mode flush data publish interval in counts.
#define RO_PUMP_TARGET_FLOW_RATE_LPM   0.8                                                 ///< RO pump target flow rate during flush/fill in L/min. TODO original flow was 0.8
#define RO_PUMP_MAX_PRESSURE_PSI       130                                                 ///< Maximum RO pump pressure during flush/fill states in psi.
#define DRAIN_PUMP_TARGET_RPM                    1800                                      ///< Drain pump target RPM during drain.

// Start state defines


// Drain R1 & R2 states defines
#define RSRVRS_DRAIN_TIME_OUT_MS      ( 2 * 60 * MS_PER_SECOND )                           ///< Reservoirs 1 & 2 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      ( 1 * 60 * MS_PER_SECOND )                               ///< Flush Drain path wait time in milliseconds. TODo original time is 2 minutes
#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 dialysate state defines
#define FLUSH_DIALYSATE_WAIT_TIME_MS  ( 0.5 * 60 * MS_PER_SECOND )                         ///< Flush dialysate wait time in milliseconds.

// Flush concentrate straws state defines
#define FLUSH_CONCENTRATE_STRAWS_TIME_MS ( 0.5 * 60 * MS_PER_SECOND )                       ///< Flush concentrate straws wait time in milliseconds. TODO original time is 3 minutes

// 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_PARTIAL_FILL_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.


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

static DG_FLUSH_STATE_T flushState = DG_FLUSH_STATE_START;     ///< Currently active flush state.
static DG_FLUSH_STATE_T prevFlushState = DG_FLUSH_STATE_START;
static U32 rsrvrFillStableTimeCounter = 0;                                                 ///< Reservoirs fill stable time counter.
static U32 overallFlushElapsedTime = 0;
static U32 stateTimer = 0;
static ALARM_ID_T alarm;
static DG_RESERVOIR_STATUS_T rsrvr1Status = NUM_OF_DG_RESERVOIR_STATUS;                        ///< Reservoir 1 status.
static DG_RESERVOIR_STATUS_T rsrvr2Status = NUM_OF_DG_RESERVOIR_STATUS;                        ///< Reservoir 2 status.
static BOOL isThisInitialDrain = TRUE;
static U32 dataPublishCounter = 0;                                                         ///< Flush data publish counter.

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

static DG_FLUSH_STATE_T handleFlushModeStartState( void );
static DG_FLUSH_STATE_T handleFlushModeDrainR1State( void );
static DG_FLUSH_STATE_T handleFlushModeDrainR2State( void );
static DG_FLUSH_STATE_T handleFlushModeFlushDrainState( void );
static DG_FLUSH_STATE_T handleFlushModeFlushDialysateState( void );
static DG_FLUSH_STATE_T handleFlushModeFlushConcentrateStrawsState( void );
static DG_FLUSH_STATE_T handleFlushModeFlushR1ToR2State( void );
static DG_FLUSH_STATE_T handleFlushModeFlushR2AndDrainR1State( void );
static DG_FLUSH_STATE_T handleFlushModeFlushCirculationState( void );
static DG_FLUSH_STATE_T handleFlushModeCancelBasicPathState( void );
static DG_FLUSH_STATE_T handleFlushModeCancelWaterPathState( void );
static DG_FLUSH_STATE_T handleFlushModeComplete( void );

static void resetActuators( void );
static void setModeToFailed( void );
static DG_RESERVOIR_STATUS_T getRsrvrFillStatus( DG_RESERVOIR_ID_T r, F32 targetVol, U32 timeout );
static DG_RESERVOIR_STATUS_T getRsrvrDrainStatus( DG_RESERVOIR_ID_T r, U32 drainSteadyStateTimeout, U32 timeout );
static void publishFlushData( void );

/*********************************************************************//**
 * @brief
 * The initFlushMode function initializes flush mode module.
 * @details Inputs: none
 * @details Outputs: Initialized flush mode module
 * @return none
*************************************************************************/
void initFlushMode( void )
{
    flushState = DG_FLUSH_STATE_START;
    isThisInitialDrain = TRUE;
}

/*********************************************************************//**
 * @brief
 * The transitionToFlushMode function prepares for transition to flush mode.
 * @details Inputs: none
 * @details Outputs: Prepares for transition to flush mode
 * @return none
*************************************************************************/
void transitionToFlushMode( void )
{
    initFlushMode();
}

/*********************************************************************//**
 * @brief
 * The execFlushMode function executes the flush mode state machine.
 * @details Inputs: none
 * @details Outputs: Flush mode state machine executed
 * @return current state
*************************************************************************/
U32 execFlushMode( void )
{
    //checkInletPressureFault(); TODO why do we need this?

    // Execute current flush state
    switch ( flushState )
    {
        case DG_FLUSH_STATE_START:
            flushState = handleFlushModeStartState();
            break;

        case DG_FLUSH_STATE_DRAIN_R1:
            flushState = handleFlushModeDrainR1State();
            break;

        case DG_FLUSH_STATE_DRAIN_R2:
            flushState = handleFlushModeDrainR2State();
            break;

        case DG_FLUSH_STATE_FLUSH_DRAIN:
            flushState = handleFlushModeFlushDrainState();
            break;

        case DG_FLUSH_STATE_FLUSH_DIALYSATE:
            flushState = handleFlushModeFlushDialysateState();
            break;

        case DG_FLUSH_STATE_FLUSH_CONCENTRATE_STRAWS:
            flushState = handleFlushModeFlushConcentrateStrawsState();
            break;

        case DG_FLUSH_STATE_FLUSH_R1_TO_R2:
            flushState = handleFlushModeFlushR1ToR2State();
            break;

        case DG_FLUSH_STATE_FLUSH_R2_AND_DRAIN_R1:
            flushState = handleFlushModeFlushR2AndDrainR1State();
            break;

        case DG_FLUSH_STATE_FLUSH_CIRCULATION:
            flushState = handleFlushModeFlushCirculationState();
            break;

        case DG_FLUSH_STATE_CANCEL_BASIC_PATH:
            flushState = handleFlushModeCancelBasicPathState();
            break;

        case DG_FLUSH_STATE_CANCEL_WATER_PATH:
            flushState = handleFlushModeCancelWaterPathState();
            break;

        case DG_FLUSH_STATE_COMPLETE:
            flushState = handleFlushModeComplete();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DG_FLUSH_INVALID_EXEC_STATE, flushState )
            flushState = DG_FLUSH_STATE_START;
            break;
    }

    publishFlushData();

    return flushState;
}

/*********************************************************************//**
 * @brief
 * The getCurrentFlushState function returns the current state of the flush
 * mode.
 * @details Inputs: flushState
 * @details Outputs: none
 * @return current state of flush mode.
 *************************************************************************/
DG_FLUSH_STATE_T getCurrentFlushState( void )
{
    return flushState;
}

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

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

// ********** private functions **********

static DG_FLUSH_STATE_T handleFlushModeStartState( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_START;

    //TODO make sure the conductivities are in range

    // Start overall flush timer
    overallFlushElapsedTime = getMSTimerCount();

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

    if ( FALSE ) //TODO figure out the conductivity and pressure check
    {
        state = DG_FLUSH_STATE_CANCEL_BASIC_PATH;
    }
    else
    {
        // Reset the load cells lowest weight prior to starting the run
        resetReservoirsLowestWeight();

        // 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 );

        stateTimer = getMSTimerCount();
        rsrvr1Status = DG_RESERVOIR_ABOVE_TARGET;
        state = DG_FLUSH_STATE_DRAIN_R1;
    }

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeDrainR1State( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_DRAIN_R1;

    if ( rsrvr1Status == DG_RESERVOIR_ABOVE_TARGET )
    {
        rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr1Status == DG_RESERVOIR_REACHED_TARGET )
    {
        // Request a tare for reservoir 2
        tareReservoir();

        // Set the actuators to drain R2
        // NOTE: Drain pump is already on and VDr is already on drain state
        setValveState( VRD, VALVE_STATE_R2_C_TO_NO );

        stateTimer = getMSTimerCount();
        rsrvr2Status = DG_RESERVOIR_ABOVE_TARGET;
        state = DG_FLUSH_STATE_DRAIN_R2;
    }
    else if ( rsrvr1Status == DG_RESERVOIR_NOT_REACHED_TARGET )
    {
        state = DG_FLUSH_STATE_CANCEL_BASIC_PATH;
    }

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeDrainR2State( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_DRAIN_R2;

     if ( rsrvr2Status == DG_RESERVOIR_ABOVE_TARGET )
     {
         rsrvr2Status = getRsrvrDrainStatus( DG_RESERVOIR_2, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_DRAIN_TIME_OUT_MS );
     }
     else if ( rsrvr2Status == DG_RESERVOIR_REACHED_TARGET )
     {
         signalDrainPumpHardStop();

         // Set the actuators to flush drain
         setValveState( VPI, VALVE_STATE_OPEN );
         setValveState( VPD, VALVE_STATE_DRAIN_C_TO_NC );

         stateTimer = getMSTimerCount();

         state = DG_FLUSH_STATE_FLUSH_DRAIN;
     }
     else if ( rsrvr2Status == DG_RESERVOIR_NOT_REACHED_TARGET )
     {
         state = DG_FLUSH_STATE_CANCEL_BASIC_PATH;
     }

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeFlushDrainState( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_FLUSH_DRAIN;

    if ( didTimeout( stateTimer, FLUSH_DRAIN_WAIT_TIME_MS ) )
    {
        setValveState( VPD, VALVE_STATE_OPEN_C_TO_NO );
        setROPumpTargetFlowRate( RO_PUMP_TARGET_FLOW_RATE_LPM, RO_PUMP_MAX_PRESSURE_PSI );

        // Turn on the UV reactors
        turnOnUVReactor( INLET_UV_REACTOR );
        turnOnUVReactor( OUTLET_UV_REACTOR );

        stateTimer = getMSTimerCount();

        state = DG_FLUSH_STATE_FLUSH_DIALYSATE;
    }

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeFlushDialysateState( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_FLUSH_DIALYSATE;

    if ( didTimeout( stateTimer, FLUSH_DIALYSATE_WAIT_TIME_MS ) )
    {
        // TODO turn on the concentrate pumps
        stateTimer = getMSTimerCount();

        state = DG_FLUSH_STATE_FLUSH_CONCENTRATE_STRAWS;
    }

    return state;
}
static DG_FLUSH_STATE_T handleFlushModeFlushConcentrateStrawsState( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_FLUSH_CONCENTRATE_STRAWS;

    if ( didTimeout( stateTimer, FLUSH_CONCENTRATE_STRAWS_TIME_MS ) )
    {
        // TODO turn off the concentrate pumps
        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();

        rsrvr1Status = DG_RESERVOIR_BELOW_TARGET;
        rsrvr2Status = DG_RESERVOIR_BELOW_TARGET;
        state = DG_FLUSH_STATE_FLUSH_R1_TO_R2;
    }

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeFlushR1ToR2State( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_FLUSH_R1_TO_R2;

     if ( rsrvr1Status == DG_RESERVOIR_BELOW_TARGET )
     {
         rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_FULL_VOL_ML, RSRVRS_FILL_UP_TIMEOUT_MS );
     }
     else if ( rsrvr1Status == DG_RESERVOIR_REACHED_TARGET )
     {
         if ( rsrvr2Status == DG_RESERVOIR_BELOW_TARGET )
         {
             rsrvr2Status = getRsrvrFillStatus( DG_RESERVOIR_2, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_PARTIAL_FILL_TIMEOUT_MS );
         }
         else if ( rsrvr2Status == DG_RESERVOIR_REACHED_TARGET )
         {
             // 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 = DG_RESERVOIR_ABOVE_TARGET;
             rsrvr2Status = DG_RESERVOIR_BELOW_TARGET;
             state = DG_FLUSH_STATE_FLUSH_R2_AND_DRAIN_R1;
         }
         else if ( rsrvr2Status == DG_RESERVOIR_NOT_REACHED_TARGET )
         {
             state = DG_FLUSH_STATE_CANCEL_WATER_PATH;
         }
     }
     else if ( rsrvr1Status == DG_RESERVOIR_NOT_REACHED_TARGET )
     {
         state = DG_FLUSH_STATE_CANCEL_WATER_PATH;
     }

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeFlushR2AndDrainR1State( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_FLUSH_R2_AND_DRAIN_R1;

    // If reservoir 1 is empty, turn off the drain pump
    if ( rsrvr1Status == DG_RESERVOIR_ABOVE_TARGET )
    {
        rsrvr1Status = getRsrvrDrainStatus( DG_RESERVOIR_1, DRAIN_WEIGHT_UNCHANGE_TIMEOUT, RSRVRS_DRAIN_TIME_OUT_MS );
    }
    else if ( rsrvr1Status == DG_RESERVOIR_REACHED_TARGET )
    {
        // Done with draining R1
        signalDrainPumpHardStop();
    }
    else if ( rsrvr1Status == DG_RESERVOIR_NOT_REACHED_TARGET )
    {
        state = DG_FLUSH_STATE_CANCEL_BASIC_PATH;
    }

    // First reservoir 2 must be completely full
    if ( rsrvr2Status == DG_RESERVOIR_BELOW_TARGET )
    {
        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 == DG_RESERVOIR_REACHED_TARGET )
    {
        rsrvr1Status = getRsrvrFillStatus( DG_RESERVOIR_1, RSRVRS_PARTIAL_FILL_VOL_ML, RSRVRS_PARTIAL_FILL_TIMEOUT_MS );

        // Once R1 is partially full, transition to the next state
        if ( rsrvr1Status == DG_RESERVOIR_REACHED_TARGET )
        {
            signalROPumpHardStop();
            setValveState( VRD, VALVE_STATE_R2_C_TO_NO );
            setDrainPumpTargetRPM( DRAIN_PUMP_TARGET_RPM );

            stateTimer = getMSTimerCount();
            isThisInitialDrain = FALSE;
            state = DG_FLUSH_STATE_DRAIN_R2;
        }
        // TODO add timeout
    }
    // TODO add timeout

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeFlushCirculationState( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_FLUSH_CIRCULATION;

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeCancelBasicPathState( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_CANCEL_BASIC_PATH;

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeCancelWaterPathState( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_CANCEL_WATER_PATH;

    return state;
}

static DG_FLUSH_STATE_T handleFlushModeComplete( void )
{
    DG_FLUSH_STATE_T state = DG_FLUSH_STATE_COMPLETE;

    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 )
{
    // Turn off the UV reactors
    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();
}

static void setModeToFailed( void )
{
    SET_ALARM_WITH_1_U32_DATA( alarm, prevFlushState )
}

static DG_RESERVOIR_STATUS_T getRsrvrFillStatus( DG_RESERVOIR_ID_T r, F32 targetVol, U32 timeout )
{
    DG_RESERVOIR_STATUS_T status = DG_RESERVOIR_BELOW_TARGET;
    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 = DG_RESERVOIR_REACHED_TARGET;
            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 on time
        alarm = ALARM_ID_DG_RESERVOIR_FILL_TIMEOUT;
        status = DG_RESERVOIR_NOT_REACHED_TARGET;
    }

    return status;
}

static DG_RESERVOIR_STATUS_T getRsrvrDrainStatus( DG_RESERVOIR_ID_T r, U32 drainSteadyStateTimeout, U32 timeout )
{
    DG_RESERVOIR_STATUS_T status = DG_RESERVOIR_ABOVE_TARGET;

    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 = DG_RESERVOIR_REACHED_TARGET;
    }
    else if ( didTimeout( stateTimer, timeout ) )
    {
        // Failed to drain on time
        prevFlushState = flushState;
        alarm = ALARM_ID_DG_RESERVOIR_DRAIN_TIMEOUT;
        status = DG_RESERVOIR_NOT_REACHED_TARGET;
    }

    return status;
}

static void publishFlushData( void )
{
    if ( ++dataPublishCounter > FLUSH_DATA_PUB_INTERVAL )
    {
        MODE_FLUSH_DATA_T data;

        data.flushState         = (U32)flushState;
        data.overallElapsedTime = calcTimeSince( overallFlushElapsedTime );
        data.stateElapsedTime   = calcTimeSince( stateTimer );

        broadcastFlushData( &data );

        dataPublishCounter = 0;
    }
}

/**@}*/