/**********************************************************************//**
 *
 *  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
 *
 *  @date 18-Dec-2019
 *  @author L. Baloa
 *
 *  @brief PIControllers source file.
 *
 **************************************************************************/

#ifndef _VECTORCAST_
    #include "math.h"
#endif

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

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

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

/// minimum integral coefficient - cannot be zero.
#define MIN_KI                      NEARLY_ZERO

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

#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 },  // PI_CONTROLLER_ID_ULTRAFILTRATION
    { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },  // PI_CONTROLLER_ID_BLOOD_FLOW
    { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }   // PI_CONTROLLER_ID_DIALYSATE_FLOW
};

/**@}*/

/*********************************************************************//**
 * @brief
 * Initialize controller before operation. Make sure to call it before
 * first call to runController function.
 *
 * @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_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }
}

/*********************************************************************//**
 * @brief
 * Reset controller before new set point. Make sure to call it before first
 * call to runController function.
 *
 * @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_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }
}

/*********************************************************************//**
 * @brief
 * Call this function whenever a new measured signal sampled is acquired.
 *
 * @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
        controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : 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;
//#ifdef DEBUG_ENABLED
//    {
//        // TODO - temporary debug code - remove later
//        char debugFlowStr[ 100 ];
//        S32  nums = (S32)(measuredSignal);
//        S32  decs = (S32)(fabs(measuredSignal-(S32)(measuredSignal))*100.0);
//        S32  nume = (S32)controller->errorSignal;
//        S32  dece = (S32)(fabs(controller->errorSignal-(S32)controller->errorSignal)*100.0);
//        S32  numes = (S32)controller->errorSum;
//        S32  deces = (S32)((controller->errorSum-(S32)(controller->errorSum))*100.0);
//        S32  nump = (S32)controller->controlSignal;
//        S32  decp = (S32)((controller->controlSignal-(S32)controller->controlSignal)*10000.0);
//
//        sprintf( debugFlowStr, "%6d.%02d  %6d.%02d  %10d.%02d  %3d.%04d\n", nums, decs, nume, dece, numes, deces, nump, decp );
//        sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) );
//    }
//#endif
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * Returns the latest requested signal sample.
 *
 * @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_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_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }

    return output;
}

/**@}*/