/**************************************************************************
*
* Copyright (c) 2019-2022 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)      Sean Nash
* @date    (last)      02-Jul-2021
*
* @author  (original)  Sean
* @date    (original)  27-Dec-2019
*
***************************************************************************/

#include <math.h>

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

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

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

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

/// Enumeration of PI controller direction.
typedef enum controller_Directions
{
    CONTROLLER_BIDIRECTIONAL = 0,           ///< Controller runs bidirectional so it covers positive and negative control signals
    CONTROLLER_UNIDIRECTIONAL,              ///< Controller runs unidirectional so it only covers positive control signals
    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.
    F32 maxErrorSumStep;                    ///< maximum change in I (error sum) for a single control interval.
    PI_CONTROLLER_DIRECTIONS_T direction;   ///< PI controller control direction.
} PI_CONTROLLER_T;

/// Macro to set a local controller pointer to a given piController.
#define SET_CONTROLLER( c, id )     ((c) = &piControllers[id])

// ********** 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  Ilimit controller type
    { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, CONTROLLER_UNIDIRECTIONAL },  // 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, 100.0, CONTROLLER_UNIDIRECTIONAL }   // PI_CONTROLLER_ID_DIALYSATE_FLOW
};

/*********************************************************************//**
 * @brief
 * Initialize controller before operation. Make sure to call it before
 * first call to runController function.
 * @details Inputs: piControllers
 * @details Outputs: piControllers
 * @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 )
{
    if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS )
    {
        PI_CONTROLLER_T *controller;

        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_HD_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.
 * @details Inputs: piControllers
 * @details Outputs: piControllers
 * @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_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }
}

/*********************************************************************//**
 * @brief
 * Call this function whenever a new measured signal sampled is acquired.
 * @details Inputs: piControllers
 * @details Outputs: piControllers
 * @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 )
        {
            controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal );
        }
        else
        {
            controller->errorSignal = referenceSignal - measuredSignal;
        }

        // Limit error sum step size
        if ( fabs( controller->errorSignal ) > controller->maxErrorSumStep )
        {
            if ( controller->errorSignal < 0.0 )
            {
                controller->errorSum += ( controller->maxErrorSumStep * -1.0 );
            }
            else
            {
                controller->errorSum += controller->maxErrorSumStep;
            }
        }
        else
        {
            controller->errorSum += controller->errorSignal;
        }

        // Calculate control signal from i term
        controller->errorSumBeforeWindUp = controller->errorSum;
        controlSignalBeforeWindup = ( 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 );
        }

        // Add p term to control signal and re-apply range limits
        controller->controlSignal += ( controller->Kp * controller->errorSignal );
        controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax );

        result = controller->controlSignal;
    }
    else
    {
        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }

    return result;
}

/*********************************************************************//**
 * @brief
 * Returns the latest requested signal sample.
 * @details Inputs: piControllers
 * @details Outputs: piControllers
 * @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_HD_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_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
    }

    return output;
}

/**@}*/