/**************************************************************************
*
* 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 DrainPump.c
*
* @author (last) Quang Nguyen
* @date (last) 14-Sep-2020
*
* @author (original) Sean
* @date (original) 08-Apr-2020
*
***************************************************************************/

#include <math.h>

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

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

/**
 *  @addtogroup DrainPump
 *  @{
 */

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

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

#define DRP_CONTROL_INTERVAL                   ( MS_PER_SECOND / TASK_GENERAL_INTERVAL )    ///< interval (ms/task time) at which the Drain pump is controlled

#define DRP_SPEED_ADC_TO_RPM_FACTOR            12.94                                        ///< conversion factor from ADC counts to RPM for Drain pump
#define DRP_SPEED_RPM_TO_ADC_FACTOR            ( 1.0 / DRP_SPEED_ADC_TO_RPM_FACTOR )        ///< conversion factor from RPM to ADC counts for Drain pump

#define DRAIN_PUMP_P_COEFFICIENT               0.005                                        ///< P term for drain pump delta pressure control
#define DRAIN_PUMP_I_COEFFICIENT               0.0015                                       ///< I term for drain pump delta pressure control

/// Enumeration of drain pump states.
typedef enum DrainPump_States
{
    DRAIN_PUMP_OFF_STATE = 0,                       ///< Drain pump off state
    DRAIN_PUMP_CONTROL_TO_TARGET_STATE,             ///< Drain pump control to target state
    DRAIN_PUMP_OPEN_LOOP_STATE,                     ///< Drain pump open loop state
    NUM_OF_DRAIN_PUMP_STATES                        ///< Number of drain pump states
} DRAIN_PUMP_STATE_T;

/// Enumeration of drain pump self-test states.
typedef enum DrainPump_Self_Test_States
{
    DRAIN_PUMP_SELF_TEST_STATE_START = 0,           ///< Drain pump self-test start state
    DRAIN_PUMP_TEST_STATE_IN_PROGRESS,              ///< Drain pump self-test in progress state
    DRAIN_PUMP_TEST_STATE_COMPLETE,                 ///< Drain pump self-test completed state
    NUM_OF_DRAIN_PUMP_SELF_TEST_STATES              ///< Number of drain pump self-test states
} DRAIN_PUMP_SELF_TEST_STATE_T;

#define DRAIN_PUMP_ENABLE_SPI3_PORT_MASK    0x00000020                                                  ///< CS5 - Out put GPIO for pump enable.
#define SET_DRAIN_PUMP_ENABLE()             {mibspiREG3->PC3 |= DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;}      ///< drain pump enable set macro.
#define CLR_DRAIN_PUMP_ENABLE()             {mibspiREG3->PC3 &= ~DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;}     ///< drain pump enable clear macro.

// TODO - test code - remove later
#define DRAIN_PUMP_TEST2_SPI1_PORT_MASK     0x00000004  // (CS2 - re-purposed as input GPIO)
#define GET_DIP_SW2_TEST()                  ( ( mibspiREG1->PC2 & DRAIN_PUMP_TEST2_SPI1_PORT_MASK ) != 0 )

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

static DRAIN_PUMP_STATE_T drainPumpState = DRAIN_PUMP_OFF_STATE;                    ///< current state of drain pump controller state machine
static U32 drainPumpDataPublicationTimerCounter = 0;                                ///< used to schedule drain pump data publication to CAN bus
static U32 drainPumpDAC = 0;                                                        ///< initial drain pump DAC value
static U32 drainPumpDACSet = 0;                                                     ///< currently set drain pump DAC value
static PUMP_CONTROL_MODE_T drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES;        ///< requested drain pump control mode
static PUMP_CONTROL_MODE_T drainPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< currently set drain pump control mode

static OVERRIDE_U32_T drainPumpDataPublishInterval = { DRAIN_PUMP_DATA_PUB_INTERVAL,
                                                       DRAIN_PUMP_DATA_PUB_INTERVAL,
                                                       0, 0 };                      ///< interval (in ms) at which to publish RO flow data to CAN bus
static OVERRIDE_U32_T targetDrainPumpSpeed  = { 0, 0, 0, 0 };                       ///< Target RO pressure (in PSI)
static OVERRIDE_F32_T targetDrainPumpDeltaPressure = { 0.0, 0.0, 0.0, 0.0 };        ///< Target delta pressure for the drain pump

static U32 drainControlTimerCounter = 0;                                            ///< determines when to perform control on drain pump

/* These variables are used for POST. POST will be implemented later
static DRAIN_PUMP_SELF_TEST_STATE_T drainPumpSelfTestState = DRAIN_PUMP_SELF_TEST_STATE_START;  ///< current drain pump self test state
static U32 drainPumpSelfTestTimerCount = 0;                                                     ///< timer counter for drain pump self test
*/

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

static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void );
static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void );
static DRAIN_PUMP_STATE_T handleDrainPumpOpenLoopState( void );
static void stopDrainPump( void );
static void publishDrainPumpData( void );
static U32 getPublishDrainPumpDataInterval( void );

/*********************************************************************//**
 * @brief
 * The initDrainPump function initializes the DrainPump module.
 * @details
 * Inputs : none
 * Outputs : DrainPump module initialized.
 * @return none
 *************************************************************************/
void initDrainPump( void )
{
    stopDrainPump();

    // Initialize the drain pump PI controller
    initializePIController( PI_CONTROLLER_ID_DRAIN_PUMP, MIN_DRAIN_PUMP_RPM_TARGET,
                            DRAIN_PUMP_P_COEFFICIENT, DRAIN_PUMP_I_COEFFICIENT,
                            MIN_DRAIN_PUMP_RPM_TARGET, MAX_DRAIN_PUMP_RPM_TARGET );
}

/*********************************************************************//**
 * @brief
 * The setDrainPumpTargetSpeed function sets a new target speed for the drain pump.
 * @details
 * Inputs: none
 * Outputs: targetDrainPumpPressure
 * @param: rpm : new target drain pump speed (in RPM).
 * @return: TRUE if new target speed is set, FALSE if not
 *************************************************************************/
BOOL setDrainPumpTargetSpeed( U32 rpm )
{
    BOOL result = FALSE;

    if ( ( 0 == rpm ) ||
         ( ( rpm >= MIN_DRAIN_PUMP_RPM_TARGET ) && ( rpm <= MAX_DRAIN_PUMP_RPM_TARGET ) ) )
    {
#ifdef EMC_TEST_BUILD
        drainPumpDAC = (U32)((F32)2500 * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET);
#else
        drainPumpDAC = (U32)((F32)rpm * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET);
#endif
        targetDrainPumpSpeed.data = rpm;
        drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP;
        result = TRUE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The setDrainPumpTargetDeltaPressure function sets the target delta\n
 * pressure in between the PRd and PDr sensors
 * @details
 * Inputs: targetDrainPumpDeltaP
 * Outputs: targetDrainPumpDeltaP
 * @param: deltaP : new target drain pump delta pressure
 * @return: TRUE if new target speed is set, FALSE if not
 *************************************************************************/
BOOL setDrainPumpTargetDeltaPressure ( F32 deltaP )
{
    BOOL result = FALSE;

    targetDrainPumpDeltaPressure.data = deltaP;
    drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP;
    result = TRUE;

    return result;
}

/*********************************************************************//**
 * @brief
 * The signalDrainPumpHardStop function stops the Drain pump immediately.
 * @details
 * Inputs: none
 * Outputs: Drain pump stopped, set point reset, state changed to off
 * @return: none
 *************************************************************************/
void signalDrainPumpHardStop( void )
{
    targetDrainPumpSpeed.data = 0;
    stopDrainPump();
    drainPumpState = DRAIN_PUMP_OFF_STATE;
    drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; // Set the control mode to none
    drainControlTimerCounter = 0;
}

/*********************************************************************//**
 * @brief
 * The execDrainPumpMonitor function executes the drain pump monitor.
 * @details
 * Inputs: none
 * Outputs: none
 * @return: none
 *************************************************************************/
void execDrainPumpMonitor( void )
{
    U16 drnPumpSpd = getFPGADrainPumpSpeed();

    // TODO - convert drain pump speed to RPM

    // TODO - check(s) ???

    // publish drain pump data on interval
    publishDrainPumpData();
}

/*********************************************************************//**
 * @brief
 * The execDrainPumpController function executes the drain pump controller.
 * @details
 * Inputs: drainPumpState
 * Outputs: drainPumpState
 * @return: none
 *************************************************************************/
void execDrainPumpController( void )
{
    switch ( drainPumpState )
    {
        case DRAIN_PUMP_OFF_STATE:
            drainPumpState = handleDrainPumpOffState();
            break;

        case DRAIN_PUMP_CONTROL_TO_TARGET_STATE:
            drainPumpState = handleDrainPumpControlToTargetState();
            break;

        case DRAIN_PUMP_OPEN_LOOP_STATE:
            drainPumpState = handleDrainPumpOpenLoopState();
            break;

        default:
            SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, 0, drainPumpState )  // TODO - replace 1st param with s/w fault enum
            drainPumpState = DRAIN_PUMP_OFF_STATE;
            break;
    }
}

/*********************************************************************//**
 * @brief
 * The handleDrainPumpOffState function handles the drain pump off state of
 * the drain pump controller state machine.
 * @details
 * Inputs: targetDrainPumpSpeed
 * Outputs: drainPumpPWMDutyCyclePctSet
 * @return: next state
 *************************************************************************/
static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void )
{
    DRAIN_PUMP_STATE_T result = DRAIN_PUMP_OFF_STATE;

#ifdef DEBUG_ENABLED
#ifdef ENABLE_DIP_SWITCHES
    // TODO - test code - remove later
    if ( GET_DIP_SW2_TEST() )
    {
        setValveState( VPI, VALVE_STATE_OPEN );
        setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO );
        setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO );
        setDrainPumpTargetSpeed( 1000 );
#ifdef EMC_TEST_BUILD
        setTrimmerHeaterTargetTemperature( 50.0 );
        startTrimmerHeater();
#endif
    }
#endif
#endif

    // If the target drain pump speed was not 0 and the control mode
    // is open loop, set the drain pump to open loop
    if ( getTargetDrainPumpSpeed() > 0 && drainPumpControlMode == PUMP_CONTROL_MODE_OPEN_LOOP )
    {
        // set drain pump enable pin
        SET_DRAIN_PUMP_ENABLE();

        // set drain pump DAC
        drainPumpDACSet = drainPumpDAC;
        setFPGADrainPumpSpeed( drainPumpDACSet );

        result = DRAIN_PUMP_OPEN_LOOP_STATE;
    }
    // If the drain pump is set to closed loop, call the proper state
    // It is checked for the value of delta pressure because it can be anything
    // including 0
    else if ( drainPumpControlMode == PUMP_CONTROL_MODE_CLOSED_LOOP )
    {
       // set drain pump enable pin
       SET_DRAIN_PUMP_ENABLE();

       resetPIController( PI_CONTROLLER_ID_DRAIN_PUMP, MIN_DRAIN_PUMP_RPM_TARGET );

       U32 rpm = targetDrainPumpSpeed.data;
       drainPumpDAC = (U32)((F32)rpm * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET);
       // set drain pump DAC
       drainPumpDACSet = drainPumpDAC;
       setFPGADrainPumpSpeed( drainPumpDACSet );

       result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleDrainPumpControlToTargetState function handles the "control to
 * target" state of the drain pump controller state machine.
 * @details
 * Inputs: none
 * Outputs: drainPumpState
 * @return: next state
 *************************************************************************/
static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void )
{
    DRAIN_PUMP_STATE_T result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE;

    // control at set interval
    if ( ++drainControlTimerCounter >= DRP_CONTROL_INTERVAL )
    {
        F32 inletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_INLET );
        F32 outletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET );
        F32 pressureDiff = inletDrainPressure - outletDrainPressure;
        F32 rpm = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, targetDrainPumpDeltaPressure.data, pressureDiff );
        drainPumpDACSet = (U32)(rpm * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET);
        setFPGADrainPumpSpeed( drainPumpDACSet );

        // From a merge from master
        if ( drainPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP )
        {
#ifndef EMC_TEST_BUILD
            // TODO - will drain pump have a closed loop?
#endif
        }

        drainControlTimerCounter = 0;
    }
    // From a merge from master

    if ( 0 == getTargetDrainPumpSpeed() )
    {
        signalDrainPumpHardStop();

        result = DRAIN_PUMP_OFF_STATE;
    }

#ifdef DEBUG_ENABLED
#ifdef ENABLE_DIP_SWITCHES
    // TODO - test code - remove later
    if ( !GET_DIP_SW2_TEST() )
    {
        signalDrainPumpHardStop();
        setValveState( VPI, VALVE_STATE_OPEN );
        setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO );
        setValveState( VPO, VALVE_STATE_FILL_C_TO_NC );
#ifdef EMC_TEST_BUILD
        stopTrimmerHeater();
#endif
        result = DRAIN_PUMP_OFF_STATE;
    }
#endif
#endif

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleDrainPumpOpenLoopState function handles the open loop \n
 * state
 * @details
 * Inputs: none
 * Outputs: none
 * @return: next state
 *************************************************************************/
static DRAIN_PUMP_STATE_T handleDrainPumpOpenLoopState( void )
{
    return DRAIN_PUMP_OPEN_LOOP_STATE;
}

/*********************************************************************//**
 * @brief
 * The stopDrainPump function sets the drain pump DAC to zero.
 * @details
 * Inputs: none
 * Outputs: DAC zeroed
 * @return: none
 *************************************************************************/
static void stopDrainPump( void )
{
    drainPumpDAC = 0;
    drainPumpDACSet = 0;
    setFPGADrainPumpSpeed( drainPumpDACSet );
    CLR_DRAIN_PUMP_ENABLE();
}

/*********************************************************************//**
 * @brief
 * The getPublishDrainPumpDataInterval function gets the drain pump data
 * publication interval.
 * @details
 * Inputs: drainPumpDataPublishInterval
 * Outputs: none
 * @return: the current Drain pump data publication interval (in ms).
 *************************************************************************/
static U32 getPublishDrainPumpDataInterval( void )
{
    U32 result = drainPumpDataPublishInterval.data;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The getTargetDrainPumpSpeed function gets the current target drain pump
 * speed.
 * @details
 * Inputs: targetDrainPumpSpeed
 * Outputs : none
 * @return: the current target drain pump speed.
 *************************************************************************/
U32 getTargetDrainPumpSpeed( void )
{
    U32 result = targetDrainPumpSpeed.data;

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The publishDrainPumpData function publishes drain pump data at the set interval.
 * @details
 * Inputs: target speed
 * Outputs: Drain pump data is published to CAN bus.
 * @return: none
 *************************************************************************/
static void publishDrainPumpData( void )
{
    // publish Drain pump data on interval
    if ( ++drainPumpDataPublicationTimerCounter >= getPublishDrainPumpDataInterval() )
    {
        // TODO: This was a const in master, why?
        U32 spdStPt = getTargetDrainPumpSpeed();

        F32 pressureDiff = fabs( getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_INLET ) -
                                 getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ) );

        broadcastDrainPumpData( spdStPt, drainPumpDACSet, pressureDiff, (U32)drainPumpState );

        drainPumpDataPublicationTimerCounter = 0;
    }
}

/*********************************************************************//**
 * @brief
 * The execDrainPumpTest function executes the state machine for the drain
 * pump self-test.
 * @details
 * Inputs: none
 * Outputs: none
 * @return: the current state of the Drain Pump self test.
 *************************************************************************/
SELF_TEST_STATUS_T execDrainPumpTest( void )
{
    SELF_TEST_STATUS_T result = SELF_TEST_STATUS_FAILED;

    // TODO - implement self-test(s)

    return result;
}

/**@}*/

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


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

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

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

    return result;
}

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

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

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetTargetDrainPumpSpeedOverride function overrides the target
 * drain pump speed (in RPM).
 * @details
 * Inputs: none
 * Outputs: targetDrainPumpSpeed
 * @param: value : override target drain pump speed (in RPM)
 * @return: TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetTargetDrainPumpSpeedOverride( U32 value )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        targetDrainPumpSpeed.ovInitData = targetDrainPumpSpeed.data; // backup current target pressure
        targetDrainPumpSpeed.ovData = value;
        targetDrainPumpSpeed.override = OVERRIDE_KEY;
        result = setDrainPumpTargetSpeed( value );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetTargetDrainPumpSpeedOverride function resets the override of the
 * target drain pump speed (in RPM).
 * @details
 * Inputs: none
 * Outputs: targetDrainPumpSpeed
 * @return: TRUE if override reset successful, FALSE if not
 *************************************************************************/
BOOL testResetTargetDrainPumpSpeedOverride( void )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        targetDrainPumpSpeed.data = targetDrainPumpSpeed.ovInitData;  // restore pre-override target speed
        targetDrainPumpSpeed.override = OVERRIDE_RESET;
        targetDrainPumpSpeed.ovInitData = 0;
        targetDrainPumpSpeed.ovData = 0;
        result = setDrainPumpTargetSpeed( targetDrainPumpSpeed.data );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testSetTargetDrainPumpDeltaPressureOverride function overrides \n
 * the target drain pump delta pressure
 * @details
 * Inputs: none
 * Outputs: targetDrainPumpDeltaPressure
 * @param: value : override target drain pump delta pressure
 * @return: TRUE if override successful, FALSE if not
 *************************************************************************/
BOOL testSetTargetDrainPumpDeltaPressureOverride( F32 value )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        targetDrainPumpDeltaPressure.ovInitData = targetDrainPumpDeltaPressure.data; // backup current target delta pressure
        targetDrainPumpDeltaPressure.ovData = value;
        targetDrainPumpDeltaPressure.override = OVERRIDE_KEY;
        result = setDrainPumpTargetDeltaPressure( value );
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The testResetTargetDrainPumpDeltaPressureOverride function resets the \n
 * override of the target drain pump delta pressure
 * @details
 * Inputs: none
 * Outputs: targetDrainPumpDeltaPressure
 * @return: TRUE if override reset successful, FALSE if not
 *************************************************************************/
BOOL testResetTargetDrainPumpDeltaPressureOverride( void )
{
    BOOL result = FALSE;

    if ( TRUE == isTestingActivated() )
    {
        targetDrainPumpDeltaPressure.data = targetDrainPumpDeltaPressure.ovInitData;  // restore pre-override target delta pressure
        targetDrainPumpDeltaPressure.override = OVERRIDE_RESET;
        targetDrainPumpDeltaPressure.ovInitData = 0;
        targetDrainPumpDeltaPressure.ovData = 0;
        result = setDrainPumpTargetDeltaPressure( targetDrainPumpDeltaPressure.data );
    }

    return result;
}

/**@}*/