/**********************************************************************//**
 *
 *  Copyright (c) 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 Dialysis.c
 *
 *  @date 15-Jan-2020
 *  @author S. Nash
 *
 *  @brief State machine for the dialysis sub-mode of treatment mode.
 *
 **************************************************************************/

#include "BloodFlow.h"
#include "Buttons.h"
#include "Dialysis.h"
#include "DialInFlow.h"
#include "DialOutFlow.h"
#include "OperationModes.h"
#include "TaskGeneral.h"
#include "Timers.h"
#include "ModeTreatment.h"

/**
 *  @addtogroup Dialysis
 *  @{
 */

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

#define MAX_UF_ACCURACY_ERROR_ML            250                                 ///< Maximum ultrafiltration accuracy error in mL over the entire treatment.
#define MAX_UF_ACCURACY_ERROR_ML_PER_HR     100                                 ///< Maximum ultrafiltration accuracy error in mL/hr.
#define UF_ACCURACY_CHECK_INTERVAL          ((1 * MIN_PER_HOUR * SEC_PER_MIN * MS_PER_SECOND) / TASK_GENERAL_INTERVAL)  ///< Ultrafiltration rate accuracy check interval count

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

static DIALYSIS_STATE_T currentDialysisState;           ///< Current state of the dialysis sub-mode state machine.
static UF_STATE_T currentUFState;                       ///< Current state of the ultrafiltration state machine.

static F32 refUFVolume;                                 ///< Current reference volume for ultrafiltration (Where should we be w/r/t ultrafiltration).
static F32 measUFVolume;                                ///< Current total measured volume for ultrafiltration (Where are we w/r/t ultrafiltration).
static F32 resStartVolume;                              ///< Reservoir start volume for ultrafiltration (i.e. where did we start with current reservoir).
static F32 measUFVolumeFromPriorReservoirs;             ///< Current total ultrafiltration volume from previous reservoirs in current treatment.

static U32 uFTimeMS;                                    ///< Current elapsed ultrafiltration time (in ms).  Used for calculating UF reference volume.
static U32 lastUFTimeStamp;                             ///< HD timer value when we last took stock of ultrafiltration time (so we can determine how much time has elapsed since).

static U32 setBloodFlowRate;                            ///< Currently set blood flow rate (from prescription).
static U32 setDialysateFlowRate;                        ///< Currently set dialysate flow rate (from prescription).
static F32 maxUFVolumeML;                               ///< Currently set total ultrafiltration volume for treatment (from prescription).
static F32 setUFRate;                                   ///< Currently set ultrafiltration rate (from prescription).

static U32 uFAccuracyCheckTimerCtr;                     ///< Timer counter to determine when next to check ultrafiltration accuracy.
static F32 lastUFVolumeChecked;                         ///< Starting ultrafiltration volume for accuracy check.

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

static DIALYSIS_STATE_T handleDialysisUltrafiltrationState( void );
static DIALYSIS_STATE_T handleDialysisSolutionInfusionState( void );

static UF_STATE_T handleUFStartState( void );
static UF_STATE_T handleUFPausedState( void );
static UF_STATE_T handleUFRunningState( void );
static UF_STATE_T handleUFCompletedOrOffState( void );

static void checkUFAccuracy( void );
static void updateUFVolumes( void );

/*********************************************************************//**
 * @brief
 * The initDialysis function initializes the Dialysis sub-mode module. \n
 * Calling this function will reset dialysis and therefore should only \n
 * be called when a new treatment is due to begin.
 * @details
 * Inputs : none
 * Outputs : Dialysis sub-mode module initialized.
 * @return none
 *************************************************************************/
void initDialysis( void )
{
    currentDialysisState = DIALYSIS_START_STATE;
    currentUFState = UF_START_STATE;

    refUFVolume = 0.0;
    measUFVolume = 0.0;
    resStartVolume = 0.0;
    measUFVolumeFromPriorReservoirs = 0.0;

    uFTimeMS = 0;
    lastUFTimeStamp = 0;

    setBloodFlowRate = 0;
    setDialysateFlowRate = 0;
    maxUFVolumeML = 0.0;
    setUFRate = 0.0;

    uFAccuracyCheckTimerCtr = 0;
    lastUFVolumeChecked = 0.0;
}

/*********************************************************************//**
 * @brief
 * The transitionToDialysis function prepares for transition to dialysis sub-mode. \n
 * This function will reset anything required for resuming dialysis in a \n
 * treatment that has already begun.  It does not reset everything as dialysis \n
 * may be stopped and resumed multiple times due to alarms or user intervention \n
 * and we don't want to start the treatment all over again.
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
void transitionToDialysis( void )
{
    // TODO - anything needed here?
}

/*********************************************************************//**
 * @brief
 * The setDialysisParams function sets the dialysis treatment parameters. \n
 * This function should be called prior to beginning dialysis treatment \n
 * and when the user changes one or more parameters during treatment.
 * @details
 * Inputs : none
 * Outputs : dialysis treatment parameters are set.
 * @param bPFlow : target blood pump flow rate (in mL/min).
 * @param dPFlow : target dialysate inlet pump flow rate (in mL/min).
 * @param maxUFVol : maximum ultrafiltration volume (in mL).
 * @param uFRate : target ultrafiltration rate (in mL/min).
 * @return none
 *************************************************************************/
void setDialysisParams( U32 bPFlow, U32 dPFlow, F32 maxUFVol, F32 uFRate )
{
    setBloodFlowRate = bPFlow;
    setDialysateFlowRate = dPFlow;
    maxUFVolumeML = maxUFVol;
    setUFRate = uFRate;
}

/*********************************************************************//**
 * @brief
 * The startDialysis function starts/resumes dialysis.  This function \n
 * will be called by Treatment Mode when beginning or resuming dialysis \n
 * treatment.
 * @details
 * Inputs : none
 * Outputs : Dialysis module prepared for immediate resumption.
 * @return none
 *************************************************************************/
void startDialysis( void )
{
    U32 tempDPORate = (setDialysateFlowRate * 6) / 10;  // TODO - temporary fudge factor - remove later

    lastUFTimeStamp = getMSTimerCount();
    setDialOutUFVolumes( refUFVolume, measUFVolume );
    setBloodPumpTargetFlowRate( setBloodFlowRate, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
    setDialInPumpTargetFlowRate( setDialysateFlowRate, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
    // setDialOutPumpTargetRate( setDialysateFlowRate + (S32)setUFRate), MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_CLOSED_LOOP );  // TODO - restore later
    setDialOutPumpTargetRate( tempDPORate + (S32)setUFRate, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_CLOSED_LOOP );  // TODO - test code - remove later
    // TODO - Heparin pump
}

/*********************************************************************//**
 * @brief
 * The stopDialysis function stops dialysis.  This function will be called \n
 * by Treatment Mode when an alarm occurs or the user pressed the stop button. \n
 * Dialysis may be resumed later.
 * @details
 * Inputs : none
 * Outputs : Blood and dialysate pumps stopped.  Heparin pump stopped.
 * @return none
 *************************************************************************/
void stopDialysis( void )
{
    setBloodPumpTargetFlowRate( 0, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
    setDialInPumpTargetFlowRate( 0, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
    setDialOutPumpTargetRate( 0, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_CLOSED_LOOP );
}

/*********************************************************************//**
 * @brief
 * The getDialysisState function gets the current dialysis state (sub-mode).
 * @details
 * Inputs : currentDialysisState
 * Outputs : none
 * @return currentDialysisState
 *************************************************************************/
DIALYSIS_STATE_T getDialysisState( void )
{
    return currentDialysisState;
}

/*********************************************************************//**
 * @brief
 * The getUltrafiltrationState function gets the current ultrafiltration state.
 * @details
 * Inputs : currentUFState
 * Outputs : none
 * @return currentUFState
 *************************************************************************/
UF_STATE_T getUltrafiltrationState( void )
{
    return currentUFState;
}

/*********************************************************************//**
 * @brief
 * The pauseUF function pauses ultrafiltration.
 * @details
 * Inputs : currentDialysisState, currentUFState
 * Outputs : currentUFState, outlet pump set point
 * @return TRUE if pause successful, FALSE if not
 *************************************************************************/
BOOL pauseUF( void )
{
    BOOL result = FALSE;
    TREATMENT_STATE_T trtState = getTreatmentState();
    OP_MODE currMode = getCurrentOperationMode();

    if ( ( MODE_TREA == currMode ) && ( TREATMENT_DIALYSIS_STATE == trtState ) &&
         ( DIALYSIS_UF_STATE == currentDialysisState ) && ( UF_RUNNING_STATE == currentUFState ) )
    {
        // set outlet pump to dialysate rate
        result = setDialOutPumpTargetRate( setDialysateFlowRate, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_CLOSED_LOOP );

        if ( TRUE == result )
        {
            // go to UF paused state
            currentUFState = UF_PAUSED_STATE;
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The resumeUF function resumes ultrafiltration.
 * @details
 * Inputs : currentDialysisState, currentUFState
 * Outputs : currentUFState, outlet pump set point
 * @return TRUE if resume successful, FALSE if not
 *************************************************************************/
BOOL resumeUF( void )
{
    BOOL result = FALSE;
    TREATMENT_STATE_T trtState = getTreatmentState();
    OP_MODE currMode = getCurrentOperationMode();

    if ( ( MODE_TREA == currMode ) && ( TREATMENT_DIALYSIS_STATE == trtState ) &&
         ( DIALYSIS_UF_STATE == currentDialysisState ) && ( UF_PAUSED_STATE == currentUFState ) )
    {
        // set outlet pump to dialysate rate + set UF rate
        result = setDialOutPumpTargetRate( setDialysateFlowRate + FLOAT_TO_INT_WITH_ROUND( setUFRate ), MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_CLOSED_LOOP );

        if ( TRUE == result )
        {
            // restart UF time accumulation for reference volume calculation
            lastUFTimeStamp = getMSTimerCount();
            // go to UF paused state
            currentUFState = UF_RUNNING_STATE;
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The execDialysis function executes the Dialysis sub-mode state machine.
 * @details
 * Inputs : currentDialysisState
 * Outputs : currentDialysisState
 * @return none
 *************************************************************************/
void execDialysis( void )
{
    // check ultrafiltration accuracy during dialysis (even when ultrafiltration is paused).
    checkUFAccuracy();

    // dialysis state machine
    switch ( currentDialysisState )
    {
        case DIALYSIS_START_STATE:
            resStartVolume = getLoadCellWeightInGrams( LOAD_CELL_RESERVOIR_1_PRIMARY );  // always start dialysis w/ reservoir 1
            currentDialysisState = DIALYSIS_UF_STATE;
            break;

        case DIALYSIS_UF_STATE:
            currentDialysisState = handleDialysisUltrafiltrationState();
            break;

        case DIALYSIS_SOLUTION_INFUSION_STATE:
            currentDialysisState = handleDialysisSolutionInfusionState();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIALYSIS_INVALID_STATE, currentDialysisState )
            break;
    }
}

/*********************************************************************//**
 * @brief
 * The handleDialysisUltrafiltrationState function handles the ultrafiltration \n
 * state of the Dialysis state machine.
 * @details
 * Inputs : currentUFState
 * Outputs : currentUFState
 * @return next Dialysis state.
 *************************************************************************/
static DIALYSIS_STATE_T handleDialysisUltrafiltrationState( void )
{
    DIALYSIS_STATE_T result = DIALYSIS_UF_STATE;

    switch ( currentUFState )
    {
        case UF_START_STATE:
            currentUFState = handleUFStartState();
            break;

        case UF_PAUSED_STATE:
            currentUFState = handleUFPausedState();
            break;

        case UF_RUNNING_STATE:
            currentUFState = handleUFRunningState();
            break;

        case UF_COMPLETED_OR_OFF_STATE:
            currentUFState = handleUFCompletedOrOffState();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIALYSIS_INVALID_UF_STATE, currentUFState )
            break;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleDialysisSolutionInfusionState function handles the solution \n
 * infustion state of the Dialysis state machine.
 * @details
 * Inputs : TBD
 * Outputs : TBD
 * @return next Dialysis state.
 *************************************************************************/
static DIALYSIS_STATE_T handleDialysisSolutionInfusionState( void )
{
    DIALYSIS_STATE_T result = DIALYSIS_SOLUTION_INFUSION_STATE;

    // TODO -

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleUFStartState function handles the Start state of the \n
 * ultrafiltration state machine.
 * @details
 * Inputs : maxUFVolumeML
 * Outputs : if ultrafiltration prescribed, ultrafiltration time is
 * initialized.
 * @return next ultrafiltration state.
 *************************************************************************/
static UF_STATE_T handleUFStartState( void )
{
    UF_STATE_T result;

    if ( maxUFVolumeML < NEARLY_ZERO )
    {
        result = UF_COMPLETED_OR_OFF_STATE;
    }
    else
    {
        lastUFTimeStamp = getMSTimerCount();
        uFTimeMS = 0;
        result = UF_RUNNING_STATE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleUFPausedState function handles the Paused state of the \n
 * ultrafiltration state machine.
 * @details
 * Inputs : none
 * Outputs : if ultrafiltration resumption requested, UF time is set to resume.
 * @return next ultrafiltration state.
 *************************************************************************/
static UF_STATE_T handleUFPausedState( void )
{
    UF_STATE_T result = UF_PAUSED_STATE;

    // calculate UF volumes and provide to dialysate outlet pump controller
    updateUFVolumes();

    // TODO - test code - remove later
    if ( TRUE == isStopButtonPressed() )
    {
        resumeUF();
        result = UF_RUNNING_STATE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleUFRunningState function handles the Running state of the \n
 * ultrafiltration state machine.
 * @details
 * Inputs : ms timer, lastUFTimeStamp
 * Outputs : UF timer incremented, UF volumes updated and provided to DPo \n
 * pump controller.
 * @return next ultrafiltration state.
 *************************************************************************/
static UF_STATE_T handleUFRunningState( void )
{
    UF_STATE_T result = UF_RUNNING_STATE;
    U32 newTime = getMSTimerCount();
    U32 msSinceLast = calcTimeBetween( lastUFTimeStamp, newTime );

    // update UF time
    uFTimeMS += msSinceLast;
    lastUFTimeStamp = newTime;

    // calculate UF volumes and provide to dialysate outlet pump controller
    updateUFVolumes();

    // if we've reached target UF volume, UF is complete
    if ( measUFVolume >= maxUFVolumeML )
    {
        result = UF_COMPLETED_OR_OFF_STATE;
    }

    // TODO - test code - remove later
    if ( TRUE == isStopButtonPressed() )
    {
        pauseUF();
        result = UF_PAUSED_STATE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleUFCompletedOrOffState function handles the UF Completed or Off \n
 * state of the ultrafiltration state machine.
 * @details
 * Inputs : none
 * Outputs : UF volumes updated and provided to DPo pump controller.
 * @return next ultrafiltration state
 *************************************************************************/
static UF_STATE_T handleUFCompletedOrOffState( void )
{
    UF_STATE_T result = UF_COMPLETED_OR_OFF_STATE;

    // calculate UF volumes and provide to dialysate outlet pump controller
    updateUFVolumes();

    // TODO - test code - remove later
    if ( TRUE == isStopButtonPressed() )
    {
        // do nothing
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The checkUFAccuracy function checks ultrafiltration accuracy for the last \n
 * minute and triggers an alarm if out of spec.
 * @details
 * Inputs : uFAccuracyCheckTimerCtr, lastUFVolumeChecked, measUFVolume
 * Outputs : uFAccuracyCheckTimerCtr, lastUFVolumeChecked
 * @return none
 *************************************************************************/
static void checkUFAccuracy( void )
{
    // check UF accuracy at 1 hour intervals
    if ( ++uFAccuracyCheckTimerCtr >= UF_ACCURACY_CHECK_INTERVAL )
    {
        F32 uFMeasRatePerHr = measUFVolume - lastUFVolumeChecked;
        F32 uFSetRatePerHr = ( setUFRate * (F32)MIN_PER_HOUR );
        F32 uFRateError = FABS( uFSetRatePerHr - uFMeasRatePerHr );

        // check UF accuracy
        if ( uFRateError > (F32)MAX_UF_ACCURACY_ERROR_ML_PER_HR )
        {
            SET_ALARM_WITH_2_F32_DATA( ALARM_ID_UF_RATE_TOO_HIGH_ERROR, uFSetRatePerHr, uFMeasRatePerHr );
        }
        // reset for next check
        lastUFVolumeChecked = measUFVolume;
        uFAccuracyCheckTimerCtr = 0;
    }

    // check total UF volume error
    if ( ( FABS( refUFVolume - measUFVolume ) ) >= (F32)MAX_UF_ACCURACY_ERROR_ML )
    {
        SET_ALARM_WITH_2_F32_DATA( ALARM_ID_UF_VOLUME_ACCURACY_ERROR, refUFVolume, measUFVolume );
    }
}

/*********************************************************************//**
 * @brief
 * The updateUFVolumes function updates the ultrafiltration volumes based on \n
 * set UF rate, latest UF elapsed time, and the latest load cell weight for the \n
 * currently used reservoir.  Updated UF volumes are then sent to the dialysate \n
 * outlet pump controller.
 * @details
 * Inputs : setUFRate, uFTimeMS, load cell weight
 * Outputs : refUFVolume, measUFVolume
 * @return none
 *************************************************************************/
static void updateUFVolumes( void )
{
    F32 latestResVolume = getLoadCellWeightInGrams( LOAD_CELL_RESERVOIR_1_PRIMARY );  // TODO - just res 1 for now - add reservoir switching, mgmt later.

    // calculate UF volumes and provide to dialysate outlet pump controller
    refUFVolume = ( ( (F32)uFTimeMS / MS_PER_SECOND ) / SEC_PER_MIN ) * setUFRate;
    measUFVolume = measUFVolumeFromPriorReservoirs + ( latestResVolume - resStartVolume );
    setDialOutUFVolumes( refUFVolume, measUFVolume );
}

/**@}*/