/**************************************************************************
*
* 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) 22-Jul-2020
*
* @author (original) Sean
* @date (original) 08-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 "Valves.h"
#include "DrainPump.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

/// 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.
    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 tests in progress state.
    DRAIN_PUMP_TEST_STATE_COMPLETE,                 ///< Drain pump self tests completed state.
    NUM_OF_DRAIN_PUMP_SELF_TEST_STATES              ///< Number of drain pump self test states.
} DRAIN_PUMP_SELF_TEST_STATE_T;

// pin assignment for pump enable
#define DRAIN_PUMP_ENABLE_SPI3_PORT_MASK    0x00000020  // (CS5 - re-purposed as output GPIO)
// drain pump enable macros
#define SET_DRAIN_PUMP_ENABLE()             {mibspiREG3->PC3 |= DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;}
#define CLR_DRAIN_PUMP_ENABLE()             {mibspiREG3->PC3 &= ~DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;}

// 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 BOOL isDrainPumpOn = FALSE;                                                  ///< Drain pump is currently running
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 = PUMP_CONTROL_MODE_CLOSED_LOOP;    ///< 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 U32 drainControlTimerCounter = 0;                                            ///< determines when to perform control on drain pump

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 void stopDrainPump( void );
static void publishDrainPumpData( void );
static DATA_GET_PROTOTYPE( U32, getPublishDrainPumpDataInterval );

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

/*********************************************************************//**
 * @brief
 * The setDrainPumpTargetSpeed function sets a new target speed for the \n
 * 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 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;
    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;

        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 \n
 * of the drain pump controller state machine.
 * @details
 * Inputs : targetDrainPumpSpeed
 * Outputs : drainPumpPWMDutyCyclePctSet, isDrainPumpOn
 * @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 we've been given a pressure, transition to control to target state
    if ( getTargetDrainPumpSpeed() > 0 )
    {
        // set drain pump enable pin
        SET_DRAIN_PUMP_ENABLE();
        // set drain pump control mode
        drainPumpControlModeSet = drainPumpControlMode;
        // set drain pump DAC
        drainPumpDACSet = drainPumpDAC;
        setFPGADrainPumpSpeed( drainPumpDACSet );
        // set pump to on
        isDrainPumpOn = TRUE;
        result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE;
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The handleDrainPumpControlToTargetState function handles the "control to \n
 * 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 )
    {
        if ( drainPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP )
        {
#ifndef EMC_TEST_BUILD
            // TODO - will drain pump have a closed loop?
#endif
        }
        drainControlTimerCounter = 0;
    }

    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 stopDrainPump function sets the Drain pump DAC to zero.
 * @details
 * Inputs : none
 * Outputs : isDrainPumpOn, DAC zeroed
 * @return none
 *************************************************************************/
static void stopDrainPump( void )
{
    isDrainPumpOn = FALSE;
    drainPumpDAC = 0;
    drainPumpDACSet = 0;
    setFPGADrainPumpSpeed( drainPumpDACSet );
    CLR_DRAIN_PUMP_ENABLE();
}

/*********************************************************************//**
 * @brief
 * The getPublishDrainPumpDataInterval function gets the Drain pump data \n
 * publication interval.
 * @details
 * Inputs : drainPumpDataPublishInterval
 * Outputs : none
 * @return the current Drain pump data publication interval (in ms).
 *************************************************************************/
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 \n
 * 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 \n
 * 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() )
    {
        U32 spdStPt = getTargetDrainPumpSpeed();

        broadcastDrainPumpData( spdStPt, drainPumpDACSet );
        drainPumpDataPublicationTimerCounter = 0;
    }
}

/*********************************************************************//**
 * @brief
 * The execDrainPumpTest function executes the state machine for the Drain \n
 * 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 \n
 * 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 \n
 * 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 \n
 * 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 \n
 * 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;
}

/**@}*/