/*
 * Copyright (c) 2022, Shenzhen CVA Innovation CO.,LTD
 * All rights reserved.
 *
 * Shenzhen CVA Innovation CO.,LTD (CVA chip) is supplying this file for use
 * exclusively with CVA's microcontroller products. This file can be freely
 * distributed within development tools that are supporting such microcontroller
 * products.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * CVA SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL,
 * OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 */

#ifndef _PID_H_
#define _PID_H_

/*! \brief  Contains public interface to various functions related
 *          to the PID object.
 *  \note   The form of this PID is parallel.
 */

/*******************************************************************************
 * the includes
 ******************************************************************************/

#include "common/iqmath/iqmath.h"

#ifdef __cplusplus
extern "C" {
#endif

/*******************************************************************************
 * the defines
 ******************************************************************************/

/*******************************************************************************
 * the typedefs
 ******************************************************************************/

/*! \brief  Declaration of PID object
 */
typedef struct _PidType_
{
    _iq ref; /*!< Input: Reference input */
    _iq fdb; /*!< Input: Feedback input */

    _iq Kp;     /*!< Parameter: Proportional gain */
    _iq Ki;     /*!< Parameter: Integral gain */
    _iq Kd;     /*!< Parameter: Derivative gain */
    _iq outMax; /*!< Parameter: Maximum output */
    _iq outMin; /*!< Parameter: Minimum output */

    _iq Ui;        /*!< Internal Variable: Integral output */
    _iq lastError; /*!< Internal Variable: last error */

    _iq output; /*!< Output: PID output */
} PidType;

/*******************************************************************************
 * the globals
 ******************************************************************************/

/*******************************************************************************
 * the function prototypes
 ******************************************************************************/

/*! \brief Initialize the PID object
 *
 * This function initializes the PID instance
 *
 * \param[in] obj : pointer to Pid instance
 * \param[in] Kp : the Kp value in IQ format
 * \param[in] Ki : the Ki value in IQ format
 * \param[in] Kd : the Kd value in IQ format
 * \param[in] min : the min value allowed in output
 * \param[in] max : the max value allowed in output
 */
static inline void Pid_Init(PidType *obj, _iq Kp, _iq Ki, _iq Kd, _iq min, _iq max)
{
    obj->Kp     = Kp;
    obj->Ki     = Ki;
    obj->Kd     = Kd;
    obj->outMin = min;
    obj->outMax = max;

    obj->ref       = 0;
    obj->fdb       = 0;
    obj->Ui        = 0;
    obj->lastError = 0;
    obj->output    = 0;
}

/*! \brief Run the PID control
 *
 * This fucntion runs the PID control
 *
 * \note The form of this PID is parallel
 *
 * \param[in] obj : pointer to Pid instance
 */
static inline void Pid_Run(PidType *obj)
{
    /* Compute the error */
    _iq error = obj->ref - obj->fdb;

    /* Compute the proportional output */
    _iq Up = _IQmpy(obj->Kp, error);

    /* Compute the integral output */
    obj->Ui = obj->Ui + _IQmpy(obj->Ki, error);
    /* Saturate Ui */
    if(obj->Ui > obj->outMax)
    {
        obj->Ui = obj->outMax;
    }
    if(obj->Ui < obj->outMin)
    {
        obj->Ui = obj->outMin;
    }

    /* Compute the derivative output */
    _iq Ud         = _IQmpy(obj->Kd, (error - obj->lastError));
    obj->lastError = error;

    /* Compute the pre-saturated output */
    _iq tmpOutput = Up + obj->Ui + Ud;

    /* Saturate the output */
    if(tmpOutput > obj->outMax)
    {
        obj->output = obj->outMax;
    }
    else if(tmpOutput < obj->outMin)
    {
        obj->output = obj->outMin;
    }
    else
    {
        obj->output = tmpOutput;
    }
}

/*! \brief Reset the PID variables
 *
 * This function reset the PID to initial state
 *
 * \note The form of this PID is parallel
 *
 * \param[in] obj : pointer to Pid instance
 */
static inline void Pid_Reset(PidType *obj)
{
    obj->lastError = 0;
    obj->output    = 0;
    obj->Ui        = 0;
}

#ifdef __cplusplus
}
#endif /* extern "C" */

#endif /* _PID_H_ */