/**************************************************************************
*
* 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 PIControllers.c
*
* @author (last) Quang Nguyen
* @date (last) 24-Aug-2020
*
* @author (original) Sean
* @date (original) 04-Feb-2020
*
***************************************************************************/

#include "math.h"

#include "SystemCommMessages.h"
#include "PIControllers.h"

/**
 *  @addtogroup PIControllers
 *  @{
 */

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

/// Enumeration of PI controller direction.
typedef enum controller_Directions
{
    CONTROLLER_BIDIRECTIONAL = 0,          ///< Controller runs bidirectional so it only covers positive and negative values
    CONTROLLER_UNIDIRECTIONAL,             ///< Controller run unidirectional so it only covers positive values
    NUM_OF_CONTROLLELR_DIRECTIONS          ///< Number of PI controllers directions
} PI_CONTROLLER_DIRECTIONS_T;

/// Record for PI controller.
typedef struct {
    // -- PI's parameters --
    F32 Kp;                                 ///< Proportional Value.
    F32 Ki;                                 ///< Integral Value.
    F32 uMax;                               ///< Maximum control signal.
    F32 uMin;                               ///< Minimum control signal.
    // -- PI's signals --
    F32 referenceSignal;                    ///< reference signal.
    F32 measuredSignal;                     ///< measured signal.
    F32 errorSignal;                        ///< reference - measured signal.
    F32 errorSumBeforeWindUp;               ///< error signal before windup correction.
    F32 errorSum;                           ///< error integral after windup correction.
    F32 controlSignal;                      ///< actual control signal.
    PI_CONTROLLER_DIRECTIONS_T direction;   ///< PI controller control direction.
} PI_CONTROLLER_T;

#define MIN_KI                      NEARLY_ZERO                         ///< minimum integral coefficient - cannot be zero.
#define SET_CONTROLLER( c, id )     ( (c) = &piControllers[ id ] )      ///< macro to set a local controller pointer to a given piController.

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

/// PI Controllers - initial configurations.
static PI_CONTROLLER_T piControllers[ NUM_OF_PI_CONTROLLERS_IDS ] =
{   // Kp   Ki  uMax  uMin  ref meas  err  esw esum ctrl
    { 0.0, 0.0, 1.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL },  // PI_CONTROLLER_ID_RO_PUMP
    { 0.0, 0.0, 0.99, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL },  // I_CONTROLLER_ID_RO_PUMP_RAMP_UP
    { 0.0, 0.0, 3000, 300, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_BIDIRECTIONAL },   // PI_CONTROLLER_ID_DRAIN_PUMP
    { 0.0, 0.0, 1.39, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL },  // PI_CONTROLLER_ID_PRIMARY_HEATER
    { 0.0, 0.0, 0.50, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }   // PI_CONTROLLER_ID_TRIMMER_HEATER
};

/*********************************************************************//**
 * @brief
 * The initializePIController function initializes controller before operation.
 * Make sure to call it before first call to runController function.
 * Inputs : none
 * Outputs : PI controllers module initialized
 * @param controllerID ID filter number
 * @param initialControlSignal Value of the output on the first iteration
 * @param kP Coefficient for proportional
 * @param kI Coefficient for integral
 * @param controlMin Minimum control output
 * @param controlMax Maximum control output
 * @return none
 *************************************************************************/
void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal,
                             F32 kP, F32 kI, F32 controlMin, F32 controlMax )
{
    PI_CONTROLLER_T *controller;

    if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS )
    {
        SET_CONTROLLER( controller, controllerID );

        controller->Kp = kP;
        if ( fabs( kI ) > MIN_KI )      // ensure kI is not zero
        {
            controller->Ki = kI;
        }
        else
        {
            controller->Ki = ( kI < 0.0 ? MIN_KI * -1.0 : MIN_KI );
        }
        controller->uMin = controlMin;
        controller->uMax = controlMax;
        resetPIController( controllerID, initialControlSignal );
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }
}

/*********************************************************************//**
 * @brief
 * The resetPIController functions resets controller before new set point.
 * Make sure to call it before first call to runController function.
 * Inputs : none
 * Outputs : Reset a PI controller
 * @param controllerID ID filter number
 * @param initialControlSignal Value of the output on the first iteration
 * @return none
 *************************************************************************/
void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal )
{
    PI_CONTROLLER_T *controller;

    if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS )
    {
        SET_CONTROLLER( controller, controllerID );
        controller->controlSignal = RANGE( initialControlSignal, controller->uMin, controller->uMax );
        controller->referenceSignal = 0.0;
        controller->errorSignal = 0.0;
        controller->errorSum = controller->controlSignal / controller->Ki;
        controller->errorSumBeforeWindUp = controller->errorSum;
        controller->measuredSignal = 0.0;
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }
}

/*********************************************************************//**
 * @brief
 * The runPIController functions executes a PI controller with a new sampled measured signal.
 * Inputs : none
 * Outputs : Feeds new signal to PI controller
 * @param controllerID ID filter number
 * @param referenceSignal reference signal value
 * @param measuredSignal latest measured sample
 * @return value of the control signal
 *************************************************************************/
F32 runPIController(PI_CONTROLLER_ID_T controllerID, F32 referenceSignal, F32 measuredSignal)
{
    PI_CONTROLLER_T *controller;
    F32 result = 0.0;

    if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS )
    {
        F32 controlSignalBeforeWindup;
        F32 windupError;

        SET_CONTROLLER( controller, controllerID );

        controller->referenceSignal = referenceSignal;
        controller->measuredSignal = measuredSignal;

        // Calculate error signal
        if ( controller->direction == CONTROLLER_UNIDIRECTIONAL )
        {
            // To make sure that the outcome will be positive
            controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal );
        }
        else
        {
            controller->errorSignal = referenceSignal - measuredSignal;
        }

        controller->errorSum += controller->errorSignal;
        // anti-windup
        controller->errorSumBeforeWindUp = controller->errorSum;
        // calculate control signal
        controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum );
        controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax );
        // handle anti-windup for i term
        windupError = controlSignalBeforeWindup - controller->controlSignal;
        if ( fabs( windupError ) > NEARLY_ZERO )
        {
            controller->errorSum -= ( windupError / controller->Ki );
        }
        result = controller->controlSignal;
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * The getPIControllerSignals function returns the latest requested signal sample.
 * Inputs : none
 * Outputs : none
 * @param controllerID ID filter number
 * @param signalID signal sample ID request
 * @return latest sample requested
 *************************************************************************/
F32 getPIControllerSignals( PI_CONTROLLER_ID_T controllerID, PI_CONTROLLER_SIGNALS_ID signalID )
{
    PI_CONTROLLER_T *controller;
    F32 output = 0.0;

    if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS )
    {
        SET_CONTROLLER( controller, controllerID );

        switch( signalID )
        {
            case CONTROLLER_SIGNAL_REFERENCE:
                output = controller->referenceSignal;
                break;

            case CONTROLLER_SIGNAL_MEASURED:
                output = controller->measuredSignal;
                break;

            case CONTROLLER_SIGNAL_ERROR:
                output = controller->errorSignal;
                break;

            case CONTROLLER_SIGNAL_ERROR_SUM:
                output = controller->errorSumBeforeWindUp;
                break;

            case CONTROLLER_SIGNAL_ERROR_SUM_AFTER_WINDUP:
                output = controller->errorSum;
                break;

            case CONTROLLER_SIGNAL_PROPORTIONAL_OUTPUT:
                output = controller->Kp * controller->errorSignal;
                break;

            case CONTROLLER_SIGNAL_INTEGRAL_OUTPUT:
                output = controller->Ki * controller->errorSum;
                break;

            case CONTROLLER_SIGNAL_CONTROL:
                output = controller->controlSignal;
                break;

            default:
                SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID )
                break;
        } // end of switch
    }
    else
    {   // invalid controller given
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }

    return output;
}

/**@}*/