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

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

typedef struct {
    // -- PI's parameters --
    F32                 Kp;                         // Proportional Value
    F32                 Ki;                         // Integral Value
    S32                 uMax;                       // Maximum control signal
    S32                 uMin;                       // Minimum control signal
    // -- PI's signals --
    BOOL                firstIteration;             // Mark true for the first iteration
    S32                 referenceSignal;            // reference signal
    S32                 measuredSignal;             // measured signal
    S32                 errorSignal;                // reference - measured signal
    S32                 errorSumBeforeWindUp;       // error signal before windup correction
    S32                 errorSum;                   // error integral after windup correction
    S32                 controlSignal;              // actual control signal
} PI_CONTROLLER_T;

// PI Controllers -- definition

static PI_CONTROLLER_T piControllers[ NUM_OF_PI_CONTROLLERS_IDS ] =
{

                    { /*Kp*/ 1.0, /*Ki*/ 1.0, /*uMax*/ 90, /*uMin*/ 0}  // PI_CONTROLLER_ID_LOAD_CELL = 0
};

static PI_CONTROLLER_T* m_controller;

#define SET_CONTROLLER(id)  (m_controller = &piControllers[id])

/*************************************************************************
 * @brief initializeController
 * Initialize controller before operation. Make sure to run 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 initializeController(PI_CONTROLLER_ID_T controllerID, U16 initialControlSignal)
{

    SET_CONTROLLER(controllerID);

    m_controller->firstIteration = TRUE;

    m_controller->controlSignal = RANGE(initialControlSignal,
                                        m_controller->uMin,
                                        m_controller->uMax);
}

/*************************************************************************
 * @brief runController
 * 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
 *************************************************************************/
S32 runController(PI_CONTROLLER_ID_T controllerID, S32 referenceSignal, S32 measuredSignal)
{

    SET_CONTROLLER(controllerID);

    m_controller->referenceSignal = referenceSignal;
    m_controller->measuredSignal = measuredSignal;
    m_controller->errorSignal = referenceSignal - measuredSignal;
    m_controller->errorSum += m_controller->errorSignal;
    m_controller->errorSumBeforeWindUp = m_controller->errorSum;

    if( !m_controller->firstIteration || ( m_controller->controlSignal == 0 ) )
    {
        S32 controlSignalBeforeWindup = (S32) (m_controller->Kp * m_controller->errorSignal +
                                         m_controller->Ki * m_controller->errorSum);

        m_controller->controlSignal = RANGE(controlSignalBeforeWindup, m_controller->uMin, m_controller->uMax);

        S32 error = m_controller->controlSignal - controlSignalBeforeWindup;

        // Anti-wind up logic

        if ( error != 0 )
        {
            // We have hit a max or min, need to compensate
            m_controller->errorSum -= (S32)(error/m_controller->Ki);
        }
    }
    else
    {
        m_controller->errorSum = (S32) ((m_controller->controlSignal - m_controller->Kp*m_controller->errorSignal)/(m_controller->Ki));
    }


    return m_controller->controlSignal;
}

/*************************************************************************
 * @brief getController
 * Returns the latest requested signal sample.
 *
 * @param controllerID - ID filter number
 * @param signalID -  signal sample ID request
 *
 * @return latest sample requested
 *************************************************************************/
S32 getControllerSignals(PI_CONTROLLER_ID_T controllerID, PI_CONTROLLER_SIGNALS_ID signalID)
{

    SET_CONTROLLER(controllerID);

    S32 output;

    switch(signalID) {

        case CONTROLLER_SIGNAL_REFERENCE:
            output = m_controller->referenceSignal;
            break;

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

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

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

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

        case CONTROLLER_SIGNAL_PROPORTIONAL_OUTPUT:
            output = (S32)(m_controller->Kp * m_controller->errorSignal);
            break;

        case CONTROLLER_SIGNAL_INTEGRAL_OUTPUT:
            output = (S32)(m_controller->Ki * m_controller->errorSum);
            break;

        case CONTROLLER_SIGNAL_CONTROL_BEFORE_WINDUP:
            output = (S32)(m_controller->Ki * m_controller->errorSumBeforeWindUp +
                           m_controller->Kp * m_controller->errorSignal);
            break;

        case CONTROLLER_SIGNAL_CONTROL:
            output = (S32)(m_controller->Ki * m_controller->errorSum +
                           m_controller->Kp * m_controller->errorSignal);
            break;

        default:
            output = 0;
            break;
    } // end of switch

    return output;
}