//FIR_filters.c
// FIR Filters Implementation
// Mark Wickert April 2015

#include "FIR_filters.h"

void FIR_init_float32(struct FIR_struct_float32 *FIR, float32_t *state, 
	                    float32_t *b_coeff, int16_t Nb) {
  // Load the filter coefficients and initialize the filter state.
  int16_t iFIR;	
  FIR->M_taps = Nb;
  FIR->b_coeff = b_coeff;
  FIR->state = state;
  for (iFIR = 0; iFIR < FIR->M_taps; iFIR++)
  {
    FIR->state[iFIR] = 0;
  }
}

void FIR_filt_float32(struct FIR_struct_float32 *FIR, float32_t *x_in, 
	                    float32_t *x_out, int16_t Nframe) {
 int16_t iframe;
 int16_t iFIR;
 float32_t accum;
 
  //Process each sample of the frame with this loop
	for (iframe = 0; iframe < Nframe; iframe++)
  {
    // Clear the accumulator/output before filtering
    accum = 0;
    // Place new input sample as first element in the filter state array
    FIR->state[0] = x_in[iframe];
    //Direct form filter each sample using a sum of products
		for (iFIR = 0; iFIR < FIR->M_taps; iFIR++)
    {
       accum += FIR->state[iFIR]*FIR->b_coeff[iFIR];
    }
    x_out[iframe] = accum;
    // Shift filter states to right or use circular buffer
    for (iFIR = FIR->M_taps-1; iFIR > 0; iFIR--)
    {
      FIR->state[iFIR] = FIR->state[iFIR-1];
    }
  }
}

void FIR_init_int16(struct FIR_struct_int16 *FIR, int16_t *state, 
	                  int16_t *b_coeff, int16_t Nb) {
  // Load the filter coefficients and initialize the filter state.
  int16_t iFIR;	
  FIR->M_taps = Nb;
	FIR->circ_idx = 0;
  FIR->b_coeff = b_coeff;
  FIR->state = state;
  for (iFIR = 0; iFIR < FIR->M_taps; iFIR++)
  {
    FIR->state[iFIR] = 0;
  }
}

void FIR_filt_int16(struct FIR_struct_int16 *FIR, int16_t *x_in, 
	                  int16_t *x_out, int16_t Nframe, int16_t scale) {
 int16_t iframe;
 int16_t iFIR;
 int32_t accum; // Accumulate sum of products in long integer
 
	for (iframe = 0; iframe < Nframe; iframe++)
  {
    // Clear the accumulator/output before filtering
    accum = 0;
    // Place new input sample as first element in the filter state array
		FIR->state[0] = x_in[iframe];
		for (iFIR = 0; iFIR < FIR->M_taps; iFIR++)
    {
			accum += (int32_t) FIR->state[iFIR]*FIR->b_coeff[iFIR];
    }
    x_out[iframe] = (int16_t)(accum>>scale); //nominally 15
    // Shift filter states to right or use circular buffer
		for (iFIR = FIR->M_taps-1; iFIR > 0; iFIR--)
    {
      FIR->state[iFIR] = FIR->state[iFIR-1];
    }
  }
}
										
void FIR_filt_int16_cb(struct FIR_struct_int16 *FIR, int16_t *x_in, 
	                  int16_t *x_out, int16_t Nframe, int16_t scale) {
 int16_t iframe;
 int16_t iFIR;
 int32_t accum; // Accumulate sum of products in long integer
 
	for (iframe = 0; iframe < Nframe; iframe++)
  {
    // Clear the accumulator/output before filtering
    accum = 0;
    // Place new input sample in the filter state array
    FIR->state[FIR->circ_idx++] = x_in[iframe];
		if (FIR->circ_idx >= FIR->M_taps) FIR->circ_idx = 0;
		for (iFIR = 0; iFIR < FIR->M_taps; iFIR++)
    {
			accum += (int32_t) FIR->state[FIR->circ_idx++]*FIR->b_coeff[iFIR];
			if (FIR->circ_idx >= FIR->M_taps) FIR->circ_idx = 0;
    }
    x_out[iframe] = (int16_t)(accum>>scale); //nominally 15
  }
}
										
void FIR_filt_int16_folded(struct FIR_struct_int16 *FIR, int16_t *x_in, 
	                  int16_t *x_out, int16_t Nframe, int16_t scale) {
 int16_t iframe;
 int16_t iFIR;
 //static int16_t circ_ptr = 0;
 //int16_t state_ptr1;
 //int16_t state_ptr2;										
 int32_t s_sum;
 int32_t accum; // Accumulate sum of products in long integer
 
  //#pragma unroll
	for (iframe = 0; iframe < Nframe; iframe++)
  {
    // Clear the accumulator/output before filtering
    accum = 0;
    // Place new input sample as first element in the filter state array
    FIR->state[0] = x_in[iframe];
		
		for (iFIR = 0; iFIR < (FIR->M_taps>>1); iFIR++)
    {
      s_sum =  FIR->state[iFIR] + FIR->state[FIR->M_taps-iFIR-1];
			accum += (int32_t) s_sum*FIR->b_coeff[iFIR];
    }
		// For an odd number of taps there is a center tap, so
		// treat this as a special case
		/*
		if (FIR->M_taps > 2*(FIR->M_taps/2))
		{
			accum += FIR->state[FIR->M_taps/2]*FIR->b_coeff[FIR->M_taps/2];
		}
		*/
    x_out[iframe] = (int16_t)(accum>>scale); //nominally 15
		// Shift filter states to right or use circular buffer
		for (iFIR = FIR->M_taps-1; iFIR > 0; iFIR--)
    {
      FIR->state[iFIR] = FIR->state[iFIR-1];
    }
  }
}
