/**********************************************************************//**
 *
 *  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 Heaters.c
 *
 *  @date 21-Apr-2020
 *  @author Dara Navaei
 *
 *  @brief DG heaters controller
 *
 **************************************************************************/

#include "etpwm.h"

// TODO for testing only remove
#include "mibspi.h"
// TODO for testing only remove

#include "AlarmMgmt.h"
#include "Common.h"
#include "Heaters.h"
#include "TemperatureSensors.h"
#include "SystemCommMessages.h"
#include "PIControllers.h"

#define MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE 0.89 ///< Main primary heater (heater A) max duty cycle (79.5%)
#define SMALL_PRIMAY_HEATER_MAX_DUTY_CYCLE 0.50    ///< Small Primary heater (heater B) max duty cycle (25%)
#define TRIMMER_HEATER_MAX_DUTY_CYCLE 0.25         ///< Trimmer heater max duty cycle (25%)
#define HEATERS_MIN_DUTY_CYCLE 0.00              ///< Primary and trimmer heaters min duty cycle (0.00%)

#define PRIMARY_HEATERS_P_COEFFICIENT 0.02
#define PRIMARY_HEATERS_I_COEFFICIENT 0.001

#define TRIMMER_HEATER_P_COEFFICIENT 0.003
#define TRIMMER_HEATER_I_COEFFICIENT 0.001

#define DELTA_TEMP_TO_PWM_DUTY_CYCLE_CONVERSION  0.05
#define CONTROLLER_CHECK_INTERVAL_COUNT 10U

#define TEMP_SENSORS_INTERVAL_COUNT 10U                                     ///< Temperature sensors interval count


typedef enum heaters_self_test_states
{
    HEATERS_SELF_TEST_START = 0,
    HEATERS_SELF_TEST_START_SMALL_PRIMARY_AND_TRIMMER,
    HEATERS_SELF_TEST_START_MAIN_PRIMARY,
    HEATERS_SELF_TEST_COMPLETE,
    NUM_OF_HEATERS_SELF_TEST_STATES
} HEATERS_SELF_TEST_STATES_T ;

typedef enum primary_heaters_exec_states
{
    PRIMARY_HEATERS_EXEC_STATE_OFF = 0,
    PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET,
    NUM_OF_PRIMARY_HEATERS_EXEC_STATES
} PRIMARY_HEATERS_EXEC_STATES_T;

typedef enum trimmer_heater_exec_states
{
    TRIMMER_HEATER_EXEC_STATE_OFF = 0,
    TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET,
    NUM_OF_TRIMMER_HEATER_EXEC_STATES
} TRIMMER_HEATER_EXEC_STATES_T;

typedef enum name_of_heaters
{
    PRIMARY_HEATER = 0,
    TRIMMER_HEATER,
    NUM_OF_HEATERS
} NAME_OF_HEATER_T;

// Private variables

static SELF_TEST_STATUS_T         heatersSelfTestResult;
static HEATERS_SELF_TEST_STATES_T heatersSelfTestState;
static PRIMARY_HEATERS_EXEC_STATES_T      primaryHeatersExecState;
static TRIMMER_HEATER_EXEC_STATES_T trimmerHeaterExecState;

static F32 primaryHeaterTargetTemperature;
static F32 trimmerHeaterTargetTemperature;

static F32 mainPrimaryHeaterDutyCycle;
static F32 smallPrimaryHeaterDutyCycle;
static U32 controllerCheckTimerCounter;
static U32 tempSensorsPublicationTimerCounter;

// private functions prototypes

static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateOff ( void );
static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateControlToTarget ( void );

static void setMainPrimaryHeaterPWM ( F32 pwm );
static void setSmallPrimaryHeaterPWM ( F32 pwm );
static void setTrimmerHeaterPWM ( F32 pwm );
static void resetHeaterState ( NAME_OF_HEATER_T heater );
static void publishTemperatureData ( void );

// TODO: Remove the below code. FOR TESTING ONLY
#define PRIMARY_HEATER_MIBSPI1_PORT_MASK     0x00000002  // (CS1 - re-purposed as input GPIO)
#define TOGGLEPRIMAYHEATER()                 (( mibspiREG1->PC2 & PRIMARY_HEATER_MIBSPI1_PORT_MASK ) != 0)
static void temporaryStartROPump ( void );
static void temporaryStopROPump ( void );
static void temporaryStartFan ( void );
static void temporaryStopFan ( void );
//TODO: Remove the above code. FOR TESTING ONLY

// Public functions

/*********************************************************************//**
 * @brief initHeaters
 * The initHeaters function initializes the variables.
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
void initHeaters ( void )
{
    heatersSelfTestResult   = SELF_TEST_STATUS_IN_PROGRESS;
    heatersSelfTestState    = HEATERS_SELF_TEST_START;
    primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_OFF;
    trimmerHeaterExecState  = TRIMMER_HEATER_EXEC_STATE_OFF;

    primaryHeaterTargetTemperature = 0.0;
    trimmerHeaterTargetTemperature = 0.0;
    controllerCheckTimerCounter = 0;
    tempSensorsPublicationTimerCounter = 0;

    // initialize the PI controller for the primary heaters
    initializePIController( PI_CONTROLLER_ID_PRIMARY_HEATER, HEATERS_MIN_DUTY_CYCLE,
                            PRIMARY_HEATERS_P_COEFFICIENT, PRIMARY_HEATERS_I_COEFFICIENT,
                            HEATERS_MIN_DUTY_CYCLE,
                            MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE + SMALL_PRIMAY_HEATER_MAX_DUTY_CYCLE );
}

/*********************************************************************//**
 * @brief setPrimaryHeaterTargetTemperature
 * The setPrimaryHeaterTargetTemperature function sets the primary heater
 *  target temperature
 * @details
 * Inputs : F32 (targetTemp)
 * Outputs : none
 * @return none
 *************************************************************************/
void setPrimaryHeaterTargetTemperature ( F32 targetTemp )
{
    primaryHeaterTargetTemperature = targetTemp;
}

/*********************************************************************//**
 * @brief setTrimmerHeaterTargetTemperature
 * The setTrimmerHeaterTargetTemperature function sets the trimmer heater
 *  target temperature
 * @details
 * Inputs : F32 (targetTemp)
 * Outputs : none
 * @return none
 *************************************************************************/
void setTrimmerHeaterTargetTemperature ( F32 targetTemp )
{
    trimmerHeaterTargetTemperature = targetTemp;
}

/*********************************************************************//**
 * @brief startPrimaryHeater
 * The startPrimaryHeater function starts the primary heaters
 * @details
 * Inputs : none
 * Outputs : BOOL (status)
 * @return BOOL (status)
 *************************************************************************/
BOOL startPrimaryHeater ( void )
{
    BOOL status = FALSE;

    if ( primaryHeaterTargetTemperature != 0.0 )
    {
        resetHeaterState ( PRIMARY_HEATER );
        setMainPrimaryHeaterPWM ( mainPrimaryHeaterDutyCycle );
        primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
        status = TRUE;
    }

    return status;
}

/*********************************************************************//**
 * @brief startTrimmerHeater
 * The startTrimmerHeater function starts the trimmer heater
 * @details
 * Inputs : none
 * Outputs : BOOL (status)
 * @return BOOL (status)
 *************************************************************************/
void startTrimmerHeater ( void )
{
    // ToDO: set the initial duty cycle for the
    // trimmer heater
    setTrimmerHeaterPWM ( 0.5 );
}

/*********************************************************************//**
 * @brief stopPrimaryHeater
 * The stopPrimaryHeater function stops the primary heaters
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
void stopPrimaryHeater ( void )
{
    setMainPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
    setSmallPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
}

/*********************************************************************//**
 * @brief stopTrimmerHeater
 * The stopTrimmerHeater function stops the trimmer heater
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
void stopTrimmerHeater ( void )
{
    setTrimmerHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
}

/*********************************************************************//**
 * @brief execHeatersMonitor
 * The execHeatersMonitor function
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
void execHeatersMonitor ( void )
{
    // TODO Decide the parts of this function

}

/*********************************************************************//**
 * @brief execHeatersSelfTest
 * The execHeatersSelfTest function executes the heaters self test
 * @details
 * Inputs : none
 * Outputs : SELF_TEST_STATUS_T
 * @return SELF_TEST_STATUS_T
 *************************************************************************/
SELF_TEST_STATUS_T execHeatersSelfTest ( void )
{
    switch ( heatersSelfTestState )
    {
        case HEATERS_SELF_TEST_START:

            break;
        case HEATERS_SELF_TEST_START_SMALL_PRIMARY_AND_TRIMMER:

            break;
        case HEATERS_SELF_TEST_START_MAIN_PRIMARY:

            break;
        case HEATERS_SELF_TEST_COMPLETE:

            break;

        default:

            SET_ALARM_WITH_2_U32_DATA ( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_HEATERS_SELF_TEST_INVALID_STATE,
                                        heatersSelfTestState );
            heatersSelfTestState = HEATERS_SELF_TEST_COMPLETE;
            break;
    }

    return heatersSelfTestResult;
}

/*********************************************************************//**
 * @brief execPrimaryHeaters
 * The execPrimaryHeaters function executes the primary heaters
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
void execPrimaryHeaters ( void )
{
    switch ( primaryHeatersExecState )
    {
        case PRIMARY_HEATERS_EXEC_STATE_OFF:

            primaryHeatersExecState = handlePrimaryHeaterStateOff();
            break;

        case PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET:

            primaryHeatersExecState = handlePrimaryHeaterStateControlToTarget();
            break;

        default:

            SET_ALARM_WITH_2_U32_DATA ( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_HEATERS_PRIMARY_HEATER_EXEC_INVALID_STATE,
                                        primaryHeatersExecState );
            primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
            break;
    }
}

/*********************************************************************//**
 * @brief execTrimmerHeater
 * The execTrimmerHeater function executes the trimmer heater
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
void execTrimmerHeater ( void )
{
    switch ( trimmerHeaterExecState )
    {
        case TRIMMER_HEATER_EXEC_STATE_OFF:

            break;

        case TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET:

            break;

        default:

            SET_ALARM_WITH_2_U32_DATA ( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_HEATERS_TRIMMER_HEATER_EXEC_INVALID_STATE,
                                        trimmerHeaterExecState );
            trimmerHeaterExecState = TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET;
            break;
    }
}

// Private functions

/*********************************************************************//**
 * @brief handlePrimaryHeaterStateOff
 * The handlePrimaryHeaterStateOff function handles the primary heaters at
 * off state
 * @details
 * Inputs : none
 * Outputs : PRIMARY_HEATERS_EXEC_STATES_T (state)
 * @return PRIMARY_HEATERS_EXEC_STATES_T (state)
 *************************************************************************/
static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateOff ( void )
{
    PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;

    // TODO for testing only. remove
    state = PRIMARY_HEATERS_EXEC_STATE_OFF;
    if ( TOGGLEPRIMAYHEATER() )
    {
        setPrimaryHeaterTargetTemperature ( 37 );
        if ( getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR ) > 0 )
        {
            startPrimaryHeater();
            state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
            temporaryStartROPump();
            temporaryStartFan();
        }
        //state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
    }
    // TODO remove this code for testing

    return state;
}

/*********************************************************************//**
 * @brief handlePrimaryHeaterStateControlToTarget
 * The handlePrimaryHeaterStateControlToTarget function handles the primary
 * heaters at control state when the heaters are active
 * @details
 * Inputs : none
 * Outputs : PRIMARY_HEATERS_EXEC_STATES_T (state)
 * @return PRIMARY_HEATERS_EXEC_STATES_T (state)
 *************************************************************************/
static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateControlToTarget ( void )
{
    PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;

    if ( ++controllerCheckTimerCounter >= CONTROLLER_CHECK_INTERVAL_COUNT )
    {
        F32 outletTemp = getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR );
        mainPrimaryHeaterDutyCycle = runPIController ( PI_CONTROLLER_ID_PRIMARY_HEATER, primaryHeaterTargetTemperature, outletTemp );

        if ( mainPrimaryHeaterDutyCycle >= MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE )
        {
            smallPrimaryHeaterDutyCycle = mainPrimaryHeaterDutyCycle - MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE;
            mainPrimaryHeaterDutyCycle = MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE;
            setMainPrimaryHeaterPWM ( mainPrimaryHeaterDutyCycle );
            setSmallPrimaryHeaterPWM ( smallPrimaryHeaterDutyCycle );
        }
        else
        {
            setMainPrimaryHeaterPWM ( mainPrimaryHeaterDutyCycle );
            smallPrimaryHeaterDutyCycle = HEATERS_MIN_DUTY_CYCLE;
            setSmallPrimaryHeaterPWM ( smallPrimaryHeaterDutyCycle );
        }
        controllerCheckTimerCounter = 0;
    }
    publishTemperatureData();

    // TODO remove this code
    if ( !TOGGLEPRIMAYHEATER() )
    {
        stopPrimaryHeater();
        temporaryStopROPump();
        temporaryStopFan();
        state = PRIMARY_HEATERS_EXEC_STATE_OFF;
    }
    // TODO Remove this code

    return state;
}

/*********************************************************************//**
 * @brief setMainPrimaryHeaterPWM
 * The setMainPrimaryHeaterPWM function sets the PWM of the main primary
 * heater
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
static void setMainPrimaryHeaterPWM ( F32 pwm )
{
    etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

/*********************************************************************//**
 * @brief setSmallPrimaryHeaterPWM
 * The setSmallPrimaryHeaterPWM function sets the PWM of the small primary
 * heater
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
static void setSmallPrimaryHeaterPWM ( F32 pwm )
{
    etpwmSetCmpB( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

/*********************************************************************//**
 * @brief setTrimmerHeaterPWM
 * The setTrimmerHeaterPWM function sets the PWM of the trimmer heater
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
static void setTrimmerHeaterPWM ( F32 pwm )
{
    etpwmSetCmpA( etpwmREG3, (U32)( (S32)( ( pwm * (F32)(etpwmREG3->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

/*********************************************************************//**
 * @brief resetHeaterState
 * The resetHeaterState function resets the PI controller of the selected
 * heater
 * @details
 * Inputs : none
 * Outputs : none
 * @return none
 *************************************************************************/
static void resetHeaterState ( NAME_OF_HEATER_T heater )
{
    if ( heater == PRIMARY_HEATER )
    {
        F32 inletTemperatrue = getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR );
        mainPrimaryHeaterDutyCycle = fabs(primaryHeaterTargetTemperature - inletTemperatrue) * DELTA_TEMP_TO_PWM_DUTY_CYCLE_CONVERSION;
        resetPIController ( PI_CONTROLLER_ID_PRIMARY_HEATER, mainPrimaryHeaterDutyCycle );
    }
    else if ( heater == TRIMMER_HEATER )
    {
        // TODO setup the trimmer heater
    }
}

/*************************************************************************
 * @brief publishTemperatureData
 * The publishTemperatureData function publishes the temperature sensors
 * data into the USB debug port at the defined time interval
 * @details
 * Inputs : none
 * Outputs : none
 * @param none
 * @return none
 *************************************************************************/
static void publishTemperatureData ( void )
{
    if ( ++tempSensorsPublicationTimerCounter >= TEMP_SENSORS_INTERVAL_COUNT )
    {
#ifdef DEBUG_ENABLED
    {
        char debugTempStr[ 256 ];
        sprintf( debugTempStr, "MPDuty: %6.2f, SPDuty: %6.2f, TPi: %6.2f, TPo: %6.2f, TD1: %6.2f, "
                                "TD2: %6.2f, TRo: %6.2f, TDi: %6.2f, TPh: %6.2f, TTh: %6.2f, PTarg: %6.2f\r\n",
                                mainPrimaryHeaterDutyCycle, smallPrimaryHeaterDutyCycle,
                                getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR ),
                                getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR ),
                                getTemperatureValue ( TEMPSENSORS_CONDUCTIVITY_SENSOR_1_TEMP_SENSOR ),
                                getTemperatureValue ( TEMPSENSORS_CONDUCTIVITY_SENSOR_2_TEMP_SENSOR ),
                                getTemperatureValue ( TEMPSENSORS_OUTLET_REDUNDANCY_TEMP_SENSOR ),
                                getTemperatureValue ( TEMPSENSORS_INLET_DIALYSATE_TEMP_SENSOR ),
                                getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_INTERNAL_TEMP_SENSOR ),
                                getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_INTERNAL_TEMP_SESNOR ),
                                primaryHeaterTargetTemperature );
        sendDebugData ( (U08*)debugTempStr, strlen(debugTempStr) );
    }
#endif
        tempSensorsPublicationTimerCounter = 0;
    }
}

// TODO REMOVE THE CODE
static void temporaryStartROPump ( void )
{
    F32 tempPWM = 0.6;
    etpwmSetCmpB( etpwmREG2, (U32)( (S32)( ( tempPWM * (F32)(etpwmREG2->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}
static void temporaryStopROPump ( void )
{
    etpwmSetCmpB( etpwmREG2, 0 );
}

static void temporaryStartFan ( void )
{
    F32 tempPWM = 0.25;
    etpwmSetCmpA( etpwmREG6, (U32)( (S32)( ( tempPWM * (F32)(etpwmREG6->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
    etpwmSetCmpB( etpwmREG6, (U32)( (S32)( ( tempPWM * (F32)(etpwmREG6->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}
static void temporaryStopFan ( void )
{
    etpwmSetCmpA( etpwmREG6, 0 );
    etpwmSetCmpB( etpwmREG6, 0 );
}
// TODO REMOVE THE CODE