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

/*! \brief  Contains public interface to various functions related
 *         to the Band-stop (NOTCH) 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 BS_SO object
 *  \note   The transfer function is as follows:
 *
 *                      1-2*cosWn*z^-1 + z^-2
 *        H(z) = b-----------------------------------
 *                   1 - 2*b*cosWn*z^-1 + (2*b -1)*z^-2
 *
 *                    1
 *        b = ---------------
 *             1+tan(deltaW/2)
 *
 *        a1= -2*b*cosWn
 *
 *        a2= 2*b - 1
 *
 */
typedef struct _BsSoType_
{
    _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) */
    _iq  coeff4;         /*!< Parameter: The coefficient of X(k-1) */
    _iq  coeff5;         /*!< Parameter: The coefficient of 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;         /*!< Output: The output value */
} BsSoType;

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

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

/*! \brief Theoretically initialize the BS_SO object with detailed parameters
 *
 * This function initializes the BsSo instance using the unit of rad/s
 *
 * \param[in] obj : pointer to BsSo instance
 * \param[in] Wc_rps : the center frequency in rad/s
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] deltaW : 3-dB bandwidth in rad/s
 */
static inline void BsSo_Init(BsSoType *obj, float Wc_rps, float runFreq_kHz, float deltaW)
{
    float b     = 1 / (tan((deltaW / runFreq_kHz * 0.001) / 2) + 1);
    float a2    = 2 * b - 1;
    float a1    = -2 * b * cos(Wc_rps / runFreq_kHz * 0.001);
    obj->coeff1 = _IQ(a1);
    obj->coeff2 = _IQ(a2);
    obj->coeff3 = _IQ(b);
    obj->coeff4 = obj->coeff1;
    obj->coeff5 = obj->coeff3;

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

/*! \brief Theoretically initialize the BS_SO object with detailed parameters, 2nd interface
 *
 * This function is an overload initialization that
 * initialize the BsSo instance using the unit of Hz
 *
 * \param[in] obj : pointer to BsSo instance
 * \param[in] fc_Hz : the center frequency in Hz
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] deltaF : 3 dB bandwidth in Hz
 */
static inline void BsSo_Init2(BsSoType *obj, float fc_Hz, float runFreq_kHz, float deltaF)
{
    float b     = 1 / (tan((MATH_2PI * deltaF / runFreq_kHz * 0.001) / 2) + 1);
    float a2    = 2 * b - 1;
    float a1    = -2 * b * cos(MATH_2PI * fc_Hz / runFreq_kHz * 0.001);
    obj->coeff1 = _IQ(a1);
    obj->coeff2 = _IQ(a2);
    obj->coeff3 = _IQ(b);
    obj->coeff4 = obj->coeff1;
    obj->coeff5 = obj->coeff3;

    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 is used to push data into the filter
 *
 * \param[in] obj : pointer to BsSo instance
 * \param[in] input : the input data in IQ format
 */
static inline void BsSo_PushInput(BsSoType *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, obj->coeff3) + _IQmpy(obj->prevInput, obj->coeff4)
                      + _IQmpy(obj->prevPrevInput, obj->coeff5);

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

/*! \brief  Gets the output value
 *
 * This function gets the output of the filter
 *
 * \param[in] obj : pointer to BsSo instance
 * \return the result of the filter in IQ format
 */
static inline _iq BsSo_GetOutput(const BsSoType *obj)
{
    return obj->output;
}

/*! \brief  Resets the filter
 *
 * This function resets the filter to initial state
 *
 * \param[in] obj : pointer to BsSo instance
 */
static inline void BsSo_Reset(BsSoType *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 parameters of the filter with unit rad/s
 *
 * \param[in] obj : pointer to BsSo instance
 * \param[in] Wc_rps : the center frequency in rad/s
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] deltaW : 3-dB bandwidth in rad/s
 */
static inline void BsSo_UpdateParams(BsSoType *obj, float Wc_rps, float runFreq_kHz, float deltaW)
{
    float b     = 1 / (tan((deltaW / runFreq_kHz * 0.001) / 2) + 1);
    float a2    = 2 * b - 1;
    float a1    = -2 * b * cos(Wc_rps / runFreq_kHz * 0.001);
    obj->coeff1 = _IQ(a1);
    obj->coeff2 = _IQ(a2);
    obj->coeff3 = _IQ(b);
    obj->coeff4 = obj->coeff1;
    obj->coeff5 = obj->coeff3;
}

/*! \brief Update the parameters of the filter
 *
 * This function update the parameters of the filter with unit Hz
 *
 * \param[in] obj : pointer to BsSo instance
 * \param[in] fc_Hz : the center frequency in Hz
 * \param[in] runFreq_kHz : the frequency pushing the data in
 * \param[in] deltaF : 3 dB bandwidth in Hz
 */
static inline void BsSo_UpdateParams2(BsSoType *obj, float fc_Hz, float runFreq_kHz, float deltaF)
{
    float b     = 1 / (tan((MATH_2PI * deltaF / runFreq_kHz * 0.001) / 2) + 1);
    float a2    = 2 * b - 1;
    float a1    = -2 * b * cos(MATH_2PI * fc_Hz / runFreq_kHz * 0.001);
    obj->coeff1 = _IQ(a1);
    obj->coeff2 = _IQ(a2);
    obj->coeff3 = _IQ(b);
    obj->coeff4 = obj->coeff1;
    obj->coeff5 = obj->coeff3;
}

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

#endif /* _BS_SO_H_ */
