/**************************************************************************
 *
 *  Copyright (c) 2019-2019 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 service module.  Creates a digital PI to be used as
 *  control loops
 *
 **************************************************************************/

#include "math.h"

#include "PIControllers.h"

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

#define MIN_KI                      NEARLY_ZERO

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])

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

// PI Controllers -- definition

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_LOAD_CELL
    { 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 initializePIController
 * Initialize controller before operation. Make sure to call it before \n
 * first call to runController function.
 *
 * @param controllerID - ID filter number
 * @param initialControlSignal - Value of the output on the first iteration
 *
 * @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 );
    }
}

/*************************************************************************
 * @brief resetPIController
 * Reset controller before new set point. Make sure to call it before first \n
 * 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;
    }
}

/*************************************************************************
 * @brief runPIController
 * 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 = 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;
    }

    return result;
}

/*************************************************************************
 * @brief getPIControllerSignals
 * 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:
                output = 0;
                break;
        } // end of switch
    }

    return output;
}