/**********************************************************************//**
 *
 *  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 "PIControllers.h"

#define MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE 0.795 ///< Main primary heater (heater A) max duty cycle (79.5%)
#define SMALL_PRIMAY_HEATER_MAX_DUTY_CYCLE 0.25    ///< 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.005
#define PRIMARY_HEATERS_I_COEFFICIENT 0.001

#define TRIMMER_HEATER_P_COEFFICIENT 0.003
#define TRIMMER_HEATER_I_COEFFICIENT 0.001

// TODO make these macros functions
#define INITIAL_DUTY_CYCLE_MAIN_PRIMARY_HEATER  0.50
#define INITIAL_DUTY_CYCLE_SMALL_PRIMARY_HEATER 0.10
#define INITIAL_DUTY_CYCLE_TRIMMER_HEATER 0.10

#define DELTA_TEMP_TO_PWM_DUTY_CYCLE_CONVERSION  0.03

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 smallPrimaryHeaterPWMDutyCycle;
static F32 mainPrimaryHeaterPWMDutyCycle;
static F32 trimmerHeaterPWMDutyCycle;

// 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 );

// TODo GPIO for enabling the primary heater
#define PRIMARY_HEATER_MIBSPI1_PORT_MASK     0x00000002  // (CS1 - re-purposed as input GPIO)
#define TOGGLEPRIMAYHEATER()                 ( ( mibspiREG1->PC2 & PRIMARY_HEATER_MIBSPI1_PORT_MASK ) != 0 )

// Public functions

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;
    smallPrimaryHeaterPWMDutyCycle = 0.0;
    mainPrimaryHeaterPWMDutyCycle = 0.0;
    trimmerHeaterPWMDutyCycle = 0.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 );
}

void setPrimaryHeaterTargetTemperature ( F32 targetTemp )
{
    primaryHeaterTargetTemperature = targetTemp;
}

void setTrimmerHeaterTargetTemperature ( F32 targetTemp )
{
    trimmerHeaterTargetTemperature = targetTemp;
}

BOOL startPrimaryHeater ( void )
{
    BOOL status = FALSE;
    // TODO: remove this code. For testing only
    setMainPrimaryHeaterPWM ( 0.8 );
    setSmallPrimaryHeaterPWM ( 0.25 );
    // TODO: remove this code. for testing only

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

    return status;
}

void startTrimmerHeater ( void )
{
    setTrimmerHeaterPWM ( INITIAL_DUTY_CYCLE_MAIN_PRIMARY_HEATER );
}

void stopPrimaryHeater ( void )
{
    setMainPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
    setSmallPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
}

void stopTrimmerHeater ( void )
{
    setTrimmerHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
}

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;
}

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;
    }
}

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

static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateOff ( void )
{
    PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;

    return state;
}

static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateControlToTarget ( void )
{
    PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;

    // TODO do we need to have a counter for the controller?
    F32 outletTemp = getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR );
    F32 newPWM = runPIController ( PI_CONTROLLER_ID_PRIMARY_HEATER, primaryHeaterTargetTemperature, outletTemp );

    if ( newPWM >= MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE )
    {
        setMainPrimaryHeaterPWM ( MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE );
        newPWM = newPWM - MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE;
        setSmallPrimaryHeaterPWM ( newPWM );
    }
    else
    {

    }


    // TODO remove this code for testing
    if ( TOGGLEPRIMAYHEATER() )
    {
        startPrimaryHeater();
    }
    if ( !TOGGLEPRIMAYHEATER() )
    {
        stopPrimaryHeater();
    }
    // TODO remove this code for testing

    return state;
}

static void setMainPrimaryHeaterPWM( F32 pwm )
{
    etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

static void setSmallPrimaryHeaterPWM ( F32 pwm )
{
    etpwmSetCmpB( etpwmREG1, (U32)( (S32)( ( pwm * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

static void setTrimmerHeaterPWM( F32 pwm )
{
    etpwmSetCmpA( etpwmREG3, (U32)( (S32)( ( pwm * (F32)(etpwmREG3->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) );
}

static void resetHeaterState ( NAME_OF_HEATER_T heater )
{
    if ( heater == PRIMARY_HEATER )
    {
        F32 inletTemperatrue = getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR );
        F32 initialPWMDutyCycle = fabs(primaryHeaterTargetTemperature - inletTemperatrue) * DELTA_TEMP_TO_PWM_DUTY_CYCLE_CONVERSION;
        resetPIController( PI_CONTROLLER_ID_PRIMARY_HEATER, initialPWMDutyCycle );
    }
    else if ( heater == TRIMMER_HEATER )
    {
        // TODO setup the trimmer heater
    }
}