/******************************************************************************
*
* Copyright 2006-2015 Freescale Semiconductor, Inc.
* Copyright 2016-2017 NXP
*
******************************************************************************/

/****************************************************************************//*!
*
* @file     filter.c
*
* @brief    Filter implementation
*
*******************************************************************************/

/*******************************************************************************
* Includes
*******************************************************************************/
#include "filter.h"
#include "ets.h"
#include "ts_cfg_hw.h"
#include "ts_cfg_app.h"
#include "device_registers.h"
#include "main.h"

/*******************************************************************************
* Variables definition
*******************************************************************************/
// IIR1 filter coefficients
tFrac16 FilterIIR1CoeffB0[NUMBER_OF_FILTERS_USED], FilterIIR1CoeffB1[NUMBER_OF_FILTERS_USED], FilterIIR1CoeffA1[NUMBER_OF_FILTERS_USED];
// IIR1 filter buffer x(k-1), y(k-1)
tFrac32 FilterIIR1BufferX[NUMBER_OF_ELECTRODES][NUMBER_OF_FILTERS_USED], FilterIIR1BufferY[NUMBER_OF_ELECTRODES][NUMBER_OF_FILTERS_USED];

// IIR2 filter coefficients
tFrac16 FilterIIR2CoeffB0, FilterIIR2CoeffB1, FilterIIR2CoeffB2, FilterIIR2CoeffA1, FilterIIR2CoeffA2;
// IIR2 filter buffer x(k-1), x(k-2), y(k-1), y(k-2)
tFrac32 FilterIIR2BufferX[2], FilterIIR2BufferY[2];

extern uint8_t   LPFilterType[NUMBER_OF_ELECTRODES];

/****************************************************************************
* Math functions
****************************************************************************/
static inline tFrac32 MLIB_ShR_F32(register tFrac32 f32In1,register tU16 u16In2)
/*
* @violates @ref MLIB_ShR_h_REF_5 MISRA 2004 Required Rule 8.5, Object/function definition in header
* file.
*/
{
  /*
  * @violates @ref MLIB_ShR_h_REF_6 MISRA 2004 Required Rule 12.7, Bitwise operator applied to
  * signed underlying type.
  */
  /** @remarks Implements DMLIB00182, DMLIB00183 */
  return((tFrac32)(f32In1 >> u16In2));
}

static inline tFrac32 Mul_F32_C(register tFrac32 f32In1, register tFrac32 f32In2)
/*
* @violates @ref MLIB_Mul_h_REF_5 MISRA 2004 Required Rule 8.5, Object/function definition in header
* file.
*/
{
  /*
  * @violates @ref MLIB_Mul_h_REF_5 MISRA 2004 Required Rule 8.5, Object/function definition in header
  * file.
  */
  register tS32 s32UpperIn1, s32UpperIn2;
  /*
  * @violates @ref MLIB_Mul_h_REF_5 MISRA 2004 Required Rule 8.5, Object/function definition in header
  * file.
  */
  register tU32 u32LowerIn1, u32LowerIn2, u32ResultLower, u32ResultUpper;
  /*
  * @violates @ref MLIB_Mul_h_REF_5 MISRA 2004 Required Rule 8.5, Object/function definition in header
  * file.
  */
  register tU32 u32Result0, u32Result1, u32Result2;

  s32UpperIn1 = MLIB_ShR_F32(f32In1,(tU16)16U); /* upper half word of the first factor */
  /*
  * @violates @ref MLIB_Mul_h_REF_6 MISRA 2004 Required Rule 12.7, Bitwise operator applied to
  * signed underlying type (>>).
  */
  /*
  * @violates @ref MLIB_Mul_h_REF_7 MISRA 2004 Required Rule 10.3, Cast of complex expression changes signedness.
  */
  u32LowerIn1 = (tU32)(f32In1 & (tFrac32)0x0000FFFFU); /* lower half word of the first factor */
  s32UpperIn2 = MLIB_ShR_F32(f32In2,(tU16)16U); /* upper half word of the second factor */
  /*
  * @violates @ref MLIB_Mul_h_REF_6 MISRA 2004 Required Rule 12.7, Bitwise operator applied to
  * signed underlying type (>>).
  */
  /*
  * @violates @ref MLIB_Mul_h_REF_7 MISRA 2004 Required Rule 10.3, Cast of complex expression changes signedness.
  */
  u32LowerIn2 = (tU32)(f32In2 & (tFrac32)0x0000FFFFU); /* lower half word of the second factor */
  u32ResultUpper = u32LowerIn1 * u32LowerIn2;
  u32ResultLower = u32ResultUpper >> (tU16)16U;

  u32ResultUpper = ((tU32)s32UpperIn1 * u32LowerIn2) + u32ResultLower;
  u32Result2 = u32ResultUpper & (tU32)0x0000FFFFU;
  /*
  * @violates @ref MLIB_Mul_h_REF_6 MISRA 2004 Required Rule 12.7, Bitwise operator applied to
  * signed underlying type (>>).
  */
  /*
  * @violates @ref MLIB_Mul_h_REF_7 MISRA 2004 Required Rule 10.3, Cast of complex expression changes signedness.
  */
  u32Result1 = (tU32)((tS32)u32ResultUpper >> (tU16)16U);
  u32ResultUpper = (u32LowerIn1 * (tU32)s32UpperIn2) + u32Result2;
  /*
  * @violates @ref MLIB_Mul_h_REF_6 MISRA 2004 Required Rule 12.7, Bitwise operator applied to
  * signed underlying type (>>).
  */
  /*
  * @violates @ref MLIB_Mul_h_REF_7 MISRA 2004 Required Rule 10.3, Cast of complex expression changes signedness.
  */
  u32ResultLower = (tU32)((tS32)u32ResultUpper >> (tU16)16U);
  u32Result0 = (((tU32)s32UpperIn1 * (tU32)s32UpperIn2) + u32Result1) + u32ResultLower;
  u32Result0 = (u32Result0 << (tU16)1U) | ((u32ResultUpper << (tU16)16U) >> (tU16)31U);

  return((tFrac32)u32Result0);
}

/*****************************************************************************
*
* Function: void FilterIIR1Init(void)
*
* Description: Init IIR1 low pass filter
*
*****************************************************************************/
void FilterIIR1Init(void)
{
#ifdef FILTER_1
	// Load coefficients
	FilterIIR1CoeffB0[FILTER_1] = FRAC16(FILTER_1_COEF_B0);
	FilterIIR1CoeffB1[FILTER_1] = FRAC16(FILTER_1_COEF_B1);
	FilterIIR1CoeffA1[FILTER_1] = FRAC16(FILTER_1_COEF_A0);
#endif

#ifdef FILTER_2
	// Load coefficients
	FilterIIR1CoeffB0[FILTER_2] = FRAC16(FILTER_2_COEF_B0);
	FilterIIR1CoeffB1[FILTER_2] = FRAC16(FILTER_2_COEF_B1);
	FilterIIR1CoeffA1[FILTER_2] = FRAC16(FILTER_2_COEF_A0);
#endif

#ifdef FILTER_3
	// Load coefficients
	FilterIIR1CoeffB0[FILTER_3] = FRAC16(FILTER_3_COEF_B0);
	FilterIIR1CoeffB1[FILTER_3] = FRAC16(FILTER_3_COEF_B1);
	FilterIIR1CoeffA1[FILTER_3] = FRAC16(FILTER_3_COEF_A0);
#endif
}

/*****************************************************************************
*
* Function: void FilterIIR1BufferInit(uint8_t elec, tFrac32 valueBufferX, tFrac32 valueBufferY)
*
* Description: Init IIR1 low pass filter buffer
*
*****************************************************************************/
void FilterIIR1BufferInit(uint8_t elec, tFrac32 valueBufferX, tFrac32 valueBufferY)
{
	FilterIIR1BufferX[elec][LPFilterType[elec]] = valueBufferX;
	FilterIIR1BufferY[elec][LPFilterType[elec]] = valueBufferY;
}

/*****************************************************************************
*
* Function: tFrac16 FilterIIR1(uint8_t elec, tFrac16 x_k)
*
* Description: IIR1 filter implementation
*              y(k) = b0*x(k) + b1*x(k-1) - a1*y(k-1)
*
*****************************************************************************/
tFrac16 FilterIIR1(uint8_t elec, tFrac16 x_k)
{
	register tFrac32 M1;
	register tFrac32 M2;
	register tFrac32 M3;
	register tFrac32 M4;
	register tFrac32 Acc;
	register tFrac16 y_k;
	register tFrac32 satMin, satMax;


	// M1 = b0 * x(k)
	M1 = (tFrac32)(((int32_t)x_k * (int32_t)FilterIIR1CoeffB0[LPFilterType[elec]]) << 1);

	// M2 = b1 * x(k-1)
	M2 = (tFrac32)(((int32_t)FilterIIR1BufferX[elec][LPFilterType[elec]] * (int32_t)FilterIIR1CoeffB1[LPFilterType[elec]]) << 1);

	// Add and saturate: Acc  = M1 + M2
	Acc = M1 + M2;
	satMax = ~(M1 | M2) & Acc;
	satMin = (M1 & M2) & ~Acc;
	Acc = (satMin < 0) ? INT32_MIN : Acc;
	Acc = (satMax < 0) ? INT32_MAX : Acc;

	// M3 = Acc
	M3 = Acc;

	// M4 = a1 * y(k-1)
	M4 = Mul_F32_C(FilterIIR1BufferY[elec][LPFilterType[elec]],((tFrac32)FilterIIR1CoeffA1[LPFilterType[elec]]) << 16);

	// Sub and saturate: Acc  = Acc - M3
	Acc = M3 - M4;
	satMax = (~M3 | M4) & Acc;
	satMin = (M3 & ~M4) & ~Acc;
	Acc = (satMin < 0) ? INT32_MIN : Acc;
	Acc = (satMax < 0) ? INT32_MAX : Acc;

	// Convert to tFrac16
	y_k = (tFrac16)(Acc >> 16);

	// IIR1 filter buffer x(k-1), y(k-1)
	// x(k-1)
	FilterIIR1BufferX[elec][LPFilterType[elec]] = (tFrac32)x_k;
	// y(k-1)
	FilterIIR1BufferY[elec][LPFilterType[elec]] = Acc;

	return(y_k);
}

/*****************************************************************************
*
* Function: tFrac16 FilterIIR2(tFrac16 x_k)
*
* Description: IIR2 filter implementation
*              y(k) = b0*x(k) + b1*x(k-1) + b2*x(k-2) - a1*y(k-1) - a2*y(k-2)
*
*****************************************************************************/
tFrac16 FilterIIR2(tFrac16 x_k)
{
	register tFrac32 M1;
	register tFrac32 M2;
	register tFrac32 M3;
	register tFrac32 M4;
	register tFrac32 M5;
	register tFrac32 Acc;
	register tFrac16 y_k;
	register tFrac32 satMin, satMax;


	// M1 = b0 * x(k)
	M1 = (tFrac32)(((int32_t)x_k * (int32_t)FilterIIR2CoeffB0) << 1);

	// M2 = b1 * x(k-1)
	M2 = (tFrac32)(((int32_t)FilterIIR2BufferX[0] * (int32_t)FilterIIR2CoeffB1) << 1);

	// M3 = b2 * x(k-2)
	M3 = (tFrac32)(((int32_t)FilterIIR2BufferX[1] * (int32_t)FilterIIR2CoeffB2) << 1);

	// Add and saturate: Acc  = M1 + M2
	Acc = M1 + M2;
	satMax = ~(M1 | M2) & Acc;
	satMin = (M1 & M2) & ~Acc;
	Acc = (satMin < 0) ? INT32_MIN : Acc;
	Acc = (satMax < 0) ? INT32_MAX : Acc;

	// M4 = Acc
	M4 = Acc;
	// Add and saturate: Acc  = M3 + M4
	Acc = M3 + M4;
	satMax = ~(M3 | M4) & Acc;
	satMin = (M3 & M4) & ~Acc;
	Acc = (satMin < 0) ? INT32_MIN : Acc;
	Acc = (satMax < 0) ? INT32_MAX : Acc;

	// M4 = Acc
	M4 = Acc;
	// M5 = a1 * y(k-1)
	M5 = (tFrac32)Mul_F32_C(FilterIIR2BufferY[0],((tFrac32)FilterIIR2CoeffA1) << 16);
	//  Sub and saturate: Acc  = Acc - M5
	Acc = M4 - M5;
	satMax = (~M4 | M5) & Acc;
	satMin = (M4 & ~M5) & ~Acc;
	Acc = (satMin < 0) ? INT32_MIN : Acc;
	Acc = (satMax < 0) ? INT32_MAX : Acc;

	// M4 = Acc
	M4 = Acc;
	// M5 = a2 * y(k-2)
	M5 = (tFrac32)Mul_F32_C(FilterIIR2BufferY[1],((tFrac32)FilterIIR2CoeffA2) << 16);
	//  Sub and saturate: Acc  = Acc - M5
	Acc = M4 - M5;
	satMax = (~M4 | M5) & Acc;
	satMin = (M4 & ~M5) & ~Acc;
	Acc = (satMin < 0) ? INT32_MIN : Acc;
	Acc = (satMax < 0) ? INT32_MAX : Acc;

	// y(k) = Acc * 2; correction due to scaled filter coefficients
	if ((INT32_MAX >> 1) < Acc)
	{
		Acc = INT32_MAX;
	}
	else if ((INT32_MIN >> 1) > Acc)
	{
		Acc = INT32_MIN;
	}
	else
	{
		Acc = Acc << 1;
	}

	// Convert to tFrac16
	y_k = (tFrac16)(Acc >> 16);

	// IIR2 filter buffer x(k-1), x(k-2), y(k-1), y(k-2)
	// x(k-2)
	FilterIIR2BufferX[1] = (tFrac32)FilterIIR2BufferX[0];
	// x(k-1)
	FilterIIR2BufferX[0] = (tFrac32)x_k;
	// y(k-2)
	FilterIIR2BufferY[1] = FilterIIR2BufferY[0];
	// y(k-1)
	FilterIIR2BufferY[0] = Acc;

	return(y_k);
}
