/**************************************************************************
*
* 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 ModeFault.c
*
* @author (last) Sean Nash
* @date (last) 10-Jun-2020
*
* @author (original) Dara Navaei
* @date (original) 05-Nov-2019
*
***************************************************************************/

#include "AlarmLamp.h"
#ifdef EMC_TEST_BUILD     // TODO - test code
#include "BloodFlow.h"
#include "Buttons.h"
#include "DialInFlow.h"
#include "DialOutFlow.h"
#include "FPGA.h"
#endif
#include "OperationModes.h"
#include "ModeFault.h"

/**
 *  @addtogroup HDFaultMode
 *  @{
 */

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

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

/*********************************************************************//**
 * @brief
 * The initFaultMode function initializes the Fault Mode module.
 * @details
 * Inputs : none
 * Outputs : Fault Mode module initialized.
 * @return none
 *************************************************************************/
void initFaultMode( void )
{
}

/*********************************************************************//**
 * @brief
 * The transitionToFaultMode function prepares for transition to fault mode.
 * @details
 * Inputs : none
 * Outputs :
 * @return none
 *************************************************************************/
void transitionToFaultMode( void )
{
#ifdef EMC_TEST_BUILD     // TODO - test code
    enableValvesPIDControl(0); // enable valves
#endif
}

/*********************************************************************//**
 * @brief
 * The execFaultMode function executes the Fault Mode state machine.
 * @details
 * Inputs : none
 * Outputs :
 * @return current state (sub-mode)
 *************************************************************************/
U32 execFaultMode( void )
{
#ifdef EMC_TEST_BUILD     // TODO - test code
    static U32 toggle = 0;
    static BOOL button_state = FALSE;
    BOOL stop = isStopButtonPressed();

    if ( TRUE == stop )
    {
        if ( stop != button_state )
        {
            toggle = INC_WRAP( toggle, 0, 3 );
            switch ( toggle )
            {
                case 0: // pumps and valves off
                    setDialyzerInletValvePosition( 0 );
                    setDialyzerOutletValvePosition( 0 );
                    setVenousBloodValvePosition( 0 );
                    setArterialBloodValvePosition( 0 );
                    break;

                case 1: // pumps on, valves off
                    setBloodPumpTargetFlowRate( 500, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
                    setDialInPumpTargetFlowRate( 500, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
                    setDialOutPumpTargetRate( 500, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP );
                    break;

                case 2: // pumps on, valves on
                    setDialyzerInletValvePosition( 12000 );
                    setDialyzerOutletValvePosition( 12000 );
                    setVenousBloodValvePosition( 12000 );
                    setArterialBloodValvePosition( 12000 );
                    break;

                case 3: // pumps off, valves on
                    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_OPEN_LOOP );
                    break;

                default: // shouldn't get here, reset if we do
                    toggle = 0;
                    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_OPEN_LOOP );
                    setDialyzerInletValvePosition( 0 );
                    setDialyzerOutletValvePosition( 0 );
                    setVenousBloodValvePosition( 0 );
                    setArterialBloodValvePosition( 0 );
                    break;
            }
        }
    }
    button_state = stop;
#endif

    return 0; // TODO - return current state
}

/**@}*/