/*
 * Valves.c
 *
 *  Created on: Aug 7, 2020
 *      Author: fw
 */

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

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

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

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

#define VDI_INITIALIZATION_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 0x00BF

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

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

#define INITIAL_OFFSET_READ_COUNT      100
#define INITIAL_DEENERGIZED_POSITION ( 0 - INITIAL_OFFSET_READ_COUNT )
#define INITIAL_ENERGIZED_POSITION   (12000 + INITIAL_OFFSET_READ_COUNT )

#define MAX_ALLOWED_POSITION_DEVIATION_FROM_TARGET          100U //TODO test this tolerance
#define VALVE_TRANSITION_TIMEOUT_MS   50U //TODO test the timeout

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

/// Valve States
typedef enum valveStates
{
    VALVE_STATE_WAIT_FOR_POST = 0,
    VALVE_STATE_IDLE,
    VALVE_STATE_IN_TRANSITION,
    VALVE_STATE_HOMING_START,
    VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE,
    VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE,
    NUM_OF_VALVE_STATES,
} VALVE_STATE_T;

typedef enum homingStatus
{
    HOMING_NOT_STARTED = 0,
    HOMING_IN_PROGRESS,
    HOMING_COMPLETE,
    NUM_OF_HOMING_STATES
} HOMING_STATUS_T;

typedef struct
{
    VALVE_POSITION_T commandedPosition;
    VALVE_POSITION_T currentPosition;
    BOOL             hasTransitionBeenRequested;
    HOMING_STATUS_T  homingStatus;
    BOOL             hasPIDBeenEnabled; //TODO remove
    BOOL             hasHomingBeenRequested;
    U16              PIDEnableBit;
    S16              insertEjectPositionA;
    S16              openPositionB;
    S16              closedPositionC;
    U32              transitionStartTime;
} VALVE_STATUS_T;

static VALVE_STATE_T dialyzerInletValveState  = VALVE_STATE_WAIT_FOR_POST;
static VALVE_STATE_T dialyzerOutletValveState = VALVE_STATE_WAIT_FOR_POST;
static VALVE_STATE_T bloodArterialValveState  = VALVE_STATE_WAIT_FOR_POST;
static VALVE_STATE_T bloodVenousValveState    = VALVE_STATE_WAIT_FOR_POST;

static VALVE_STATUS_T valvesStatus [ NUM_OF_VALVE_POSITIONS ];

static void execMonitorValves( VALVE_T valve );

static void processSetPosition( VALVE_T valve, VALVE_POSITION_T position );

static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve );
static VALVE_STATE_T handleValveStateIdle( VALVE_T valve );
static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve );
static VALVE_STATE_T handleValveStateHomingStart( VALVE_T valve );
static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve );
static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve );

void initValves( void )
{
    U08 i;

    dialyzerInletValveState  = VALVE_STATE_WAIT_FOR_POST;
    dialyzerOutletValveState = VALVE_STATE_WAIT_FOR_POST;
    bloodArterialValveState  = VALVE_STATE_WAIT_FOR_POST;
    bloodVenousValveState    = VALVE_STATE_WAIT_FOR_POST;

    // Set the PID enable for the valves.
    valvesStatus[ VDI ].PIDEnableBit = VDI_ENABLE_PID_INTERFACE;
    valvesStatus[ VDO ].PIDEnableBit = VDO_ENABLE_PID_INTERFACE;
    valvesStatus[ VBA ].PIDEnableBit = VBA_ENABLE_PID_INTERFACE;
    valvesStatus[ VBV ].PIDEnableBit = VBV_ENABLE_PID_INTERFACE;

    // Enable the PID controller of all of the valves
    enableValvesPIDControl( VDI_ENABLE_PID_INTERFACE );
    enableValvesPIDControl( VDO_ENABLE_PID_INTERFACE );
    enableValvesPIDControl( VBA_ENABLE_PID_INTERFACE );
    enableValvesPIDControl( VBV_ENABLE_PID_INTERFACE );

    // Initialize some of the variables
    for ( i = 0; i < NUM_OF_VALVE_POSITIONS; i++ )
    {
        valvesStatus[ i ].homingStatus = HOMING_NOT_STARTED;
        valvesStatus[ i ].hasPIDBeenEnabled = TRUE;
    }
}

SELF_TEST_STATUS_T execValvesSelfTest( void )
{

}

void execValves( void )
{
    // VDi state machine
    switch ( dialyzerInletValveState )
    {
        case VALVE_STATE_WAIT_FOR_POST:

            dialyzerInletValveState = handleValveStateWaitForPost( VDI );
            break;

        case VALVE_STATE_IDLE:

            dialyzerInletValveState = handleValveStateIdle( VDI );
            break;

        case VALVE_STATE_IN_TRANSITION:

            dialyzerInletValveState = handleValveStateInTransition( VDI );
            break;

        case VALVE_STATE_HOMING_START:

            dialyzerInletValveState = handleValveStateHomingStart( VDI );
            break;

        case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE:

            dialyzerInletValveState = handleValveStateHomingFindEnergizedEdge( VDI );
            break;

        case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE:

            dialyzerInletValveState = handleValveStateHomingFindDeenergizedEdge( VDI );
            break;

        default:
            break;
    }
}

BOOL homeValve( VALVE_T valve )
{
    BOOL result = FALSE;

    valvesStatus [ valve ].hasHomingBeenRequested = TRUE;

    return result;
}

BOOL setValvePosition( VALVE_T valve, VALVE_POSITION_T position )
{
    valvesStatus[ valve ].commandedPosition = position;

    return TRUE;
}

S16 getValvePosition( VALVE_T valve)
{

}


BOOL setValveBloodTrap( VALVE_BLOOD_TRAP_STATE_T state )
{

}

static void execMonitorValves( VALVE_T valve )
{
    // TODO check current sense
    // TODO check enable status
    // TODO check timeout and out of tolerance here instead of in each state?
    switch ( valve )
    {
        case VDI:
            break;

        case VDO:
            break;

        case VBA:
            break;

        case VBV:
            break;

        default:
            break;
    }
}


static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_WAIT_FOR_POST;

    switch ( valve )
    {
        case VDI:
            //TODO change this to keep waiting
            state = VALVE_STATE_IDLE;
            // FOR TESTING ONLY
            break;

        case VDO:
            break;

        case VBA:
            break;

        case VBV:
            break;

        default:
            break;
    }

    return state;
}

static VALVE_STATE_T handleValveStateIdle( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_IDLE;

    if ( valvesStatus[ valve ].hasHomingBeenRequested )
    {
        state = VALVE_STATE_HOMING_START;
    }
    else if ( valvesStatus[ valve ].homingStatus == HOMING_COMPLETE &&
              valvesStatus[ valve ].hasTransitionBeenRequested )
    {
        if ( valvesStatus[ valve ].currentPosition != valvesStatus[ valve ].commandedPosition )
        {
            processSetPosition( valve, valvesStatus[ valve ].commandedPosition );
        }
    }

    return state;
}

static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve )
{

}


static VALVE_STATE_T handleValveStateHomingStart( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_HOMING_START;

    switch ( valve )
    {
        case VDI:

            setValveDialyzerInletPosition( INITIAL_ENERGIZED_POSITION );
            valvesStatus[ valve ].homingStatus = HOMING_IN_PROGRESS;
            valvesStatus[ valve ].transitionStartTime = getMSTimerCount();
            break;

        case VDO:
            break;

        case VBA:
            break;

        case VBV:
            break;

        default:
            break;
    }

    return state;
}

static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE;

    S16 position = 0;

    switch ( valve )
    {
        case VDI:

            // Read the energized position back
            position = getValveDialyzerInletPosition();

            if ( fabs( position - INITIAL_ENERGIZED_POSITION ) <= MAX_ALLOWED_POSITION_DEVIATION_FROM_TARGET )
            {
                // Fully de-energized state which is closed position
                valvesStatus[ valve ].closedPositionC = position;
                // Reset the timer for the next state
                valvesStatus[ valve ].transitionStartTime = getMSTimerCount();

                setValveDialyzerInletPosition( INITIAL_DEENERGIZED_POSITION );
                state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE;
            }
            else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) )
            {
                // TODO Fault alarm
                valvesStatus[ valve ].homingStatus = HOMING_COMPLETE;
                // Homing failed, go back to start
                state = VALVE_STATE_HOMING_START;
            }
            break;

        case VDO:
            break;

        case VBA:
            break;

        case VBV:
            break;

        default:
            break;
    }

    return state;
}

static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve )
{
    VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE;
    S16 position = 0;
    S16 avgPosition = 0;

    switch ( valve )
    {
        case VDI:

            // Read the energized position back
            position = getValveDialyzerInletPosition();

            if ( fabs( position - INITIAL_DEENERGIZED_POSITION ) <= MAX_ALLOWED_POSITION_DEVIATION_FROM_TARGET )
            {
                // Fully energized state which is open
                valvesStatus[ valve ].openPositionB = position;

                // Insert/eject position which is the average of the fully energized and de-energized states
                avgPosition = ( position + valvesStatus[ valve ].insertEjectPositionA ) / 2;

                // This is the homing value
                valvesStatus[ valve ].insertEjectPositionA = avgPosition;
                valvesStatus[ valve ].homingStatus = HOMING_COMPLETE;
                // Turn off the homing flag
                valvesStatus[ valve ].hasHomingBeenRequested = FALSE;
                // Done with homing
                state = VALVE_STATE_IDLE;
            }
            else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) )
            {
                // TODO alarm
                valvesStatus[ valve ].homingStatus = HOMING_COMPLETE;
                state = VALVE_STATE_HOMING_START;
            }
            break;

        case VDO:
            break;

        case VBA:
            break;

        case VBV:
            break;

        default:
            break;
    }

    return state;
}

static void processSetPosition( VALVE_T valve, VALVE_POSITION_T position )
{
    S16 setPoint = 0;

    switch ( position )
    {
        case VALVE_POSITION_A_INSERT_EJECT:

            setPoint = valvesStatus[ valve ].insertEjectPositionA;
            break;

        case VALVE_POSITION_B_OPEN:

            setPoint = valvesStatus[ valve ].openPositionB;
            break;

        case VALVE_POSITION_C_CLOSE:

            setPoint = valvesStatus[ valve ].closedPositionC;
            break;
    }


    switch ( valve )
    {
        case VDI:

            setValveDialyzerInletPosition ( setPoint );
            break;

        case VDO:
            break;

        case VBA:
            break;

        case VBV:
            break;

        default:
            break;
    }

}