/**********************************************************************//**
 *
 *  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 ROPump.c
 *
 *  @date 03-Apr-2020
 *  @author S. Nash
 *
 *  @brief Monitor/Controller for RO pump.
 *
 **************************************************************************/

#ifndef _VECTORCAST_
    #include <math.h>
#endif

#include "etpwm.h"
#include "mibspi.h"

#include "FPGA.h"
#include "OperationModes.h"
#include "PIControllers.h"
#include "Pressures.h"
#include "SystemCommMessages.h"
#include "TaskGeneral.h"
#include "TaskPriority.h"
#include "Timers.h"
#include "ROPump.h"

/**
 *  @addtogroup ROPump
 *  @{
 */

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

#define RO_PUMP_DATA_PUB_INTERVAL           ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL )  ///< interval (ms/task time) at which the RO Pump data is published on the CAN bus

#define MAX_RO_PUMP_PWM_STEP_CHANGE         0.01                ///< max duty cycle change for controller
#define MAX_RO_PUMP_PWM_DUTY_CYCLE          0.99                ///< max duty cycle
#define MIN_RO_PUMP_PWM_DUTY_CYCLE          0.00                ///< min duty cycle

#define ROP_CONTROL_INTERVAL                ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )   ///< interval (ms/task time) at which the RO pump is controlled
#define ROP_P_COEFFICIENT                   0.005               ///< P term for RO pump control
#define ROP_I_COEFFICIENT                   0.00025             ///< I term for RO pump control

#define ROP_PSI_TO_PWM_DC(p)                ( 0.2 + ( (F32)((p) - 100) * 0.01 ) )  ///< conversion factor from target PSI to PWM duty cycle estimate   TODO - this is a place holder for real conversion

#define RO_FLOW_ADC_TO_LPM_FACTOR           0.00018             ///< conversion factor from ADC counts to LPM (liters/min) for RO flow rate.

typedef enum ROPump_States
{
    RO_PUMP_OFF_STATE = 0,
    RO_PUMP_CONTROL_TO_TARGET_STATE,
    NUM_OF_RO_PUMP_STATES
} RO_PUMP_STATE_T;

typedef enum ROPump_Self_Test_States
{
    RO_PUMP_SELF_TEST_STATE_START = 0,
    RO_PUMP_TEST_STATE_IN_PROGRESS,
    RO_PUMP_TEST_STATE_COMPLETE,
    NUM_OF_RO_PUMP_SELF_TEST_STATES
} RO_PUMP_SELF_TEST_STATE_T;

// TODO - test code - remove later
// pin assignments for pump test DIP switch 0
#define RO_PUMP_TEST_SPI5_PORT_MASK     0x00000100  // (ENA - re-purposed as input GPIO)
// dialIn pump stop and direction macros
#define GET_DIP_SW0_TEST()              ( ( mibspiREG5->PC2 & RO_PUMP_TEST_SPI5_PORT_MASK ) != 0 )

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

static RO_PUMP_STATE_T roPumpState = RO_PUMP_OFF_STATE;                          ///< current state of RO pump controller state machine
static U32 roPumpDataPublicationTimerCounter = 0;                                ///< used to schedule RO pump data publication to CAN bus
static BOOL isROPumpOn = FALSE;                                                  ///< RO pump is currently running
static F32 roPumpPWMDutyCyclePct = 0.0;                                          ///< initial RO pump PWM duty cycle
static F32 roPumpPWMDutyCyclePctSet = 0.0;                                       ///< currently set RO pump PWM duty cycle
static PUMP_CONTROL_MODE_T roPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP;    ///< requested RO pump control mode.
static PUMP_CONTROL_MODE_T roPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< currently set RO pump control mode.

static OVERRIDE_U32_T roPumpDataPublishInterval = { 0, 0, 0, 0 };                ///< interval (in ms) at which to publish RO flow data to CAN bus.
static OVERRIDE_U32_T targetROPumpPressure = { 0, 0, 0, 0 };                     ///< Target RO pressure (in PSI).
static OVERRIDE_F32_T measuredROPumpPressure = { 0.0, 0.0, 0.0, 0 };             ///< measured RO pressure (in PSI).
static OVERRIDE_F32_T measuredROFlowRateLPM = { 0.0, 0.0, 0.0, 0 };              ///< measured RO flow rate (in LPM).

static U32 roControlTimerCounter = 0;                                            ///< determines when to perform control on ro pump

static RO_PUMP_SELF_TEST_STATE_T roPumpSelfTestState = RO_PUMP_SELF_TEST_STATE_START;  ///< current ro pump self test state
static U32 roPumpSelfTestTimerCount = 0;                                         ///< timer counter for ro pump self test

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

static RO_PUMP_STATE_T handleROPumpOffState( void );
static RO_PUMP_STATE_T handleROPumpControlToTargetState( void );
static void setROPumpControlSignalPWM( F32 newPWM );
static void stopROPump( void );
static void publishROPumpData( void );
static DATA_GET_PROTOTYPE( U32, getPublishROPumpDataInterval );

/*********************************************************************//**
 * @brief initROPump
 * The initROPump function initializes the ROPump module.
 * @details
 * Inputs : none
 * Outputs : ROPump module initialized.
 * @param none
 * @return none
 *************************************************************************/
void initROPump( void )
{
    stopROPump();

    // initialize RO pump PI controller
    initializePIController( PI_CONTROLLER_ID_RO_PUMP, MIN_RO_PUMP_PWM_DUTY_CYCLE,
                            ROP_P_COEFFICIENT, ROP_I_COEFFICIENT,
                            MIN_RO_PUMP_PWM_DUTY_CYCLE, MAX_RO_PUMP_PWM_DUTY_CYCLE );
}

/*********************************************************************//**
 * @brief
 * The setROPumpTargetPressure function sets a new target pressure for the \n
 * RO pump.
 * @details
 * Inputs : none
 * Outputs : targetROPumpPressure, roPumpPWMDutyCyclePct
 * @param roPressure : new target RO pressure
 * @param mode : new control mode
 * @return TRUE if new target pressure is set, FALSE if not
 *************************************************************************/
BOOL setROPumpTargetPressure( U32 roPressure, PUMP_CONTROL_MODE_T mode )
{
    BOOL result = FALSE;

    // verify pressure
    if ( roPressure >= MIN_RO_PRESSURE && roPressure <= MAX_RO_PRESSURE  )
    {
        targetROPumpPressure.data = roPressure;
        roPumpControlMode = mode;
        // set PWM duty cycle target to an estimated initial target based on target pressure - then we'll control to pressure going forward
        roPumpPWMDutyCyclePct = ROP_PSI_TO_PWM_DC( roPressure );

        result = TRUE;
    }
    else  // requested pressure out of range
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, 0, roPressure )     // TODO - replace 1st param with s/w fault enum
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The signalROPumpHardStop function stops the RO pump immediately.
 * @details
 * Inputs : none
 * Outputs : RO pump stopped, set point reset, state changed to off
 * @param none
 * @return none
 *************************************************************************/
void signalROPumpHardStop( void )
{
    targetROPumpPressure.data = 0;
    stopROPump();
    roPumpState = RO_PUMP_OFF_STATE;
    roPumpPWMDutyCyclePct = 0.0;
    roControlTimerCounter = 0;
    resetPIController( PI_CONTROLLER_ID_RO_PUMP, MIN_RO_PUMP_PWM_DUTY_CYCLE );
}

/*********************************************************************//**
 * @brief
 * The execROPumpMonitor function executes the RO Pump monitor.
 * @details
 * Inputs : none
 * Outputs : measuredROPumpPressure, measuredROFlowRateLPM
 * @param none
 * @return none
 *************************************************************************/
void execROPumpMonitor( void )
{
    S32 roFlow = (S32)getFPGAROPumpFlowRate();
    F32 roPressure = getMeasuredDGPressure( PRESSURE_SENSOR_RO_PUMP_OUTLET );

    measuredROFlowRateLPM.data = (F32)(roFlow) * RO_FLOW_ADC_TO_LPM_FACTOR;
    measuredROPumpPressure.data = (F32)((S16)roPressure);

    // check RO flow
    // TODO - check flow

    // check pressure while RO pump is on
    if ( TRUE == isROPumpOn )
    {
        if ( roPressure < MIN_RO_PRESSURE || roPressure > MAX_RO_PRESSURE )
        {
            SET_ALARM_WITH_1_F32_DATA( ALARM_ID_RO_PUMP_OUT_PRESSURE_OUT_OF_RANGE, roPressure )  // TODO - add persistence
        }
    }

    // publish RO pump data on interval
    publishROPumpData();
}

/*********************************************************************//**
 * @brief
 * The execROPumpController function executes the RO Pump controller.
 * @details
 * Inputs : roPumpState
 * Outputs : roPumpState
 * @param none
 * @return none
 *************************************************************************/
void execROPumpController( void )
{
    switch ( roPumpState )
    {
        case RO_PUMP_OFF_STATE:
            roPumpState = handleROPumpOffState();
            break;

        case RO_PUMP_CONTROL_TO_TARGET_STATE:
            roPumpState = handleROPumpControlToTargetState();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, 0, roPumpState )  // TODO - replace 1st param with s/w fault enum
            break;
    }
}

/*********************************************************************//**
 * @brief
 * The handleROPumpOffState function handles the ro pump off state \n
 * of the ro pump controller state machine.
 * @details
 * Inputs : targetROPumpPressure
 * Outputs : roPumpPWMDutyCyclePctSet, isROPumpOn
 * @param none
 * @return next state
 *************************************************************************/
static RO_PUMP_STATE_T handleROPumpOffState( void )
{
    RO_PUMP_STATE_T result = RO_PUMP_OFF_STATE;

    // TODO - test code - remove later
    if ( GET_DIP_SW0_TEST() )
    {
        setROPumpTargetPressure( 120, PUMP_CONTROL_MODE_CLOSED_LOOP );
    }

    // if we've been given a pressure, transition to control to target state
    if ( getTargetROPumpPressure() > 0 )
    {
        roPumpControlModeSet = roPumpControlMode;
        // set initial PWM duty cycle
        roPumpPWMDutyCyclePctSet = roPumpPWMDutyCyclePct;
        setROPumpControlSignalPWM( roPumpPWMDutyCyclePctSet );
        // reset controller
        resetPIController( PI_CONTROLLER_ID_RO_PUMP, MIN_RO_PUMP_PWM_DUTY_CYCLE );
        // set pump to on
        isROPumpOn = TRUE;
        result = RO_PUMP_CONTROL_TO_TARGET_STATE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleROPumpControlToTargetState function handles the "control to \n
 * target" state of the ro pump controller state machine.
 * @details
 * Inputs : none
 * Outputs : roPumpState
 * @param none
 * @return next state
 *************************************************************************/
static RO_PUMP_STATE_T handleROPumpControlToTargetState( void )
{
    RO_PUMP_STATE_T result = RO_PUMP_CONTROL_TO_TARGET_STATE;

    // control at set interval
    if ( ++roControlTimerCounter >= ROP_CONTROL_INTERVAL )
    {
        if ( roPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP )
        {
            F32 tgtPres = (F32)getTargetROPumpPressure();
            F32 actPres = getMeasuredROPumpPressure();
            F32 newPWM;

            newPWM = runPIController( PI_CONTROLLER_ID_RO_PUMP, tgtPres, actPres );
            roPumpPWMDutyCyclePctSet = newPWM;
            setROPumpControlSignalPWM( newPWM );
        }
        roControlTimerCounter = 0;
    }

    // TODO - test code - remove later
    if ( !GET_DIP_SW0_TEST() )
    {
        signalROPumpHardStop();
        result = RO_PUMP_OFF_STATE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The setROPumpControlSignalPWM function sets the PWM duty cycle for \n
 * the RO pump to a given %.
 * @details
 * Inputs : none
 * Outputs : PWM duty cycle zeroed
 * @param newPWM : new duty cycle % to apply to PWM
 * @return none
 *************************************************************************/
static void setROPumpControlSignalPWM( F32 newPWM )
{
    etpwmSetCmpB( etpwmREG2, (U32)( (S32)( ( newPWM * (F32)(etpwmREG2->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

/*********************************************************************//**
 * @brief
 * The stopROPump function sets the RO pump PWM to zero.
 * @details
 * Inputs : none
 * Outputs : PWM duty cycle zeroed
 * @param none
 * @return none
 *************************************************************************/
static void stopROPump( void )
{
    isROPumpOn = FALSE;
    roPumpPWMDutyCyclePctSet = 0.0;
    etpwmSetCmpB( etpwmREG2, 0 );
}

/*********************************************************************//**
 * @brief
 * The getPublishROPumpDataInterval function gets the RO pump data \n
 * publication interval.
 * @details
 * Inputs : roPumpDataPublishInterval
 * Outputs : none
 * @param none
 * @return the current RO pump data publication interval (in ms).
 *************************************************************************/
U32  getPublishROPumpDataInterval( void )
{
    U32 result = roPumpDataPublishInterval.data;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The getTargetROPumpPressure function gets the current target RO pump \n
 * pressure.
 * @details
 * Inputs : targetROPumpPressure
 * Outputs : none
 * @param none
 * @return the current target RO pressure (in PSI).
 *************************************************************************/
U32 getTargetROPumpPressure( void )
{
    U32 result = targetROPumpPressure.data;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The getMeasuredROPumpPressure function gets the measured RO pump \n
 * pressure.
 * @details
 * Inputs : measuredROPumpPressure
 * Outputs : none
 * @param none
 * @return the current RO pressure (in PSI).
 *************************************************************************/
F32  getMeasuredROPumpPressure( void )
{
    F32 result = measuredROPumpPressure.data;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The getMeasuredROFlowRate function gets the measured RO pump \n
 * flow rate.
 * @details
 * Inputs : roPumpSpeedRPM
 * Outputs : none
 * @param none
 * @return the current RO pump speed (in RPM).
 *************************************************************************/
F32 getMeasuredROFlowRate( void )
{
    F32 result = measuredROFlowRateLPM.data;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The publishROPumpData function publishes RO pump data at the set \n
 * interval.
 * @details
 * Inputs : target pressure, measured pressure, measured RO pump speed.
 * Outputs : RO pump data is published to CAN bus.
 * @return none
 *************************************************************************/
static void publishROPumpData( void )
{
    // publish RO pump data on interval
    if ( ++roPumpDataPublicationTimerCounter >= getPublishROPumpDataInterval() )
    {
        U32 presStPt = getTargetROPumpPressure();
        F32 measFlow = getMeasuredROFlowRate();
        F32 pumpPWMPctDutyCycle = roPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR;
        broadcastROPumpData( presStPt, measFlow, pumpPWMPctDutyCycle );
        roPumpDataPublicationTimerCounter = 0;
    }
}

/*********************************************************************//**
 * @brief
 * The execROPumpTest function executes the state machine for the ROPump self test.
 * @details
 * Inputs : none
 * Outputs : none
 * @return the current state of the ROPump self test.
 *************************************************************************/
SELF_TEST_STATUS_T execROPumpTest( void )
{
    SELF_TEST_STATUS_T result = SELF_TEST_STATUS_FAILED;

    // TODO - implement self test(s)

    return result;
}

/**@}*/


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


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

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

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetROPumpDataPublishIntervalOverride function resets the override \n
 * of the RO pump data publish interval.
 * @details
 * Inputs : none
 * Outputs : roPumpDataPublishInterval
 * @return TRUE if override reset successful, FALSE if not
 *************************************************************************/
BOOL testResetROPumpDataPublishIntervalOverride( void )
{
    BOOL result = FALSE;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetTargetROPumpPressureOverride function overrides the target \n
 * RO pressure. \n
 * @details
 * Inputs : none
 * Outputs : targetROPumpPressure
 * @param value : override target RO pressure (in PSI)
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetTargetROPumpPressureOverride( S32 value )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        targetROPumpPressure.ovInitData = targetROPumpPressure.data; // backup current target pressure
        targetROPumpPressure.ovData = value;
        targetROPumpPressure.override = OVERRIDE_KEY;
        result = setROPumpTargetPressure( value, roPumpControlMode );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetTargetROPumpPressureOverride function resets the override of the \n
 * target RO pressure.
 * @details
 * Inputs : none
 * Outputs : targetROPumpPressure
 * @param none
 * @return TRUE if override reset successful, FALSE if not
 *************************************************************************/
BOOL testResetTargetROPumpPressureOverride( void )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        targetROPumpPressure.data = targetROPumpPressure.ovInitData;  // restore pre-override target pressure
        targetROPumpPressure.override = OVERRIDE_RESET;
        targetROPumpPressure.ovInitData = 0;
        targetROPumpPressure.ovData = 0;
        result = setROPumpTargetPressure( targetROPumpPressure.data, roPumpControlMode );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetMeasuredROFlowRateOverride function overrides the measured \n
 * RO flow rate.
 * @details
 * Inputs : none
 * Outputs : measuredROFlowRateLPM
 * @param value : override measured RO pump motor speed (in RPM)
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetMeasuredROFlowRateOverride( F32 value )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        result = TRUE;
        measuredROFlowRateLPM.ovData = value;
        measuredROFlowRateLPM.override = OVERRIDE_KEY;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetMeasuredROFlowRateOverride function resets the override of the \n
 * measured RO flow rate.
 * @details
 * Inputs : none
 * Outputs : measuredROFlowRateLPM
 * @param none
 * @return TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testResetMeasuredROFlowRateOverride( void )
{
    BOOL result = FALSE;

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

    return result;
}