#include "FIRfilterPtByPt.h"

/**
* \brief   Function used for initialization FIR filter structure used for filtering
* \param[in]   signalBufferLength    Length of cyclic buffer storing signal
* \param[in]   signalCyclicBuffer[]   Cyclic buffer storing signal
* \param[in]   coeffLength              Number of filter coefficients
* \param[in]   coefficients[]            Filter coefficients
* \return        FIRfilterObject          FIR filter structure
*/
FIRfilterObject CreateFIRfilterObject(uint16_t signalBufferLength, float signalCyclicBuffer[], uint16_t coeffLength, float coefficients[])
{
                FIRfilterObject _FIRfilterObject;
                _FIRfilterObject.coefficients = &coefficients[0];
                _FIRfilterObject.coefficientsLength = coeffLength;
                _FIRfilterObject.signalBuffer = &signalCyclicBuffer[0];
                _FIRfilterObject.signalBufferHead = 0;
                _FIRfilterObject.signalBufferLength = signalBufferLength;
                _FIRfilterObject.signalBufferMask = signalBufferLength - 1;
               
        //======= Zero out signalBuffer values  =======//
                uint16_t i;
                uint16_t counterBy8 = _FIRfilterObject.signalBufferLength >> 3;  //  Division by 8
                uint16_t remainder = _FIRfilterObject.signalBufferLength - ((_FIRfilterObject.signalBufferLength >> 3) << 3);  // % 8
                float* signalBufferPtr = &_FIRfilterObject.signalBuffer[0];
       
                for(i=0; i<counterBy8; i++)
                {
                                *signalBufferPtr++ = 0.0f;
                                *signalBufferPtr++ = 0.0f;
                                *signalBufferPtr++ = 0.0f;
                                *signalBufferPtr++ = 0.0f;
                                *signalBufferPtr++ = 0.0f;
                                *signalBufferPtr++ = 0.0f;
                                *signalBufferPtr++ = 0.0f;
                                *signalBufferPtr++ = 0.0f;
                }
               
                for(i=0; i<remainder; i++)
                {
                                *signalBufferPtr++ = 0.0f;
                }
        //===================================//
               
                return _FIRfilterObject;
}

/**
* \brief   Function filtering input signal point-by-point
* \param[in]     filterObject    Pointer to FIR filter structure
* \param[in]     signal           One sample of input signal
* \param[out]   result            Result of filtering
*/
void FIRfilterPtByPt(FIRfilterObject* filterObject, float signal, float* result)
{
    *result = 0;
    filterObject->signalBuffer[filterObject->signalBufferHead] = signal;
    float* coefficientsPtr;
    coefficientsPtr        = &filterObject->coefficients[0];
    float* signalPtr;
    signalPtr        = &filterObject->signalBuffer[filterObject->signalBufferHead];
    int16_t i;
    int16_t counterBy8;
    int16_t remainder;
       
                if( filterObject->signalBufferHead >=  filterObject->coefficientsLength)
                {
        counterBy8 = filterObject->coefficientsLength >> 3;   //  filterObject->coefficientsLength / 8
        remainder = filterObject->coefficientsLength - ((filterObject->coefficientsLength >> 3) << 3);  // filterObject->coefficientsLength % 8
                                for(i=0; i<counterBy8; i++)
                                {
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                }
                               
                                for(i=0; i<remainder; i++)
                                {
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                }
                }
                else
                {       
        counterBy8 = filterObject->signalBufferHead >> 3;  //  filterObject->signalBufferHead / 8
        remainder = filterObject->signalBufferHead - ((filterObject->signalBufferHead >> 3) << 3);  // filterObject->signalBufferHead % 8

                                for(i=0; i<counterBy8; i++)
                                {
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                                *result += *signalPtr-- * *coefficientsPtr++;
                                }
                               
        for(i=0; i<remainder; i++)
        {
            *result += *signalPtr-- * *coefficientsPtr++;
        }
                               
        *result += filterObject->signalBuffer[0] * *coefficientsPtr++;
        signalPtr = &filterObject->signalBuffer[filterObject->signalBufferLength-1];
                               
        counterBy8 = filterObject->coefficientsLength - filterObject->signalBufferHead - 1;
        remainder = counterBy8 - ((counterBy8 >> 3) << 3);  // counterBy8 % 8
        counterBy8 = counterBy8 >> 3;  //  counterBy8 / 8
                               
        for(i=0; i<counterBy8; i++)
        {
            *result += *signalPtr-- * *coefficientsPtr++;
            *result += *signalPtr-- * *coefficientsPtr++;
            *result += *signalPtr-- * *coefficientsPtr++;
            *result += *signalPtr-- * *coefficientsPtr++;
            *result += *signalPtr-- * *coefficientsPtr++;
            *result += *signalPtr-- * *coefficientsPtr++;
            *result += *signalPtr-- * *coefficientsPtr++;
            *result += *signalPtr-- * *coefficientsPtr++;
        }               
                               
        for(i=0; i<remainder; i++)
        {
            *result += *signalPtr-- * *coefficientsPtr++;
        }
    }
       
    filterObject->signalBufferHead = (filterObject->signalBufferHead + 1) & filterObject->signalBufferMask;
}




#include "IIRfilterPtByPt.h"

/**
* \brief   Function used for initialization IIR 2nd order filter structure used for filtering
* \param[in]   forwardCoeff[3]            Filter forward coefficients
* \param[in]   reverseCoeff[3]            Filter reverse coefficients
* \return        IIR2ndOrderFilterObj    IIR 2nd order filter structure
*/
IIR2ndOrderFilterObj IIR_2ndOrderSignalFilteringPtByPtInitialize(float forwardCoeff[3], float reverseCoeff[3])
{
    IIR2ndOrderFilterObj  IIR2ndOrderFilterObj_;
    IIR2ndOrderFilterObj_.forwardCoeff = forwardCoeff;
    IIR2ndOrderFilterObj_.reverseCoeff = reverseCoeff;

    return IIR2ndOrderFilterObj_;
}

/**
* \brief   Function used for initialization IIR 3rd order filter structure used for filtering
* \param[in]   forwardCoeff[4]            Filter forward coefficients
* \param[in]   reverseCoeff[4]            Filter reverse coefficients
* \return        IIR3rdOrderFilterObj     IIR 3rd order filter structure
*/
IIR3rdOrderFilterObj IIR_3rdOrderSignalFilteringPtByPtInitialize(float forwardCoeff[4], float reverseCoeff[4])
{
    IIR3rdOrderFilterObj  IIR3rdOrderFilterObj_;
    IIR3rdOrderFilterObj_.forwardCoeff = forwardCoeff;
    IIR3rdOrderFilterObj_.reverseCoeff = reverseCoeff;

    return IIR3rdOrderFilterObj_;
}

/**
* \brief   Function used for initialization IIR 4th order filter structure used for filtering
* \param[in]   forwardCoeff[5]            Filter forward coefficients
* \param[in]   reverseCoeff[5]            Filter reverse coefficients
* \return        IIR4thOrderFilterObj     IIR 4th order filter structure
*/
IIR4thOrderFilterObj IIR_4thOrderSignalFilteringPtByPtInitialize(float forwardCoeff[5], float reverseCoeff[5])
{
    IIR4thOrderFilterObj  IIR4thOrderFilterObj_;
    IIR4thOrderFilterObj_.forwardCoeff = forwardCoeff;
    IIR4thOrderFilterObj_.reverseCoeff = reverseCoeff;

    return IIR4thOrderFilterObj_;
}

/**
* \brief   Function for recursive Infinite Impulse Response 2nd order filtering of input signal point-by-point
* \warning This IIR filter implemantation can be used only when first reverse coefficient value == 1
* \param[in]  filterObject       Pointer to structure of 2nd order IIR filter
* \param[in]  signal               Input signal last sample
* \param[out] result              Output filtered signal sample
*/
void IIR_2ndOrderSignalFilteringPtByPt(IIR2ndOrderFilterObj* filterObject, float signal, float* result)
{
                static float as;           

                filterObject->stateBuffer[0] += signal * filterObject->forwardCoeff[0];
    filterObject->stateBuffer[1] += signal * filterObject->forwardCoeff[1];
    filterObject->stateBuffer[2] += signal * filterObject->forwardCoeff[2];

                as = -filterObject->stateBuffer[0];

                filterObject->stateBuffer[1] += as * filterObject->reverseCoeff[1];
    filterObject->stateBuffer[2] += as * filterObject->reverseCoeff[2];

                *result = filterObject->stateBuffer[0];

                filterObject->stateBuffer[0] = filterObject->stateBuffer[1];
    filterObject->stateBuffer[1] = filterObject->stateBuffer[2];
    filterObject->stateBuffer[2] = 0;
}

/**
* \brief   Function for recursive Infinite Impulse Response 3rd order filtering of input signal point-by-point
* \warning This IIR filter implemantation can be used only when first reverse coefficient value == 1
* \param[in]  filterObject       Pointer to structure of 3rd order IIR filter
* \param[in]  signal               Input signal last sample
* \param[out] result              Output filtered signal sample
*/
void IIR_3rdOrderSignalFilteringPtByPt(IIR3rdOrderFilterObj* filterObject, float signal, float* result)
{
                static float as;           

                filterObject->stateBuffer[0] += signal * filterObject->forwardCoeff[0];
    filterObject->stateBuffer[1] += signal * filterObject->forwardCoeff[1];
    filterObject->stateBuffer[2] += signal * filterObject->forwardCoeff[2];
          filterObject->stateBuffer[3] += signal * filterObject->forwardCoeff[3];

                as = -filterObject->stateBuffer[0];

                filterObject->stateBuffer[1] += as * filterObject->reverseCoeff[1];
    filterObject->stateBuffer[2] += as * filterObject->reverseCoeff[2];
          filterObject->stateBuffer[3] += as * filterObject->reverseCoeff[3];

                *result = filterObject->stateBuffer[0];

                filterObject->stateBuffer[0] = filterObject->stateBuffer[1];
    filterObject->stateBuffer[1] = filterObject->stateBuffer[2];
    filterObject->stateBuffer[2] = filterObject->stateBuffer[3];
          filterObject->stateBuffer[3] = 0;
}

/**
* \brief   Function for recursive Infinite Impulse Response 4th order filtering of input signal point-by-point
* \warning This IIR filter implemantation can be used only when first reverse coefficient value == 1
* \param[in]  filterObject       Pointer to structure of 4th order IIR filter
* \param[in]  signal               Input signal last sample
* \param[out] result              Output filtered signal sample
*/
void IIR_4thOrderSignalFilteringPtByPt(IIR4thOrderFilterObj* filterObject, float signal, float* result)
{
                static float as;           

                filterObject->stateBuffer[0] += signal * filterObject->forwardCoeff[0];
    filterObject->stateBuffer[1] += signal * filterObject->forwardCoeff[1];
    filterObject->stateBuffer[2] += signal * filterObject->forwardCoeff[2];
          filterObject->stateBuffer[3] += signal * filterObject->forwardCoeff[3];
          filterObject->stateBuffer[4] += signal * filterObject->forwardCoeff[4];

                as = -filterObject->stateBuffer[0];

                filterObject->stateBuffer[1] += as * filterObject->reverseCoeff[1];
    filterObject->stateBuffer[2] += as * filterObject->reverseCoeff[2];
          filterObject->stateBuffer[3] += as * filterObject->reverseCoeff[3];
          filterObject->stateBuffer[4] += as * filterObject->reverseCoeff[4];

                *result = filterObject->stateBuffer[0];

                filterObject->stateBuffer[0] = filterObject->stateBuffer[1];
    filterObject->stateBuffer[1] = filterObject->stateBuffer[2];
    filterObject->stateBuffer[2] = filterObject->stateBuffer[3];
          filterObject->stateBuffer[3] = filterObject->stateBuffer[4];
                filterObject->stateBuffer[4] = 0;
}