/**************************************************************************
*
* 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
*
* @author (last) Quang Nguyen
* @date (last) 21-Jul-2020
*
* @author (original) Sean
* @date (original) 04-Apr-2020
*
***************************************************************************/

#include <math.h>

#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"
#ifdef EMC_TEST_BUILD
#include "Heaters.h"
#endif
#ifdef ENABLE_DIP_SWITCHES
#include "Valves.h"
#endif

/**
 *  @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.0020              ///< P term for RO pump control
#define ROP_I_COEFFICIENT                   0.0015              ///< I term for RO pump control

#define FLOW_SENSOR_ZERO_READING            0xFFFF              ///< Flow sensor reading indicates zero flow (or flow lower than can be detected by sensor)

#define FLOW_SAMPLES_TO_AVERAGE             (250 / TASK_PRIORITY_INTERVAL)              ///< Averaging flow data over 250 ms intervals.
#define FLOW_AVERAGE_MULTIPLIER             (1.0 / (F32)FLOW_SAMPLES_TO_AVERAGE)        ///< Optimization - multiplying is faster than dividing.

#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           10909.0909          ///< conversion factor from ADC counts to LPM (liters/min) for RO flow rate (multiply this by inverse of FPGA reading).

/// Enumeration of RO pump states.
typedef enum ROPump_States
{
    RO_PUMP_OFF_STATE = 0,                                  ///< RO pump off state.
    RO_PUMP_CONTROL_TO_TARGET_STATE,                        ///< RO pump control to target pressure state.
    NUM_OF_RO_PUMP_STATES                                   ///< Number of RO pump states.
} RO_PUMP_STATE_T;

/// Enumeration of RO pump self test states.
typedef enum ROPump_Self_Test_States
{
    RO_PUMP_SELF_TEST_STATE_START = 0,                      ///< RO pump self test start state.
    RO_PUMP_TEST_STATE_IN_PROGRESS,                         ///< RO pump self tests in progress state.
    RO_PUMP_TEST_STATE_COMPLETE,                            ///< RO pump self tests completed state.
    NUM_OF_RO_PUMP_SELF_TEST_STATES                         ///< Number 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 = { RO_PUMP_DATA_PUB_INTERVAL, RO_PUMP_DATA_PUB_INTERVAL, 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 measuredROFlowRateLPM = { 0.0, 0.0, 0.0, 0 };              ///< measured RO flow rate (in LPM).
static F32 measuredROPumpPressure = 0.0;                                         ///< measured RO pressure (in PSI).

static S32 measuredFlowReadingsSum = 0;                                          ///< Raw flow reading sums for averaging.
static U32 flowFilterCounter = 0;                                                ///< used to schedule flow filtering.

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.
 * @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
#ifdef EMC_TEST_BUILD
        roPumpPWMDutyCyclePct = 1.0;
#else
        roPumpPWMDutyCyclePct = ROP_PSI_TO_PWM_DC( roPressure );
#endif

        result = TRUE;
    }
    else  // requested pressure out of range
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_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
 * @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
 * @return none
 *************************************************************************/
void execROPumpMonitor( void )
{
    U16 roFlowReading = getFPGAROPumpFlowRate();
    S32 roFlow = (S32)roFlowReading;

    // update sum for flow average calculation
    measuredFlowReadingsSum += roFlow;
    // filter every 250ms
    if ( ++flowFilterCounter >= FLOW_SAMPLES_TO_AVERAGE )
    {
        F32 avgROFlow = (F32)measuredFlowReadingsSum * FLOW_AVERAGE_MULTIPLIER;

        if ( ( avgROFlow == FLOW_SENSOR_ZERO_READING ) || ( measuredFlowReadingsSum == 0 ) )
        {
            measuredROFlowRateLPM.data = 0.0;
        }
        else
        {
            measuredROFlowRateLPM.data = RO_FLOW_ADC_TO_LPM_FACTOR / avgROFlow;
        }
        measuredFlowReadingsSum = 0;
        flowFilterCounter = 0;
    }

    measuredROPumpPressure = getMeasuredDGPressure( PRESSURE_SENSOR_RO_PUMP_OUTLET );

    // TODO - check pressure?

    // TODO - check flow?

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

/*********************************************************************//**
 * @brief
 * The execROPumpController function executes the RO Pump controller.
 * @details
 * Inputs : roPumpState
 * Outputs : roPumpState
 * @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_DG_SOFTWARE_FAULT, 0, roPumpState )  // TODO - replace 1st param with s/w fault enum
            roPumpState = RO_PUMP_OFF_STATE;
            break;
    }
}

/*********************************************************************//**
 * @brief
 * The isROPumpOn function returns the status of RO pump
 * @details
 * Inputs : none
 * Outputs : none
 * @return isROPumpOn
 *************************************************************************/
BOOL isReverseOsmosisPumpOn( void )
{
    return isROPumpOn;
}

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

#ifdef DEBUG_ENABLED
#ifdef ENABLE_DIP_SWITCHES
    // TODO - test code - remove later
    if ( GET_DIP_SW0_TEST() )
    {
        setValveState( VSP, VALVE_STATE_CLOSED );
        setValveState( VPI, VALVE_STATE_OPEN );
        setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO );
        setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO );
        setValveState( VDR, VALVE_STATE_DRAIN_C_TO_NO );
        setROPumpTargetPressure( 120, PUMP_CONTROL_MODE_CLOSED_LOOP );
#ifdef EMC_TEST_BUILD
        {
            F32 fanPWM = 0.25;
            etpwmSetCmpA( etpwmREG6, (U32)( (S32)( ( fanPWM * (F32)(etpwmREG6->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
            etpwmSetCmpB( etpwmREG6, (U32)( (S32)( ( fanPWM * (F32)(etpwmREG6->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
            setPrimaryHeaterTargetTemperature( 50.0 );
            startPrimaryHeater();
        }
#endif
    }
#endif
#endif

    // 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, roPumpPWMDutyCyclePctSet );
        // 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
 * @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 = measuredROPumpPressure;
            F32 newPWM;

#ifndef EMC_TEST_BUILD
            newPWM = runPIController( PI_CONTROLLER_ID_RO_PUMP, tgtPres, actPres );
            roPumpPWMDutyCyclePctSet = newPWM;
            setROPumpControlSignalPWM( newPWM );
#endif
        }
        roControlTimerCounter = 0;
    }

#ifdef DEBUG_ENABLED
#ifdef ENABLE_DIP_SWITCHES
    // TODO - test code - remove later
    if ( !GET_DIP_SW0_TEST() )
    {
        signalROPumpHardStop();
        result = RO_PUMP_OFF_STATE;
#ifdef EMC_TEST_BUILD
        etpwmSetCmpA( etpwmREG6, 0 );
        etpwmSetCmpB( etpwmREG6, 0 );
        stopPrimaryHeater();
#endif
    }
#endif
#endif

    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
 * @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
 * @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
 * @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 getMeasuredROFlowRate function gets the measured RO pump \n
 * flow rate.
 * @details
 * Inputs : measuredROFlowRateLPM
 * Outputs : none
 * @return the current RO pump flow rate (in LPM).
 *************************************************************************/
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( U32 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
 * @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 LPM)
 * @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
 * @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;
}

/**@}*/