/***************************************************************************************/
/* FILE NAME: dsp_filter.c                     COPYRIGHT (c) Freescale 2015   */
/*                                                      All Rights Reserved  						 */
/* DESCRIPTION: Filter calling program    for LSP/SDADC Example           */
/*                                                                                             				 */
/***************************************************************************************/	
/* REV      AUTHOR        DATE        DESCRIPTION OF CHANGE 			 */
/* ---   -----------    ----------    ---------------------                                     		     */
/* 1.0	  A Turner   			April 2015   Initial Public Release        		     */
/***************************************************************************************/


#include "../headers/typedefs.h"



/*----------------------------------------------------------------------------------------------*/
/* FIR input, output vectors and coefficients */   
 
extern int16_t SDADC1_RESULTS[400]; 
extern void fir_Signed16(uint16_t N, int16_t *x, int16_t *y, int16_t *h);
extern void fir_frac16_LSP_Intrinsic(uint16_t N, int16_t *x, int16_t *y, int16_t *h);
extern void fir_frac16_LSP__trans_intrinsic(uint16_t N, int16_t *x, int16_t *y, int16_t *h);


#pragma alignvar (16)
int16_t LSP_FIR_OUT[400];
int16_t LSP_FIR_OUT_INTRI[400];
//#pragma alignvar (16)
int16_t C_FIR_OUT[400];
#pragma alignvar (16)
int16_t hr1[10/*+1*/] = { /* +1 to ensure there are 2 bytes of readable memory behind hr1 */
 2233,   2873,   3421,   3822,   4034,   4034,   3822,   3421,   2873, 2233 /*9KHz Cutoff lowpass*/
   };


#pragma ghs section text=".vle_imem"

uint16_t N, ntaps;

//fir_frac16_c(uint16_t N, uint16_t ntaps,uint16_t *x,uint16_t *y, int16_t *h)

void fir_frac16_c(uint16_t N, int16_t *x, int16_t *y, int16_t *h)
{
 int32_t n, k;
 int32_t acc;

  for (n = 0; n < N; n++)
  {
    acc = 0;
    for (k = 0; k < 10; k++)
    {
      acc += h[k] * x[n+k];
    }
    y[n] = (int16_t) (acc >> 15);
  }
}   



#pragma ghs section vletext=default

uint32_t ui32_dsp_filter_run()
{
  /********** FIR filter for frac16 data **********
     N - number of output samples

     ntaps - number of filter coefficients
     x - input array of size N+ntaps-1
     y - output array of size N
     hr - array of coefficients of length ntaps,
          coefficients are stored in reversed order */
  N = sizeof SDADC1_RESULTS/2;  // Amount of 16 bit ADC results
  //ntaps = 33;  
  
// C - code filter
 fir_frac16_c(N, SDADC1_RESULTS, C_FIR_OUT, hr1);
//  fir_frac16_c(N, SDADC1_RESULTS, C_FIR_OUT, B);

//  fir_frac16_LSP_Intrinsic(N, SDADC1_RESULTS, LSP_FIR_OUT_INTRI, hr1);

fir_frac16_LSP__trans_intrinsic (N, SDADC1_RESULTS, LSP_FIR_OUT_INTRI, hr1);

  // LSP assembly filter
  fir_Signed16(N, SDADC1_RESULTS, LSP_FIR_OUT, hr1);

  return 0;
}
