// Includes
#include "reg_het.h"
#include "gio.h"

#include "Valves.h"
#include "FPGA.h"
#include "Timers.h"
#include "TaskPriority.h"
#include "SystemCommMessages.h"

/**
 *  @addtogroup Valves
 *  @{
 */

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

// Valve dialyzer inlet defines
#define VDI_ENABLE_PID_INTERFACE    0x0010
#define VDI_ENABLE_BYPASS_INTERFACE 0x0020

#define VDI_INIT_STATUS_BIT_MASK 0x0040
#define VDI_ENABLE_PID_STATUS_BIT_MASK 0x0080
#define VDI_ENABLE_BYPASS_STATUS_BIT_MASK 0x00100

// Valve dialyzer outlet defines
#define VDO_ENABLE_PID_INTERFACE    0x0040
#define VDO_ENABLE_BYPASS_INTERFACE 0x0080

#define VDO_INIT_STATUS_BIT_MASK 0x0200
#define VDO_ENABLE_PID_STATUS_BIT_MASK 0x0400
#define VDO_ENABLE_BYPASS_STATUS_BIT_MASK 0x0800

// Valve blood arterial defines
#define VBA_ENABLE_PID_INTERFACE    0x0001
#define VBA_ENABLE_BYPASS_INTERFACE 0x0002

#define VBA_INIT_STATUS_BIT_MASK 0x0001
#define VBA_ENABLE_PID_STATUS_BIT_MASK 0x0002
#define VBA_ENABLE_BYPASS_STATUS_BIT_MASK 0x0004

// Valve blood venous defines
#define VBV_ENABLE_PID_INTERFACE    0x0004
#define VBV_ENABLE_BYPASS_INTERFACE 0x0008

#define VBV_INIT_STATUS_BIT_MASK 0x0008
#define VBV_ENABLE_PID_STATUS_BIT_MASK 0x0010
#define VBV_ENABLE_BYPASS_STATUS_BIT_MASK 0x0020

#define ALL_VALVES_ENABLE_PID_INTERFACE 0x0055

#define ADC_TO_CURRENT_CONVERSION_CONSTANT       2048.0

#define INITIAL_EDGE_OFFSET_READ_COUNT 100

#define INITIAL_ENERGIZED_EDGE_UPPER_RANGE 13000U
#define INITIAL_ENERGIZED_EDGE_LOWER_RANGE 9000U

#define AIR_TRAP_VALVE_GPIO_PIN 0x12

#define STEP_CHANGE_IN_COUNTS            1000

#define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150
#define MAX_ALLOWED_FAILED_HOMINGS                   3U
#define HOMING_EDGE_DETECTION_TIME_INTERVAL         ( MS_PER_SECOND / ( 2 * TASK_PRIORITY_INTERVAL )  )    ///< The time that the valve must be at the edge to be considered for edge detection
#define VALVES_CURRENT_THRESHOLD_AMPS                1U
#define OVER_CURRENT_COUNTER                    100U
#define OUT_OF_RANGE_COUNTER                    100U
#define VALVE_TRANSITION_TIMEOUT_MS             3000U //TODO experiment with this

#define NEXT_STEP_COMD_TIME_INTERVAL_COUNTER    ( MS_PER_SECOND / ( 10 * TASK_PRIORITY_INTERVAL ) )

// TODO changed the interval count to every 10 calls in priority task which is 100 ms
#define VALVES_DATA_PUB_INTERVAL                ( MS_PER_SECOND / ( 10 * TASK_PRIORITY_INTERVAL ) )

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

/// Exec valve self test states
typedef enum valves_Self_Test_States
{
    VALVE_SELF_TEST_ENABLE_VALVES = 0,
    VALVE_SELF_TEST_CONFIRM_ENABLE,
    VALVE_SELF_TEST_COMPLETE,
    NUM_OF_VALVE_SELF_TEST_STATES
} VALVE_SELF_TEST_STATE_T;

/// Exec valve states
typedef enum valve_Exec_States
{
    VALVE_STATE_WAIT_FOR_POST = 0,
    VALVE_STATE_HOMING_NOT_STARTED,
    VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE,
    VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE,
    VALVE_STATE_IDLE,
    VALVE_STATE_IN_TRANSITION,
    NUM_OF_VALVE_STATES,
} VALVE_STATE_T;

#pragma pack(push, 1)
/// Valve status structure
typedef struct
{
    VALVE_POSITION_T commandedPosition;
    VALVE_POSITION_T currentPosition;
    S16              currentPositionInCounts;
    S16              targetPositionInCounts;
    BOOL             hasTransitionBeenRequested;
    VALVE_STATE_T    execState;
    U32              transitionStartTime;
    S16              positions[ NUM_OF_VALVE_POSITIONS ];
    F32              valveCurrent;
    U32              valveOverCurrentCounter;
    U32              valvePositionOutOfRangeCounter;
    U32              valveDataPublishCounter;
    U32              valveRestCounter;  //TODO REMOVE?
    // Homing variables
    U32              numberOfFailedHomings;
    U32              homingEdgeDetectionCounter;
    BOOL             hasHomingBeenRequested;
    BOOL             hasValveBeenHomed;
    BOOL             hasHomingFailed;
} VALVE_STATUS_T;
#pragma pack(pop)

static VALVE_SELF_TEST_STATE_T valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES;
static SELF_TEST_STATUS_T      valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS;

static VALVE_STATUS_T valvesStatus[ NUM_OF_VALVES ];

static OVERRIDE_U32_T valvesDataPublishInterval = { VALVES_DATA_PUB_INTERVAL, VALVES_DATA_PUB_INTERVAL, 0, 0 };

static OVERRIDE_U32_T valvesPositionOverride[ NUM_OF_VALVES ] = { VALVE_POSITION_C_CLOSE, VALVE_POSITION_C_CLOSE, 0, 0 };

// Self test function prototypes
static VALVE_SELF_TEST_STATE_T handleValveSelfTestEnableValves( void );
static VALVE_SELF_TEST_STATE_T handleValveSelfTestConfirmEnable( void );

// Exec function prototypes
static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve );
static VALVE_STATE_T handleValveStateHomingNotStarted( VALVE_T valve );
static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve );
static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve );
static VALVE_STATE_T handleValveStateIdle( VALVE_T valve );
static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve );

// Private function prototypes
static void setValveNextStep( VALVE_T valve );
static void execMonitorValves( void );
static BOOL areValvesEnabled( void );
static void setFPGAValue( VALVE_T valve, S16 position );
static void convertAndMonitorValvesCurrent( void );
static void getAndMonitorValvesCurrentPosition( void );
static DATA_GET_PROTOTYPE( U32, getPublishValvesDataInterval );
static void publishValvesData( VALVE_T valve );

/*********************************************************************//**
 * @brief
 * The initValves function initializes the valves driver.
 * @details
 * Inputs: valveSelfTestState, valvesSelfTestResult, valvesStatus
 * Outputs: valveSelfTestState, valvesSelfTestResult, valvesStatus
 * @return: none
 *************************************************************************/
void initValves( void )
{
    VALVE_T valve;
    valveSelfTestState   = VALVE_SELF_TEST_ENABLE_VALVES;
    valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS;

    // Initialize some of the variables
    for ( valve = VDI; valve < NUM_OF_VALVES; valve++ )
    {
        valvesStatus[ valve ].execState = VALVE_STATE_WAIT_FOR_POST;
    }
}

/*********************************************************************//**
 * @brief
 * The homeValve function sets the homing request flag of a valve to TRUE.
 * @details
 * Inputs: valvesStatus
 * Outputs: valvesStatus (The flag that sets the homing request)
 * @return: none
 *************************************************************************/
void homeValve( VALVE_T valve )
{
    valvesStatus[ valve ].hasHomingBeenRequested = TRUE;
}

/*********************************************************************//**
 * @brief
 * The setValvePosition function sets the requested position of a valve.
 * @details
 * Inputs: valvesStatus
 * Outputs: valvesStatus (commanded position)
 * @param: valve : Enumeration of the valve
 * @param: position : Enumeration of the commanded position
 * @return: returns TRUE if the requested position was legal
 *************************************************************************/
BOOL setValvePosition( VALVE_T valve, VALVE_POSITION_T position )
{
    BOOL result = FALSE;

    // Valve not in position cannot be requested
    // All the other positions are valid
    if ( position != VALVE_POSITION_NOT_IN_POSITION )
    {
        valvesStatus[ valve ].commandedPosition = position;
        valvesStatus[ valve ].hasTransitionBeenRequested = TRUE;
        result = TRUE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getValvePosition function returns the current position of a valve.
 * @details
 * Inputs: none
 * Outputs: valvesStatus (current position)
 * @param: valve : Enumeration of the valve
 * @param: position : Enumeration of the commanded position
 * @return: returns TRUE if the requested position was legal
 *************************************************************************/
VALVE_POSITION_T getValvePosition( VALVE_T valve )
{
    return valvesStatus[ valve ].currentPosition;
}

/*********************************************************************//**
 * @brief
 * The setValveBloodTrap function set the blood trap valve to open or close
 * position.
 * @details
 * Inputs: none
 * Outputs: none
 * @param: state enumeration of open or close for valve
 * @return: none
 *************************************************************************/
void setValveAirTrap( OPN_CLS_STATE_T state )
{
    BOOL energized = FALSE;

    if ( state == STATE_OPEN )
    {
        energized = TRUE;
    }

    gioSetBit( hetPORT1, AIR_TRAP_VALVE_GPIO_PIN, energized );
}

/*********************************************************************//**
 * @brief
 * The execValvesSelfTest function executes the valves self test.
 * @details
 * Inputs: valveSelfTestState
 * Outputs: valveSelfTestState
 * @return: valvesSelfTestResult : The status of the self test
 *************************************************************************/
SELF_TEST_STATUS_T execValvesSelfTest( void )
{
    switch( valveSelfTestState )
    {
        case VALVE_SELF_TEST_ENABLE_VALVES:
            valveSelfTestState = handleValveSelfTestEnableValves();
            break;

        case VALVE_SELF_TEST_CONFIRM_ENABLE:
            valveSelfTestState = handleValveSelfTestConfirmEnable();
            break;

        case VALVE_SELF_TEST_COMPLETE:
            // Do nothing POST completed
            break;

        default:
            //TODO alarm
            valveSelfTestState = VALVE_SELF_TEST_COMPLETE;
    }

    return valvesSelfTestResult;
}

/*********************************************************************//**
 * @brief
 * The execValves function executes the valves exec state machine.
 * @details
 * Inputs: none
 * Outputs: none (All the values are stored in the structure of each valve)
 * @return: none
 *************************************************************************/
void execValves( void )
{
    VALVE_T valve;
    VALVE_STATE_T state;

    execMonitorValves();

    // TODO un-comment the for loop
    //for ( valve = VDI; valve < NUM_OF_VALVES; valve++ )
    {
        //TODO FOR TESTING ONLY REMOVE
        valve = VBA;
        //TODO FOR TESTING ONLY REMOVE

        switch ( valvesStatus[ valve ].execState )
        {
            case VALVE_STATE_WAIT_FOR_POST:

                state = handleValveStateWaitForPost( valve );
                break;

            case VALVE_STATE_HOMING_NOT_STARTED:

                state = handleValveStateHomingNotStarted( valve );
                break;

            case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE:

                state = handleValveStateHomingFindEnergizedEdge( valve );
                break;

            case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE:

                state = handleValveStateHomingFindDeenergizedEdge( valve );
                break;

            case VALVE_STATE_IDLE:

                state = handleValveStateIdle( valve );
                break;

            case VALVE_STATE_IN_TRANSITION:

                state = handleValveStateInTransition( valve );
                break;

            default:
                break;
        }

        valvesStatus[ valve ].execState = state;

        publishValvesData( valve );
    }
}

/*********************************************************************//**
 * @brief
 * The handleValveSelfTestEnableValves function starts the valves self test.
 * @details
 * Inputs: valvesSelfTestResult
 * Outputs: nvalvesSelfTestResult
 * @return: Next state of the self test state machine
 *************************************************************************/
static VALVE_SELF_TEST_STATE_T handleValveSelfTestEnableValves( void )
{
    VALVE_SELF_TEST_STATE_T state = VALVE_SELF_TEST_CONFIRM_ENABLE;

    valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS;

    // Enable the PID controller of all of the valves
    enableFPGAValvesPIDControl( ALL_VALVES_ENABLE_PID_INTERFACE );

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleValveSelfTestConfirmEnable function checks to make sure the
 * valves are enabled properly.
 * @details
 * Inputs: valvesSelfTestResult
 * Outputs: valvesSelfTestResult
 * @return: Next state of the self test state machine
 *************************************************************************/
static VALVE_SELF_TEST_STATE_T handleValveSelfTestConfirmEnable( void )
{
    VALVE_SELF_TEST_STATE_T state = VALVE_SELF_TEST_CONFIRM_ENABLE;

    // TODO do we need to wait for 50ms?
    if ( areValvesEnabled() )
    {
        valvesSelfTestResult = SELF_TEST_STATUS_PASSED;
        state = VALVE_SELF_TEST_COMPLETE;
    }
    else
    {
        valvesSelfTestResult = SELF_TEST_STATUS_FAILED;
        state = VALVE_SELF_TEST_COMPLETE;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleValveStateWaitForPost function handles the wait for POST \n
 * state.
 * @details
 * Inputs: valveSelfTestState, valveSelfTestState
 * Outputs: valveSelfTestState (the next exec state of a valve)
 * @return: none
 *************************************************************************/
static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_WAIT_FOR_POST;

    if ( valveSelfTestState == VALVE_SELF_TEST_COMPLETE )
    {
        state = VALVE_STATE_HOMING_NOT_STARTED;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleValveStateHomingNotStarted function handles homing not started \n
 * state of the state machine.
 * @details
 * Inputs: valvesStatus, valveSelfTestState
 * Outputs: valvesStatus
 * @return: none
 *************************************************************************/
static VALVE_STATE_T handleValveStateHomingNotStarted( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_HOMING_NOT_STARTED;

    // TODO connect this flag to the doors driver later
    // For now it is assumed the door is closed
    BOOL isDoorClosed = TRUE;

    // If homing has been requested or POST is completed and the door has been close for the specified
    // period of time, start the homing
    if ( ( valvesStatus[ valve ].hasHomingBeenRequested || valveSelfTestState == VALVE_SELF_TEST_COMPLETE )
            && isDoorClosed && ! valvesStatus[ valve ].hasHomingFailed )
    {
        // Get ready for the energized state
        valvesStatus[ valve ].homingEdgeDetectionCounter = 0;
        valvesStatus[ valve ].targetPositionInCounts     = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS;
        valvesStatus[ valve ].hasValveBeenHomed          = FALSE;
        valvesStatus[ valve ].currentPosition            = VALVE_POSITION_NOT_IN_POSITION;
        setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts );
        state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE;
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleValveStateHomingFindEnergizedEdge function handles find \n
 * energized state of the state machine.
 * @details
 * Inputs: valvesStatus, valveSelfTestState
 * Outputs: valvesStatus
 * @return: none
 *************************************************************************/
static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE;

    S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts;
    S16 targetPosition  = valvesStatus[ valve ].targetPositionInCounts;
    S16 deltaPosition   = targetPosition - currentPosition;

    valvesStatus[ valve ].homingEdgeDetectionCounter++;

    // If there has not been any major travel for a certain period of time
    if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS &&
         valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL )
    {
        // Current position (positive or negative) will be stored in the Position B of the current valve
        valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition;
        valvesStatus[ valve ].homingEdgeDetectionCounter         = 0;
        valvesStatus[ valve ].targetPositionInCounts             = currentPosition - STEP_CHANGE_IN_COUNTS;
        setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts );

        state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE;
    }
    else if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS )
    {
        if ( valvesStatus[ valve ].valveRestCounter >= 0 )//TODO add a #define for the rest time. Do we need the rest?
        {
            valvesStatus[ valve ].homingEdgeDetectionCounter = 0;
            valvesStatus[ valve ].targetPositionInCounts     = currentPosition + STEP_CHANGE_IN_COUNTS;
            setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts );
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleValveStateHomingFindDeenergizedEdge function handles find \n
 * de-energized state of the state machine.
 * @details.
 * Inputs: valvesStatus, valveSelfTestState
 * Outputs: valvesStatus
 * @return: none
 *************************************************************************/
static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve )
{
    VALVE_STATE_T state   = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE;

    S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts;
    S16 targetPosition  = valvesStatus[ valve ].targetPositionInCounts;
    S16 deltaPosition   = targetPosition - currentPosition;

    valvesStatus[ valve ].homingEdgeDetectionCounter++;

    if ( abs(deltaPosition) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS &&
         valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL )
    {
        S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ];
        S16 deltaEdges = energizedEdge - currentPosition;

        if ( deltaEdges <= INITIAL_ENERGIZED_EDGE_UPPER_RANGE && deltaEdges >= INITIAL_ENERGIZED_EDGE_LOWER_RANGE )
        {
             S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ];
             // Positions B and C will have an offset from the edge to make sure each time the valve will not hit the edge
             valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]  = positionB - INITIAL_EDGE_OFFSET_READ_COUNT;
             valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] = currentPosition + INITIAL_EDGE_OFFSET_READ_COUNT;
             // Position A is the average of the Position B that was read last time and position C that was the target of this state
             valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ] = ( valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] +
                                                                                  abs(valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ]) ) / 2;

             // Set the current position to Position C and the commanded position to Position A         // Request a transition
             valvesStatus[ valve ].hasValveBeenHomed          = TRUE;
             valvesStatus[ valve ].currentPosition            = VALVE_POSITION_C_CLOSE;
             valvesStatus[ valve ].commandedPosition          = VALVE_POSITION_A_INSERT_EJECT;
             valvesStatus[ valve ].hasTransitionBeenRequested = TRUE;
             valvesStatus[ valve ].hasHomingBeenRequested     = FALSE;
             valvesStatus[ valve ].numberOfFailedHomings      = 0;

             // Idle will initiate a transition
             state = VALVE_STATE_IDLE;
         }
         // Max number of failed homing. Fault and go back to homing not started
         else if ( valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS )
         {
             //TODO Alarm
             valvesStatus[ valve ].hasValveBeenHomed = FALSE;
             valvesStatus[ valve ].hasHomingFailed = TRUE;
             state = VALVE_STATE_HOMING_NOT_STARTED;
         }
         else
         {
             valvesStatus[ valve ].numberOfFailedHomings++;
             state = VALVE_STATE_HOMING_NOT_STARTED;
         }

    }
    else if ( abs(deltaPosition) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS )
    {
        if ( valvesStatus[ valve ].valveRestCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest?
        {
            valvesStatus[ valve ].homingEdgeDetectionCounter = 0;
            valvesStatus[ valve ].targetPositionInCounts     = currentPosition - STEP_CHANGE_IN_COUNTS;
            setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts );
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleValveStateIdle function handles the idle state of the valves \n
 * state machine.
 * @details
 * Inputs: valveSelfTestState, valveSelfTestState
 * Outputs: valveSelfTestState (the next exec state of a valve)
 * @return: none
 *************************************************************************/
static VALVE_STATE_T handleValveStateIdle( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_IDLE;

    if ( valvesStatus[ valve ].hasHomingBeenRequested )
    {
        state = VALVE_STATE_HOMING_NOT_STARTED;
    }
    else if ( valvesStatus[ valve ].hasValveBeenHomed &&
              valvesStatus[ valve ].hasTransitionBeenRequested )
    {
        if ( valvesStatus[ valve ].currentPosition != valvesStatus[ valve ].commandedPosition )
        {
            // Just set the valves to transition so it will not be in a known position for a while
            valvesStatus[ valve ].transitionStartTime = getMSTimerCount();
            setValveNextStep( valve );

            state = VALVE_STATE_IN_TRANSITION;
        }
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The handleValveStateInTransition function handles the in transition \n
 * state of the state machine.
 * @details
 * Inputs: valvesStatus (commanded position, current position, current position \n
 * in counts, has transition been requested flag)
 * Outputs: valvesStatus
 * @return: none
 *************************************************************************/
static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_IN_TRANSITION;

    VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition;

    // Get the corresponding counts of the positions
    S16 commandedPosition = valvesStatus[ valve ].positions[ commandedPositionEnum ];
    S16 currentPosition   = valvesStatus[ valve ].currentPositionInCounts;
    S16 targetPosition    = valvesStatus[ valve ].targetPositionInCounts;

    // Check if the valve is within range of the commanded position
    if ( abs( currentPosition - commandedPosition ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS )
    {
        // The valve is in range. Set the current position to the commanded position
        valvesStatus[ valve ].currentPosition = commandedPositionEnum;
        // Set the transition request to false as it has been serviced
        valvesStatus[ valve ].hasTransitionBeenRequested = FALSE;
        // Go back to Idle state
        state = VALVE_STATE_IDLE;
    }
    // Check if the valve's transition time has timed out
    else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) )
    {
        // TODO raise an alarm
        valvesStatus[ valve ].hasTransitionBeenRequested = FALSE;
        // Go back to Idle state
        state = VALVE_STATE_IDLE;
    }
    //TODO not sure if this works or not, Test this
    else if ( currentPosition - targetPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS )
    {
        setValveNextStep( valve );
    }

    return state;
}

/*********************************************************************//**
 * @brief
 * The execMonitorValves function handles monitoring of the valves. The \n
 * function calls other functions to monitor the values.
 * @details
 * Inputs: none
 * Outputs: none
 * @return: none
 *************************************************************************/
static void execMonitorValves( void )
{
    // Check if the valves are still enabled
    areValvesEnabled();

    // Get the current position of the valves in counts and store them
    getAndMonitorValvesCurrentPosition();

    // Get the current in ADC and convert them to amps
    // Check whether any of the valves are over current
    convertAndMonitorValvesCurrent();
}

/*********************************************************************//**
 * @brief
 * The areValvesEnabled function checks the current valves status from FPGA \n
 * to the enable bit mask of each valve. If any of the valves is not enabled \n
 * the function raises an alarm.
 * @details
 * Inputs: none
 * Outputs: none
 * @return: Returns TRUE if all the valves are enabled properly
 *************************************************************************/
static BOOL areValvesEnabled( void )
{
    //TODO alarm for the valve status
    BOOL result = TRUE;

    U16 status = getFPGAValvesStatus();

    // VDi status
    if ( ! ( status & VDI_INIT_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( ! ( status & VDI_ENABLE_PID_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( status & VDI_ENABLE_BYPASS_STATUS_BIT_MASK )
    {
        result = FALSE;
    }

    // VDo status
    if ( ! ( status & VDO_INIT_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( ! ( status & VDO_ENABLE_PID_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( status & VDO_ENABLE_BYPASS_STATUS_BIT_MASK )
    {
        result = FALSE;
    }

    // VBA status
    if ( ! ( status & VBA_INIT_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( ! ( status & VBA_ENABLE_PID_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( status & VBA_ENABLE_BYPASS_STATUS_BIT_MASK )
    {
        result = FALSE;
    }

    // VBV status
    if ( ! ( status & VBV_INIT_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( ! ( status & VBV_ENABLE_PID_STATUS_BIT_MASK ) )
    {
        result = FALSE;
    }
    if ( status & VBV_ENABLE_BYPASS_STATUS_BIT_MASK )
    {
        result = FALSE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The setFPGAValue function sets the position of a valve to FPGA.
 * @details
 * Inputs: none
 * Outputs: none
 * @param: valve : Enumeration of the valve
 * @param: position : Position in counts
 * @return: none
 *************************************************************************/
static void setFPGAValue( VALVE_T valve, S16 position )
{
    switch ( valve )
    {
        case VDI:

            setFPGAValveDialyzerInletPosition( position );
            break;

        case VDO:

            setFPGAValveDialyzerOutletPosition( position );
            break;

        case VBA:

            setFPGAValveBloodArterialPosition( position );
            break;

        case VBV:

            setFPGAValveBloodVenousPosition( position );
            break;

        default:
            // TODO alarm
            break;
    }
}

/*********************************************************************//**
 * @brief
 * The convertAndMonitorValvesCurrent function gets the current values \n
 * in counts from FPGA and converts them to current. If any of the values \n
 * is above the threshold, the function will raise an alarm.
 * @details
 * Inputs: valvesStatus
 * Outputs: valvesStatus
 * @return: none
 *************************************************************************/
static void convertAndMonitorValvesCurrent( void )
{
    VALVE_T valve;
    U16 currentInADC = 0;

    currentInADC = getFPGAValveDialyzerInletCurrentCounts();
    valvesStatus[ VDI ].valveCurrent = ( (F32)currentInADC / ADC_TO_CURRENT_CONVERSION_CONSTANT ) - 1;

    currentInADC = getFPGAValveDialyzerOutletCurrentCounts();
    valvesStatus[ VDO ].valveCurrent = ( (F32)currentInADC / ADC_TO_CURRENT_CONVERSION_CONSTANT ) - 1;

    currentInADC = getFPGAValveBloodVenousCurrentCounts();
    valvesStatus[ VBA ].valveCurrent = ( (F32)currentInADC / ADC_TO_CURRENT_CONVERSION_CONSTANT ) - 1;

    currentInADC = getFPGAValveBloodArterialCurrentCounts();
    valvesStatus[ VBV ].valveCurrent = ( (F32)currentInADC / ADC_TO_CURRENT_CONVERSION_CONSTANT ) - 1;

    // Check the current of all the valves
    for ( valve = VDI; valve < NUM_OF_VALVES; valve++)
    {
        // Check if the current is over the threshold for the defined amount of time
        // Absolute value was used to cover both positive and negative ranges
        if ( abs(valvesStatus[ valve ].valveCurrent) > VALVES_CURRENT_THRESHOLD_AMPS &&
                ++valvesStatus[ valve ].valveOverCurrentCounter > OVER_CURRENT_COUNTER )
        {
            //TODO fault
        }
        // If the current is below the threshold again and the counter for the time is greater than
        // zero, set the counter to zero. Absolute value is used here to cover both positive and negative ranges
        else if ( abs(valvesStatus[ valve ].valveCurrent) < VALVES_CURRENT_THRESHOLD_AMPS &&
                  valvesStatus[ valve ].valveOverCurrentCounter > 0 )
        {
            valvesStatus[ valve ].valveOverCurrentCounter = 0;
        }
    }
}

/*********************************************************************//**
 * @brief
 * The getAndMonitorValvesCurrentPosition function gets the current position \n
 * of the valves and stores them into the structure of each valve. The function \n
 * checks whether any of the valves have deviated from their current position \n
 * if they are in idle state. If any of the valves have deviated, the function \n
 * raises an alarm.
 * @details
 * Inputs: valvesStatus
 * Outputs: valvesStatus
 * @return: none
 *************************************************************************/
static void getAndMonitorValvesCurrentPosition( void )
{
    VALVE_T valve;
    S16 currentPostion = 0;
    S16 commandedPoistion = 0;
    VALVE_POSITION_T commandedPositionEnum;

    // Get the position of the valves and update the structure of each valve
    valvesStatus[ VDI ].currentPositionInCounts = getFPGAValveDialyzerInletPosition();
    valvesStatus[ VDO ].currentPositionInCounts = getFPGAValveDialyzerOutletPosition();
    valvesStatus[ VBA ].currentPositionInCounts = getFPGAValveBloodArterialPosition();
    valvesStatus[ VBV ].currentPositionInCounts = getFPGAValveBloodVenousPosition();

    // Check the position of each valve
    for ( valve = VDI; valve < NUM_OF_VALVES; valve++ )
    {
        // Check the valve is in Idle state, otherwise the position should not be checked
        if ( valvesStatus[ valve ].execState == VALVE_STATE_IDLE )
        {
            currentPostion = valvesStatus[ valve ].currentPositionInCounts;
            commandedPositionEnum = valvesStatus[ valve ].commandedPosition;
            commandedPoistion = valvesStatus[ valve ].positions[ commandedPositionEnum ];

            // Check if the current position has deviated from the position it is supposed to be in
            // for more than a certain amount of time. If it has, raise an alarm
            // Absolute value is used for comparison to cover +/- from the commanded position
            if ( abs( currentPostion - commandedPoistion ) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS )
            {
                valvesStatus[ valve ].valvePositionOutOfRangeCounter++;
            }

            if ( valvesStatus[ valve ].valvePositionOutOfRangeCounter >  OUT_OF_RANGE_COUNTER )
            {
                //TODO fault
            }
            else
            {
                valvesStatus[ valve ].valvePositionOutOfRangeCounter = 0;
            }
        }
    }
}

/*********************************************************************//**
 * @brief
 * The getPublishValvesDataInterval function gets the valves data publish \n
 * interval.
 * @details
 * Inputs: valvesDataPublishInterval
 * Outputs: none
 * @return: The data publish time interval
 *************************************************************************/
U32 getPublishValvesDataInterval( void )
{
    U32 result = valvesDataPublishInterval.data;

    if ( OVERRIDE_KEY == valvesDataPublishInterval.override )
    {
        result = valvesDataPublishInterval.ovData;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The publishValvesData function publishes the data of the valves at the \n
 * specified time interval.
 * @details
 * Inputs: valvesStatus
 * Outputs: valvesStatus
 * @return: none
 *************************************************************************/
static void publishValvesData( VALVE_T valve )
{
    // Check the counter
    if ( ++valvesStatus[ valve ].valveDataPublishCounter > getPublishValvesDataInterval() )
    {
        HD_VALVE_DATA_T valveData;
        valveData.valveID      = (U32)valve;
        valveData.state        = (U32)valvesStatus[ valve ].execState;
        valveData.currentPosID = (U32)valvesStatus[ valve ].currentPosition;
        valveData.currentPos   = valvesStatus[ valve ].currentPositionInCounts;
        valveData.nextPos      = valvesStatus[ valve ].targetPositionInCounts;
        valveData.current      = valvesStatus[ valve ].valveCurrent;
        valveData.posC         = valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ];
        valveData.posA         = valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ];
        valveData.posB         = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ];

        broadcastHDValves( &valveData );

        valvesStatus[ valve ].valveDataPublishCounter = 0;
    }

    // TODO REMOVE Fast 10ms publish
    fastDataRemoveLater.currentPos = valvesStatus[ valve ].currentPositionInCounts;
    fastDataRemoveLater.current = valvesStatus[ valve ].valveCurrent;
    fastDataRemoveLater.cmdPosition = valvesStatus[ valve ].targetPositionInCounts;
    broadcastFastTempHDValves();
    // TODO REMOVE
}

static void setValveNextStep( VALVE_T valve )
{
    S16 nextStep = 0;
    VALVE_POSITION_T currentPositionEnum   = valvesStatus[ valve ].currentPosition;
    VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition;
    S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts;
    S16 commandedPosition = valvesStatus[ valve ].positions[ commandedPositionEnum ];

    if ( abs( currentPosition - commandedPosition ) <= STEP_CHANGE_IN_COUNTS )
    {
        nextStep = abs( currentPosition - commandedPosition );
    }
    else
    {
        nextStep = STEP_CHANGE_IN_COUNTS;
    }

    // If the valve is currently in position A and its commanded position is position B,
    // add the defined number of steps for the next transition
    // If the valve is currently in position A and its commanded position is position C,
    // subtract the defined number of steps for the next transition
    if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT )
    {
        if ( commandedPositionEnum == VALVE_POSITION_B_OPEN )
        {
            valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep;
        }
        if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE )
        {
            valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep;
        }
    }
    // If the valve is currently in position B, subtract the defined number of steps for the next transition
    else if ( currentPositionEnum == VALVE_POSITION_B_OPEN )
    {
        valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep;
    }
    // If the valve is currently in position C, add the defined number of steps for the next transition
    else if ( currentPositionEnum == VALVE_POSITION_C_CLOSE )
    {
        valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep;
    }

    setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts );
}

/*************************************************************************
 *                       TEST SUPPORT FUNCTIONS
 *************************************************************************/

/*********************************************************************//**
 * @brief
 * The testSetValvesDataPublishInterval function overrides the valves \n
 * data publish interval.
 * @details
 * Inputs: valvesDataPublishInterval
 * Outputs: valvesDataPublishInterval
 * @param: value : override valve data publish interval with (in ms)
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetValvesDataPublishInterval( U32 value )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        U32 intvl = value / TASK_PRIORITY_INTERVAL;

        result = TRUE;
        valvesDataPublishInterval.ovData   = intvl;
        valvesDataPublishInterval.override = OVERRIDE_KEY;
    }

    return result;
}


/*********************************************************************//**
 * @brief
 * The testResetValvesDataPublishInterval function resets the override \n
 * of the valves publish interval.
 * @details
 * Inputs: valvesDataPublishInterval
 * Outputs: valvesDataPublishInterval
 * @return TRUE if override reset successful, FALSE if not
 *************************************************************************/
BOOL testResetValvesDataPublishInterval( void )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        result = TRUE;
        valvesDataPublishInterval.override = OVERRIDE_RESET;
        valvesDataPublishInterval.ovData   = valvesDataPublishInterval.ovInitData;
    }

    return result;
}


/*********************************************************************//**
 * @brief
 * The testSetValvesPositionOverride function overrides the valves \n
 * position.
 * @details
 * Inputs: valvesPositionOverride, valvesStatus
 * Outputs: valvesPositionOverride, valvesStatus
 * @param valve The valve ID
 * @param position The position the valve has to transition to
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetValvesPositionOverride( U32 valve, U32 position )
{
    BOOL result = FALSE;

    if ( valve < NUM_OF_VALVES )
    {
        if ( TRUE == isTestingActivated() )
        {
            // Get the current position of the valve as the initial position
            valvesPositionOverride[ valve ].ovInitData = (U32)valvesStatus[ valve ].currentPosition;
            valvesPositionOverride[ valve ].ovData     = position;
            valvesPositionOverride[ valve ].override   = OVERRIDE_KEY;

            result = setValvePosition( (VALVE_T)valve, (VALVE_POSITION_T)position );
        }
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetValvesDataPublishInterval function resets the override \n
 * of the valves publish interval.
 * @details
 * Inputs: valvesDataPublishInterval
 * Outputs: valvesDataPublishInterval
 * @param valve The valve ID
 * @return TRUE if override reset successful, FALSE if not
 *************************************************************************/
BOOL testResetValvesPositionOverride( U32 valve )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        valvesPositionOverride[ valve ].override = OVERRIDE_RESET;
        valvesPositionOverride[ valve ].ovData   = valvesPositionOverride[ valve ].ovInitData;

        result = setValvePosition( (VALVE_T)valve, (VALVE_POSITION_T)valvesPositionOverride[ valve ].ovInitData );
    }

    return result;
}

/**@}*/