/**************************************************************************
 *
 *  Copyright (c) 2019-2019 Diality Inc. - All Rights Reserved.
 *
 *  THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
 *  WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
 *
 *  @file FIRFilters.c
 *
 *  @date 12-Dec-2019
 *  @author L. Baloa
 *
 *  @brief Filters service module.  Creates digital filters to be used in \n
 *  in Controllers and it provides an API to use them.
 *
 **************************************************************************/
#include "FIRFilters.h"

// ********** private definitions **********

typedef struct {
    U16*                inputSignal;              // input signal buffer
    F32*                bCoeffs;                  // coefficient buffer
    U16                 N_InputSignal;            // size of the signal input buffer
    U16                 currentIndex;             // location of the latest sample
    U32                 inputSum;                 // inputSum, only used if it is a average filter
    BOOL                isAverageFilter;          // true if all coefficient are the same
} FIR_FILTER_T;



// ********** private data **********

// [[ 0. load cell fir filter ]]

#define N_FILTER_NUM_ORDER_LOAD_CELL_WEIGHT  100                          // 100 samples of INPUT
U16 LOAD_CELL_INPUT_SIGNAL_ARRAY[ N_FILTER_NUM_ORDER_LOAD_CELL_WEIGHT ];
F32 B_COEFF_LOAD_CELL[ N_FILTER_NUM_ORDER_LOAD_CELL_WEIGHT ] = {  1.0 / ((F32) N_FILTER_NUM_ORDER_LOAD_CELL_WEIGHT) };

// [[ Uploading all filters ]]

static FIR_FILTER_T firFilters[ NUM_OF_FILTERS_IDS ] = {

                    { LOAD_CELL_INPUT_SIGNAL_ARRAY, B_COEFF_LOAD_CELL,  // 0. Load cell filter
                      N_FILTER_NUM_ORDER_LOAD_CELL_WEIGHT, 0, 0, TRUE}

                                                        };


// *** modular variable needed to process filter info
static FIR_FILTER_T *m_filter;

static void loadFilterInModularVariables(FILTER_ID_T filterID);


/*************************************************************************
 * @brief loadFilterInModularVariables
 * Loads filter parameters into modular variables to be processed by
 * modular functions.
 *
 * @param filterID - ID filter number
 *
 * @return none
 *************************************************************************/
static void loadFilterInModularVariables(FILTER_ID_T filterID)
{

    m_filter = &firFilters[filterID];

}

/*************************************************************************
 * @brief initializeFilter
 * Initializes the filter buffer with a initial value. Do this once.
 *
 * @param filterID - ID filter number
 * @param initialValue - unsigned integer 16 value
 *
 * @return none
 *************************************************************************/
void initializeFilter(FILTER_ID_T filterID, U16 initialValue)
{
    loadFilterInModularVariables(filterID);

    F32 bWeightedCoeff;
    U16 i;

    // First reset input buffer
    resetFilter(filterID, initialValue);

    // If this is an average filter,
    // then we set the coefficient values;

    if ( TRUE == m_filter->isAverageFilter)
    {
        for ( i = 0; i < m_filter->N_InputSignal; i++ )
        {
            if ( i == 0 )
            {
                bWeightedCoeff = m_filter->bCoeffs[0];
            }
            else
            {
                m_filter->bCoeffs[i] = bWeightedCoeff;
            }
        } 
    }
}

/*************************************************************************
 * @brief resetFilter
 * Reset the filter buffer with a reset value.
 *
 * @param filterID - ID filter number
 * @param resetValue - unsigned integer 16 value
 *
 * @return none
 *************************************************************************/
void resetFilter(FILTER_ID_T filterID, U16 resetValue)
{
    loadFilterInModularVariables(filterID);
    U16 i;
    m_filter->inputSum = resetValue * m_filter->N_InputSignal;

    for ( i = 0; i < m_filter->N_InputSignal; i++)
    {
        m_filter->inputSignal[i] = resetValue;
    }

    m_filter->currentIndex = m_filter->N_InputSignal - 1;
}

/*************************************************************************
 * @brief setFilterInput
 * Set a new input sample to the filter buffer.
 *
 * @param filterID - ID filter number
 * @param resetValue - unsigned integer 16 value
 *
 * @return none
 *************************************************************************/
void setFilterInput(FILTER_ID_T filterID, U16 inputSignalSample)
{
    loadFilterInModularVariables(filterID);

    U16 currentIndex = m_filter->currentIndex;
    U16 newIndex = CIRCULAR_INCREASE(currentIndex, m_filter->N_InputSignal);

    m_filter->inputSum -= m_filter->inputSignal[newIndex];
    m_filter->inputSum += inputSignalSample;

    m_filter->inputSignal[newIndex] = inputSignalSample;

    m_filter->currentIndex = newIndex;

}

/*************************************************************************
 * @brief setFilterInput
 * Set a new input sample to the filter buffer.
 *
 * @param filterID - ID filter number
 * @param resetValue - unsigned integer 16 value
 *
 * @return none
 *************************************************************************/
U16 getFilterOuput(FILTER_ID_T filterID)
{
    loadFilterInModularVariables(filterID);

    F32 output = 0;

    if( TRUE == m_filter->isAverageFilter )
    {
        output= m_filter->inputSum * m_filter->bCoeffs[0];
    }
    else // It is not a average filter, thus we need to add it up
    {
        U16 index = m_filter->currentIndex;
        U16 i;

        for ( i = 0; i < m_filter->N_InputSignal; i++) 
        {

            output += (m_filter->bCoeffs[i]*m_filter->inputSignal[index]);

            index = CIRCULAR_DECREASE(index, m_filter->N_InputSignal);

        }

    }

    return ((U16) output);
}