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

/*! \brief  Contains public interface to various functions related
 *         to the 1st-Order IIR (IirSo) low-pass filter object
 */

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

#include <stdint.h>
#include <stdbool.h>
#include "common/iqmath/iqmath.h"

#ifdef __cplusplus
extern "C" {
#endif

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

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

/*!  \brief  Defines the IirSo object
 *   \note   The transfer function is as follows:
 *
 *                           Wc^2
 *          H(s) = -------------------------
 *                  s^2 + 2*Zeta*Wc*s + Wc^2
 *
 *          The discrete equation uses Tustin method, in which
 *
 *                   2     z - 1
 *              s = --- * -------
 *                   T     z + 1
 *
 *          We add pre-warping to compensate the frequency, in which
 *
 *                    2           T
 *              W' = --- * tan(W*---)
 *                    T           2
 */
typedef struct _IirSoType_
{
    _iq coeff1; /*!< Parameter: The coefficient of Y(k-1) */
    _iq coeff2; /*!< Parameter: The coefficient of Y(k-2) */
    _iq coeff3; /*!< Parameter: The coefficient of X(k)+2*X(k-1)+X(k-2) */

    bool isInitial;      /*!< Internal variable: Has the filter got the first input */
    _iq  prevOutput;     /*!< Internal variable: The previous output cached */
    _iq  prevPrevOutput; /*!< Internal variable: The previous previous output cached */
    _iq  prevInput;      /*!< Internal variable: The previous input cached */
    _iq  prevPrevInput;  /*!< Internal variable: The previous previous input cached */
    _iq  output;
} IirSoType;

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

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

/*! \brief Initialize the IirSo object
 *
 * This function initialize the IirSo instance with parameters
 *
 * \note The Wc_rps is the cutoff frequency in the unit of rad/s
 *
 * \param[in] obj : pointer to IirSo instance
 * \param[in] Wc_rps : the cut-off frequency in rad/s
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] zeta : the zeta coefficient
 */
static inline void IirSo_Init(IirSoType *obj, float Wc_rps, float runFreq_kHz, float zeta)
{
    /* Prewarping */
    float twoph = 2000 * runFreq_kHz;
    float hptwo = 0.0005 / runFreq_kHz;
    Wc_rps      = twoph * tan(Wc_rps * hptwo);

    _iq tmp = _IQ(Wc_rps / runFreq_kHz * 0.001);
    _iq c1  = _IQmpy(tmp, tmp);
    _iq c2  = _IQ(zeta * Wc_rps / runFreq_kHz * 0.001);
    _iq k   = _IQ(1) + c2 + _IQdiv4(c1);

    obj->coeff1 = _IQdiv(_IQ(2) - _IQdiv2(c1), k);
    obj->coeff2 = _IQdiv(-_IQdiv4(c1) - _IQ(1) + c2, k);
    obj->coeff3 = _IQdiv(_IQdiv4(c1), k);

    obj->isInitial      = true;
    obj->output         = 0;
    obj->prevOutput     = 0;
    obj->prevPrevOutput = 0;
    obj->prevInput      = 0;
    obj->prevPrevInput  = 0;
}

/*! \brief Theoretically initialize the IirSo object with detailed parameters, 2nd interface
 *
 * This function theoretically initialize the IirSo object with detailed parameters
 * This is an overload function.
 *
 * \note The fc_Hz is the cutoff frequency in the unit of Hz
 *
 * \param[in] obj : pointer to IirSo instance
 * \param[in] fc_Hz : the cut-off frequency in Hz
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] zeta : the zeta coefficient
 */
static inline void IirSo_Init2(IirSoType *obj, float fc_Hz, float runFreq_kHz, float zeta)
{
    /* Prewarping */
    float twoph  = 2000 * runFreq_kHz;
    float hptwo  = 0.0005 / runFreq_kHz;
    float Wc_rps = twoph * tan(MATH_2PI * fc_Hz * hptwo);

    _iq tmp = _IQ(Wc_rps / runFreq_kHz * 0.001);
    _iq c1  = _IQmpy(tmp, tmp);
    _iq c2  = _IQ(zeta * Wc_rps / runFreq_kHz * 0.001);
    _iq k   = _IQ(1) + c2 + _IQdiv4(c1);

    obj->coeff1 = _IQdiv(_IQ(2) - _IQdiv2(c1), k);
    obj->coeff2 = _IQdiv(-_IQdiv4(c1) - _IQ(1) + c2, k);
    obj->coeff3 = _IQdiv(_IQdiv4(c1), k);

    obj->isInitial      = true;
    obj->output         = 0;
    obj->prevOutput     = 0;
    obj->prevPrevOutput = 0;
    obj->prevInput      = 0;
    obj->prevPrevInput  = 0;
}

/*! \brief Push input to the filter
 *
 * This function push data into the filter
 *
 * \param[in] obj : pointer to IirSo instance
 * \param[in] input : input data in IQ format
 */
static inline void IirSo_PushInput(IirSoType *obj, _iq input)
{
    if(obj->isInitial)
    {
        obj->output         = input;
        obj->prevOutput     = input;
        obj->prevPrevOutput = input;
        obj->prevInput      = input;
        obj->prevPrevInput  = input;
        obj->isInitial      = false;
    }
    else
    {
        obj->output = _IQmpy(obj->prevOutput, obj->coeff1) + _IQmpy(obj->prevPrevOutput, obj->coeff2)
                      + _IQmpy(input + _IQmpy2(obj->prevInput) + obj->prevPrevInput, obj->coeff3);

        obj->prevPrevInput  = obj->prevInput;
        obj->prevInput      = input;
        obj->prevPrevOutput = obj->prevOutput;
        obj->prevOutput     = obj->output;
    }
}

/*! \brief Get the output value
 *
 * This function gets the filter result
 *
 * \param[in] obj : pointer to IirSo instance
 * \return the filter result
 */
static inline _iq IirSo_GetOutput(const IirSoType *obj)
{
    return obj->output;
}

/*! \brief Reset the filter
 *
 * This function resets the filter to the initial state
 *
 * \param[in] obj : pointer to IirSo instance
 */
static inline void IirSo_Reset(IirSoType *obj)
{
    obj->isInitial      = true;
    obj->output         = 0;
    obj->prevOutput     = 0;
    obj->prevPrevOutput = 0;
    obj->prevInput      = 0;
    obj->prevPrevInput  = 0;
}

/*! \brief Update the parameters of the filter
 *
 * This function updates the parameter of the filter in the runtime.
 *
 * \param[in] obj : pointer to IirSo instance
 * \param[in] Wc_rps : the cut-off frequency in rad/s
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] zeta : the zeta coefficient
 */
static inline void IirSo_UpdateParams(IirSoType *obj, float Wc_rps, float runFreq_kHz, float zeta)
{
    /* Prewarping */
    float twoph = 2000 * runFreq_kHz;
    float hptwo = 0.0005 / runFreq_kHz;
    Wc_rps      = twoph * tan(Wc_rps * hptwo);

    _iq tmp = _IQ(Wc_rps / runFreq_kHz * 0.001);
    _iq c1  = _IQmpy(tmp, tmp);
    _iq c2  = _IQ(zeta * Wc_rps / runFreq_kHz * 0.001);
    _iq k   = _IQ(1) + c2 + _IQdiv4(c1);

    obj->coeff1 = _IQdiv(_IQ(2) - _IQdiv2(c1), k);
    obj->coeff2 = _IQdiv(-_IQdiv4(c1) - _IQ(1) + c2, k);
    obj->coeff3 = _IQdiv(_IQdiv4(c1), k);
}

/*! \brief Update the parameters of the filter
 *
 * This function updates the parameter of the filter in the runtime.
 * This is an overload function.
 *
 * \param[in] obj : pointer to IirSo instance
 * \param[in] fc_Hz : the cut-off frequency in Hz
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] zeta : the zeta coefficient
 */
static inline void IirSo_UpdateParams2(IirSoType *obj, float fc_Hz, float runFreq_kHz, float zeta)
{
    /* Prewarping */
    float twoph  = 2000 * runFreq_kHz;
    float hptwo  = 0.0005 / runFreq_kHz;
    float Wc_rps = twoph * tan(MATH_2PI * fc_Hz * hptwo);

    _iq tmp = _IQ(Wc_rps / runFreq_kHz * 0.001);
    _iq c1  = _IQmpy(tmp, tmp);
    _iq c2  = _IQ(zeta * Wc_rps / runFreq_kHz * 0.001);
    _iq k   = _IQ(1) + c2 + _IQdiv4(c1);

    obj->coeff1 = _IQdiv(_IQ(2) - _IQdiv2(c1), k);
    obj->coeff2 = _IQdiv(-_IQdiv4(c1) - _IQ(1) + c2, k);
    obj->coeff3 = _IQdiv(_IQdiv4(c1), k);
}

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

#endif /* _IIR_SO_H_ */
