/**@file
 * @defgroup FIR Filter for cadence calculation
 * @{
 * @ingroup nrf_ant_bicycle_power
 *
 * @brief FIR LP and BP filters for cadence calculations
 */

#include "firfilter.h"

/****************************************************************************
*
* Description:
*
*   This module provides functions to implement Finite Impulse Response (FIR)
*   filters using a variety of algorithms.  
*
*   These functions use most or all of the following input parameters:
*
*       input    - the input sample data
*       ntaps    - the number of taps in the filter   
*       h[]      - the FIR coefficient array
*       z[]      - the FIR delay line array 
*       *p_state - the "state" of the filter; initialize this to 0 before
*                  the first call
*
*   The functions fir_basic, fir_shuffle, and fir_circular are not very
*   efficient in C, and are provided here mainly to illustrate FIR 
*   algorithmic concepts.  However, the functions fir_split, fir_double_z
*   and fir_double_h are all fairly efficient ways to implement FIR filters
*   in C.
*
* Copyright 2000 by Grant R. Griffin
*
* Thanks go to contributors of comp.dsp for teaching me some of these
* techniques, and to Jim Thomas for his review and great suggestions.
*
*                          The Wide Open License (WOL)
*
* Permission to use, copy, modify, distribute and sell this software and its
* documentation for any purpose is hereby granted without fee, provided that
* the above copyright notice and this license appear in all source copies. 
* THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY OF
* ANY KIND. See http://www.dspguru.com/wol.htm for more information.
*
*****************************************************************************/


#define FIRTAPS 27

/*
OLD VERSION:
// http://t-filter.appspot.com/fir/index.html
// LP 0-3.33Hz (62.5Hz samplerate) scaled to unity at 8 bits
// Used for X-axis accelerometer
static const SAMPLE lp_filter_taps[FIRTAPS] = { 
2, 
2, 
3, 
4, 
6, 
7, 
9, 
11, 
13, 
14, 
16, 
17, 
17, 
17, 
17, 
17, 
16, 
14, 
13, 
11, 
9, 
7, 
6, 
4, 
3, 
2, 
2 
};
*/

// http://t-filter.appspot.com/fir/index.html
// 0Hz -> 4.5Hz =  4.17dB
// 7Hz -> 32Hz = -40dB
// Sample freq = 64Hz
// LP scaled to sum = 256
// Used for X-axis accelerometer
static const SAMPLE lp_filter_taps[FIRTAPS] = { 
-4,
-6,
-9,
-11,
-12,
-11,
-7,
0,
10,
22,
34,
44,
51,
54,
51,
44,
34,
22,
10,
0,
-7,
-11,
-12,
-11,
-9,
-6,
-4
};

static const SAMPLE32 lp_filter_taps32[FIRTAPS] = { 
-4,
-6,
-9,
-11,
-12,
-11,
-7,
0,
10,
22,
34,
44,
51,
54,
51,
44,
34,
22,
10,
0,
-7,
-11,
-12,
-11,
-9,
-6,
-4
};

/* OLD VERSION
// 0.41-3.33Hz (62.5Hz samplerate) scaled to unity at 8 bits 
// Used for Y-axis accelerometer
static const SAMPLE bp_filter_taps[FIRTAPS] = { 
-23, 
-14, 
-16, 
-17, 
-15, 
-12, 
-6, 
2, 
11, 
20, 
28, 
35, 
39, 
41, 
39, 
35, 
28, 
20, 
11, 
2, 
-6, 
-12, 
-15, 
-17, 
-16, 
-14, 
-23
};
*/

// http://t-filter.appspot.com/fir/index.html
// 0Hz -> 0.001Hz = -14.79dB
// 1Hz -> 4.2Hz = 7.98dB
// 7Hz -> 32Hz = -14.79dB
// Sample freq = 64Hz
// BP scaled to sum=64
// Used for Y-axis accelerometer (after subtraction of r*w^2)
static const SAMPLE bp_filter_taps[FIRTAPS] = { 
-4,
-6,
-9,
-11,
-12,
-11,
-7,
0,
10,
22,
34,
44,
51,
54,
51,
44,
34,
22,
10,
0,
-7,
-11,
-12,
-11,
-9,
-6,
-4

/*
-42,
-20,
-22,
-21,
-18,
-12,
-5,
5,
14,
23,
31,
37,
41,
42,
41,
37,
31,
23,
14,
5,
-5,
-12,
-18,
-21,
-22,
-20,
-42
*/

};

/****************************************************************************/
/* clear: zeroize a FIR delay line                                          */
/****************************************************************************/
void fir_clear(int ntaps, SAMPLE z[])      
{
    int ii;
    for (ii = 0; ii < ntaps; ii++) {
        z[ii] = 0;
    }
}


/****************************************************************************
* fir_split: This splits the calculation into two parts so the circular    
* buffer logic doesn't have to be done inside the calculation loop         
*****************************************************************************/
SAMPLE fir_split(SAMPLE input, int ntaps, const SAMPLE h[], SAMPLE z[],
                 int *p_state)
{
    int ii, end_ntaps, state = *p_state;
    // SAMPLE accum;
		int32_t accum;
    SAMPLE const *p_h;
    SAMPLE *p_z;

    /* setup the filter */
    accum = 0;
    p_h = h;

    /* calculate the end part */
    p_z = z + state;
    *p_z = input;
    end_ntaps = ntaps - state;
    for (ii = 0; ii < end_ntaps; ii++) {
        accum += *p_h++ * *p_z++;
    }

    /* calculate the beginning part */
    p_z = z;
    for (ii = 0; ii < state; ii++) {
        accum += *p_h++ * *p_z++;
    }

    /* decrement the state, wrapping if below zero */
    if (--state < 0) {
        state += ntaps;
    }
    *p_state = state;       /* return new state to caller */

		if (accum > 32767) accum = 32767;
		if (accum < -32768) accum = -32768;
		
    return accum;
}

/****************************************************************************
* fir_split: This splits the calculation into two parts so the circular    
* buffer logic doesn't have to be done inside the calculation loop         
*****************************************************************************/
SAMPLE32 fir_split32(SAMPLE32 input, int ntaps, const SAMPLE32 h[], SAMPLE32 z[],
                 int *p_state)
{
    int ii, end_ntaps, state = *p_state;
    // SAMPLE accum;
		int32_t accum;
    SAMPLE32 const *p_h;
    SAMPLE32 *p_z;

    /* setup the filter */
    accum = 0;
    p_h = h;

    /* calculate the end part */
    p_z = z + state;
    *p_z = input;
    end_ntaps = ntaps - state;
    for (ii = 0; ii < end_ntaps; ii++) {
        accum += *p_h++ * *p_z++;
    }

    /* calculate the beginning part */
    p_z = z;
    for (ii = 0; ii < state; ii++) {
        accum += *p_h++ * *p_z++;
    }

    /* decrement the state, wrapping if below zero */
    if (--state < 0) {
        state += ntaps;
    }
    *p_state = state;       /* return new state to caller */

		/*
		if (accum > 32767) accum = 32767;
		if (accum < -32768) accum = -32768;
		*/
    return accum;
}

/* Run low-pass FIR filter on one sample */
SAMPLE fir_lp_filter(SAMPLE input)
{
	static int p_state = 0;
	static SAMPLE z[FIRTAPS];

	return fir_split(input, FIRTAPS, lp_filter_taps, z, &p_state) >> 8;
}	

/* Run band-pass FIR filter on one sample */
SAMPLE fir_bp_filter(SAMPLE input)
{
	static int p_state = 0;
	static SAMPLE z[FIRTAPS];

	return fir_split(input, FIRTAPS, bp_filter_taps, z, &p_state) >> 8;
}	

/* Delay line for (FIRTAPS+1)/2 samples to align to FIR output */
SAMPLE fir_delay(SAMPLE input)
{
		static SAMPLE z[(FIRTAPS+1)/2];
		static uint32_t ptr;
    SAMPLE out;
	
		out = z[ptr]; 						// First get the delayed value
    z[ptr++] = input; 	  		// Replace with new value
    if (ptr >= (FIRTAPS+1)/2) { ptr -= (FIRTAPS+1)/2; }
    return out;
}

/* Delay line for (FIRTAPS+1)/2-1 samples to align with one sample delayed FIR output */
SAMPLE fir_delay_1(SAMPLE input)
{
		static SAMPLE z[(FIRTAPS+1)/2-1];
		static uint32_t ptr;
    SAMPLE out;
	
		out = z[ptr]; 						// First get the delayed value
    z[ptr++] = input; 	  		// Replace with new value
    if (ptr >= (FIRTAPS+1)/2-1) { ptr -= (FIRTAPS+1)/2-1; }
    return out;
}

/* Delay line for (FIRTAPS+1)/2-1 samples to align with one sample delayed FIR output */
SAMPLE fir_delay_2(SAMPLE input)
{
		static SAMPLE z[(FIRTAPS+1)/2-1];
		static uint32_t ptr;
    SAMPLE out;
	
		out = z[ptr]; 						// First get the delayed value
    z[ptr++] = input; 	  		// Replace with new value
    if (ptr >= (FIRTAPS+1)/2-1) { ptr -= (FIRTAPS+1)/2-1; }
    return out;
}

/* Run low-pass FIR filter on one sample */
SAMPLE32 fir_filter_tq(SAMPLE32 input)
{
	static int p_state = 0;
	static SAMPLE32 z[FIRTAPS];

	return fir_split32(input, FIRTAPS, lp_filter_taps32, z, &p_state) >> 8;
}	
