/*
 * 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 _SMC_H_
#define _SMC_H_

/*! \brief  Contains public interface to various functions related
 *          to the Sliding Mode Control (SMC) object.
 */

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

#include "common/iqmath/iqmath.h"

#ifdef __cplusplus
extern "C" {
#endif

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

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

/*! \brief  Defines all the parameters used to initialize SMC object
 *  \note   We use 2 coefficients to determine the current output:
 *
 *          accumulator(k) = coeff1 * accumulator(k-1) + coeff2 * Z(k)
 *
 *          If coeff1 + coeff2 = 1, this will act like a normal LPF
 *          whose gain will be 1. But sometimes we need this algorithm's
 *          gain NOT equal to 1. So 2 coefficients are put here for the
 *          user to set freely.
 */
typedef struct _Smc_ParamsType_
{
    float Kslide;   /*!< The gain of the sliding mode */
    float maxError; /*!< The max error of input */
    float coeff1;   /*!< The coefficient of last output */
    float coeff2;   /*!< The coefficient of bang-bang control value */
} Smc_ParamsType;

/*! \brief  Declaration of SMC object
 */
typedef struct _SmcType_
{
    _iq error; /*!< Input: Reference input */

    _iq Kslide;   /*!< Parameter: The gain of the sliding mode */
    _iq maxError; /*!< Parameter: The max error of input */
    _iq gain;     /*!< Parameter: The max error of input */
    _iq coeff1;   /*!< Parameter: The coefficient of last output */
    _iq coeff2;   /*!< Parameter: The coefficient of bang-bang control value */

    _iq accum; /*!< Internal variable: The accumulator of bang-bang control value */

    _iq output; /*!< Output: The output of the sliding-mode control */
} SmcType;

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

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

/*! \brief Initialize the SMC object
 *
 * This function initialize the SMC instance
 *
 * \param[in] obj : pointer to Smc instance
 * \param[in] pParams :
 */
static inline void Smc_Init(SmcType *obj, const Smc_ParamsType *pParams)
{
    /* Initialize all parameters */
    obj->Kslide   = _IQ(pParams->Kslide);
    obj->maxError = _IQ(pParams->maxError);
    obj->gain     = _IQdiv(obj->Kslide, obj->maxError);
    obj->coeff1   = _IQ(pParams->coeff1);
    obj->coeff2   = _IQ(pParams->coeff2);

    /* Initialize all internal variables */
    obj->accum = 0;

    /* Reset the outputs */
    obj->output = 0;
}

/*! \brief Run the SMC control
 *
 * This function run the SMC algorithm.
 *
 * \param[in] obj : pointer to Smc instance
 */
static inline void Smc_Run(SmcType *obj)
{
    /* Bang-bang control with linear treatment around zero */
    _iq Z;
    if(_IQabs(obj->error) < obj->maxError)
    {
        Z = _IQmpy(obj->gain, obj->error);
    }
    else if(obj->error > 0)
    {
        Z = obj->Kslide;
    }
    else
    {
        Z = -obj->Kslide;
    }

    /* Filter the bang-bang control value */
    obj->accum = _IQmpy(obj->accum, obj->coeff1) + _IQmpy(Z, obj->coeff2);

    /* Put together the final output */
    obj->output = obj->accum + Z;
}

/*! \brief  Reset the SMC variables
 *
 * This function reset the SMC instance to the initial state.
 *
 * \param[in] obj : pointer to Smc instance
 */
static inline void Smc_Reset(SmcType *obj)
{
    obj->accum  = 0;
    obj->output = 0;
}

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

#endif /* _SMC_H_ */