/**************************************************************************
 *
 *  Copyright (c) 2019-2019 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 BloodFlow.c
 *
 *  @date 05-Nov-2019
 *  @author S. Nash
 *
 *  @brief Monitor/Controller for blood pump and flow sensor.
 *
 **************************************************************************/

#include "can.h"
#include "etpwm.h"

#include "Common.h"
#include "SystemCommMessages.h"
#include "TaskPriority.h"
#include "Timers.h"
#include "BloodFlow.h"

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

#define MAX_BLOOD_FLOW_RATE                 600     // mL/min
#define MAX_BLOOD_PUMP_PWM_STEP_CHANGE      0.005   // duty cycle

typedef enum BloodPump_States
{
    BLOOD_PUMP_OFF_STATE = 0,
    BLOOD_PUMP_RAMPING_UP_STATE,
    BLOOD_PUMP_RAMPING_DOWN_STATE,
    BLOOD_PUMP_CONTROL_TO_TARGET_STATE,
    NUM_OF_BLOOD_PUMP_STATES
} BLOOD_PUMP_STATE_T;

typedef enum BloodFlow_Self_Test_States
{
    BLOOD_FLOW_SELF_TEST_STATE_START = 0,
    BLOOD_FLOW_TEST_STATE_IN_PROGRESS,
    BLOOD_FLOW_TEST_STATE_COMPLETE,
    NUM_OF_BLOOD_FLOW_SELF_TEST_STATES
} BLOOD_FLOW_SELF_TEST_STATE_T;

// CAN3 port pin assignments for pump stop and direction outputs
#define STOP_CAN3_PORT_MASK         0x00000002  // (Tx - re-purposed as output GPIO)
#define DIR_CAN3_PORT_MASK          0x00000002  // (Rx - re-purposed as output GPIO)
// blood pump stop and direction macros
#define SET_BP_DIR()                {canREG3->RIOC |= DIR_CAN3_PORT_MASK;}
#define SET_BP_STOP()               {canREG3->TIOC |= STOP_CAN3_PORT_MASK;}
#define CLR_BP_DIR()                {canREG3->RIOC &= ~DIR_CAN3_PORT_MASK;}
#define CLR_BP_STOP()               {canREG3->TIOC &= ~STOP_CAN3_PORT_MASK;}

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

static BLOOD_PUMP_STATE_T bloodPumpState = BLOOD_PUMP_OFF_STATE;    // current state of blood flow controller state machine
static BOOL isBloodPumpOn = FALSE;                                  // blood pump is currently running
static U32 bloodPumpTargetFlowRate = 0;                             // requested blood flow rate
static U32 bloodPumpTargetFlowRateSet = 0;                          // currently set blood flow rate
static F32 bloodPumpPWMDutyCyclePct = 0.0;                          // initial blood pump PWM duty cycle
static F32 bloodPumpPWMDutyCyclePctSet = 0.0;                       // currently set blood pump PWM duty cycle
static MOTOR_DIR_T bloodPumpDirection = MOTOR_DIR_FORWARD;          // requested blood flow direction
static MOTOR_DIR_T bloodPumpDirectionSet = MOTOR_DIR_FORWARD;       // currently set blood flow direction

static BLOOD_FLOW_SELF_TEST_STATE_T bloodPumpSelfTestState = BLOOD_FLOW_SELF_TEST_STATE_START;
static U32 bloodPumpSelfTestTimerCount = 0;

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

static BLOOD_PUMP_STATE_T handleBloodPumpOffState( void );
static BLOOD_PUMP_STATE_T handleBloodPumpRampingUpState( void );
static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void );
static BLOOD_PUMP_STATE_T handleBloodPumpControlToTargetState( void );
static void stopBloodPump( void );
static void releaseBloodPumpStop( void );
static void setBloodPumpDirection( MOTOR_DIR_T dir );

/*************************************************************************
 * @brief initBloodFlow
 * The initBloodFlow function initializes the BloodFlow module.
 * @details
 * Inputs : none
 * Outputs : BloodFlow module initialized.
 * @param none
 * @return none
 *************************************************************************/
void initBloodFlow( void )
{
    stopBloodPump();
    setBloodPumpDirection( MOTOR_DIR_FORWARD );
}

/*************************************************************************
 * @brief setBloodPumpTargetFlowRate
 * The setBloodPumpTargetFlowRate function sets a new target flow rate and
 * pump direction.
 * @details
 * Inputs : isBloodPumpOn, bloodPumpDirectionSet
 * Outputs : bloodPumpTargetFlowRate, bloodPumpdirection, bloodPumpPWMDutyCyclePct
 * @param flowRate : new target blood flow rate
 * @param dir : new blood flow direction
 * @return TRUE if new flow rate & dir are set, FALSE if not
 *************************************************************************/
BOOL setBloodPumpTargetFlowRate(  U32 flowRate, MOTOR_DIR_T dir  )
{
    BOOL result = FALSE;

    // direction change while pump is running is not allowed
    if ( ( FALSE == isBloodPumpOn ) || ( 0 == flowRate ) || ( dir == bloodPumpDirectionSet ) )
    {
        // verify flow rate
        if ( flowRate <= MAX_BLOOD_FLOW_RATE )
        {
            bloodPumpTargetFlowRate = flowRate;
            bloodPumpDirection = dir;
            // TODO - this is temporary conversion to initial duty cycle
            bloodPumpPWMDutyCyclePct = ( (F32)flowRate / 800.0 );
            switch ( bloodPumpState )
            {
                case BLOOD_PUMP_RAMPING_UP_STATE:  // see if we need to reverse direction of ramp
                    if ( bloodPumpPWMDutyCyclePct < bloodPumpPWMDutyCyclePctSet )
                    {
                        bloodPumpState = BLOOD_PUMP_RAMPING_DOWN_STATE;
                    }
                    break;
                case BLOOD_PUMP_RAMPING_DOWN_STATE:  // see if we need to reverse direction of ramp
                    if ( bloodPumpPWMDutyCyclePct > bloodPumpPWMDutyCyclePctSet )
                    {
                        bloodPumpState = BLOOD_PUMP_RAMPING_UP_STATE;
                    }
                    break;
                case BLOOD_PUMP_CONTROL_TO_TARGET_STATE:  // start ramp in appropriate direction
                    if ( bloodPumpTargetFlowRate < bloodPumpTargetFlowRateSet )
                    {
                        bloodPumpState = BLOOD_PUMP_RAMPING_DOWN_STATE;
                    }
                    else
                    {
                        bloodPumpState = BLOOD_PUMP_RAMPING_UP_STATE;
                    }
                    break;
                default:
                    // ok - do nothing
                    break;
            }
            result = TRUE;
        }
        else
        {
            // TODO - s/w fault?
        }
    }

    return result;
}

/*************************************************************************
 * @brief execBloodFlowMonitor
 * The execBloodFlowMonitor function executes the blood flow monitor.
 * @details
 * Inputs : none
 * Outputs : none
 * @param none
 * @return none
 *************************************************************************/
void execBloodFlowMonitor( void )
{
    // TODO
}

/*************************************************************************
 * @brief execBloodFlowController
 * The execBloodFlowController function executes the blood flow controller.
 * @details
 * Inputs : bloodPumpState
 * Outputs : bloodPumpState
 * @param none
 * @return none
 *************************************************************************/
void execBloodFlowController( void )
{
    switch ( bloodPumpState )
    {
        case BLOOD_PUMP_OFF_STATE:
            bloodPumpState = handleBloodPumpOffState();
            break;

        case BLOOD_PUMP_RAMPING_UP_STATE:
            bloodPumpState = handleBloodPumpRampingUpState();
            break;

        case BLOOD_PUMP_RAMPING_DOWN_STATE:
            bloodPumpState = handleBloodPumpRampingDownState();
            break;

        case BLOOD_PUMP_CONTROL_TO_TARGET_STATE:
            bloodPumpState = handleBloodPumpControlToTargetState();
            break;

        default:
            // TODO - s/w fault
            break;
    }
}

/*************************************************************************
 * @brief handleBloodPumpOffState
 * The handleBloodPumpOffState function handles the blood pump off state \n
 * of the blood pump controller state machine.
 * @details
 * Inputs : bloodPumpTargetFlowRate, bloodPumpDirection
 * Outputs : bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet, isBloodPumpOn
 * @param none
 * @return next state
 *************************************************************************/
static BLOOD_PUMP_STATE_T handleBloodPumpOffState( void )
{
    BLOOD_PUMP_STATE_T result = BLOOD_PUMP_OFF_STATE;

    // if we've been given a flow rate, setup ramp up and transition to ramp up state
    if ( bloodPumpTargetFlowRate > 0 )
    {
        // set initial PWM duty cycle
        bloodPumpPWMDutyCyclePctSet = MAX_BLOOD_PUMP_PWM_STEP_CHANGE;
        etpwmSetCmpA( etpwmREG1, (U32)(bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD)) );
        // allow blood pump to run in requested direction
        setBloodPumpDirection( bloodPumpDirection );
        releaseBloodPumpStop();
        // start PWM for blood pump
        etpwmStartTBCLK();
        isBloodPumpOn = TRUE;
        result = BLOOD_PUMP_RAMPING_UP_STATE;
    }

    return result;
}

/*************************************************************************
 * @brief handleBloodPumpRampingUpState
 * The handleBloodPumpRampingUpState function handles the ramp up state \n
 * of the blood pump controller state machine.
 * @details
 * Inputs : bloodPumpPWMDutyCyclePctSet
 * Outputs : bloodPumpPWMDutyCyclePctSet
 * @param none
 * @return next state
 *************************************************************************/
static BLOOD_PUMP_STATE_T handleBloodPumpRampingUpState( void )
{
    BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_UP_STATE;

    // have we been asked to stop the blood pump?
    if ( 0 == bloodPumpTargetFlowRate )
    {
        // start ramp down to stop
        bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_CHANGE;
        etpwmSetCmpA( etpwmREG1, (U32)(bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD)) );
        result = BLOOD_PUMP_RAMPING_DOWN_STATE;
    }
    // have we reached end of ramp up?
    else if ( bloodPumpPWMDutyCyclePctSet >= bloodPumpPWMDutyCyclePct )
    {
        bloodPumpTargetFlowRateSet = bloodPumpTargetFlowRate;
        result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE;
    }
    // continue ramp up
    else
    {
        bloodPumpPWMDutyCyclePctSet += MAX_BLOOD_PUMP_PWM_STEP_CHANGE;
        etpwmSetCmpA( etpwmREG1, (U32)(bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD)) );
    }

    return result;
}

/*************************************************************************
 * @brief handleBloodPumpRampingDownState
 * The handleBloodPumpRampingDownState function handles the ramp down state \n
 * of the blood pump controller state machine.
 * @details
 * Inputs : bloodPumpPWMDutyCyclePctSet
 * Outputs : bloodPumpPWMDutyCyclePctSet
 * @param none
 * @return next state
 *************************************************************************/
static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void )
{
    BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_DOWN_STATE;

    // have we essentially reached zero speed
    if ( bloodPumpPWMDutyCyclePctSet < MAX_BLOOD_PUMP_PWM_STEP_CHANGE )
    {
        isBloodPumpOn = FALSE;
        bloodPumpTargetFlowRateSet = 0;
        bloodPumpPWMDutyCyclePctSet = 0.0;
        etpwmSetCmpA( etpwmREG1, 0 );
        etpwmStopTBCLK();
        stopBloodPump();
        result = BLOOD_PUMP_OFF_STATE;
    }
    // have we reached end of ramp down?
    else if ( bloodPumpPWMDutyCyclePctSet <= bloodPumpPWMDutyCyclePct )
    {
        bloodPumpTargetFlowRateSet = bloodPumpTargetFlowRate;
        result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE;
    }
    // continue ramp down
    else
    {
        bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_CHANGE;
        etpwmSetCmpA( etpwmREG1, (U32)(bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD)) );
    }

    return result;
}

/*************************************************************************
 * @brief handleBloodPumpControlToTargetState
 * The handleBloodPumpControlToTargetState function handles the "control to \n
 * target" state of the blood pump controller state machine.
 * @details
 * Inputs : none
 * Outputs : bloodPumpState
 * @param none
 * @return next state
 *************************************************************************/
static BLOOD_PUMP_STATE_T handleBloodPumpControlToTargetState( void )
{
    BLOOD_PUMP_STATE_T result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE;

    // TODO - control

    return result;
}

/*************************************************************************
 * @brief stopBloodPump
 * The stopBloodPump function sets the blood pump stop signal.
 * @details
 * Inputs : none
 * Outputs : blood pump stop signal
 * @param none
 * @return none
 *************************************************************************/
static void stopBloodPump( void )
{
    SET_BP_STOP();
}

/*************************************************************************
 * @brief releaseBloodPumpStop
 * The releaseBloodPumpStop function clears the blood pump stop signal.
 * @details
 * Inputs : none
 * Outputs : blood pump stop signal
 * @param none
 * @return none
 *************************************************************************/
static void releaseBloodPumpStop( void )
{
    CLR_BP_STOP();
}

/*************************************************************************
 * @brief setBloodPumpDirection
 * The setBloodPumpDirection function sets the set blood pump direction to \n
 * the given direction.
 * @details
 * Inputs : bloodPumpState
 * Outputs : bloodPumpState
 * @param dir : blood pump direction to set
 * @return none
 *************************************************************************/
static void setBloodPumpDirection( MOTOR_DIR_T dir )
{
    switch ( dir )
    {
        case MOTOR_DIR_FORWARD:
            bloodPumpDirectionSet = dir;
            CLR_BP_DIR();
            break;

        case MOTOR_DIR_REVERSE:
            bloodPumpDirectionSet = dir;
            SET_BP_DIR();
            break;

        default:
            // TODO - s/w fault
            break;
    }
}