/************************************************************************************

   p r o c e s s i n g . c

 ************************************************************************************

 Source file for the high priority signal processing functions

 For sample mode: -------------------------------------------------------------------

 You can define several alternate processing functions
 All should have the prototype:

     functionName(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
     Where in1 and in2 are channel 1 and 2 inputs in Q15 format
     And out1, out2 are pointers to channel 1 and 2 outputs in Q15 format

 Only one function will be active after compilation, this be the one selected
 in the PROC_FUNCTION #define in the processing.h file

 Some processing functions are given as examples in this file:

     identity   : Copies input to output
     changeSign : Change sign of every other sample
     firFilter  : Implements a FIR filter

 For block mode: --------------------------------------------------------------------

 You can define several alternate processing functions
 All should have the prototype:

     functionName(q15_t *inp1,q15_t *inp2,q15_t *outp1,q15_t *outp2)
     Where inp1 and inp2 are channel 1 and 2 input buffers in Q15 format
     And outp1, outp2 are channels 1 and 2 output buffers in Q15 format
     The processing function shall process HBLOCK_SIZE samples from those buffers

 Some processing functions are given as examples in this file:

    blockIdentity   : Copies input to output

 ***********************************************************************************/

#include "arm_math.h"       // Include for the DSP lib
#include "base.h"           // Basic header
#include "configuration.h"  // Firmware configuration
#include "dsp_kernel.h"     // DSP Kernel header
#include "chprintf.h"       // Serial printf

// Data for box filter
#define BOX_TAP 40

// Data for board test box filter
#define TEST_TAP 30

// 16bit fir coefficients
// http://t-filter.engineerjs.com/
// Sample frequency: 10kHz
//   5dB ripple from  0Hz to 2kHz
// -40dB        from 3kHz to 5kHz
//  13 Tap
/*
#define FIR_TAP 13
const q15_t fir[]={660,470,-1980,-3830,504,
		         10027,15214,10027,504,-3830,
		         -1980,470,660};

// 16bit fir coefficients
// http://t-filter.engineerjs.com/
// Sample frequency: 20kHz
//   5dB ripple from    0Hz to 400Hz
// -40dB        from 1600Hz to 10kHz
//  41 Tap
#define FIR_TAP 41
const q15_t fir[]={-49,-67,-102,-139,-172,-191,-187,-148,-64,72,
                  265,516,816,1156,1518,1882,2223,2519,2747,2892,
                  2942,2892,2747,2519,2223,1882,1518,1156,816,516,
                  265,72,-64,-148,-187,-191,-172,-139,-102,-67,
				  -49};
*/



/**********************************************************************************

 SAMPLE TO SAMPLE GIVEN FUNCTIONS

 **********************************************************************************/

// Identity function
// Just sets the outputs equal to the inputs
void identity(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;

 (*out1) = in1-offset1;
 (*out2) = in2-offset2;

 // Set offset
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

/**********************************************************************************

 SAMPLE TO SAMPLE PROCESSING SOLUTIONS
 P2 : Introduction to Software

 **********************************************************************************/

// Modulation frequency
#define MOD_FREQ 1000
// Angle increase in one step
#define ANGLE_STEP (Q15_ONE*MOD_FREQ/SAMPLE_FREQ)
// Modulates the signal by multiplying by a sine
void p2modulation(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t angle = 0;
 static q15_t offset1=0, offset2=0;
 q15_t sineValue;

 // Compute sine
 sineValue = arm_sin_q15(angle);

 // Compute outputs
 (*out1) = (in1-offset1)*sineValue/Q15_ONE;
 (*out2) = (in2-offset2)*sineValue/Q15_ONE;

 // Update angle
 angle = (Q15_ONE+angle+ANGLE_STEP)%Q15_ONE;

 // Set offset
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

// Modulates the signal by Nyquist Frequency
void p2modulationFn(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;
 static int32_t state=0;

 // Compute outputs
 if (state)
     {
	 (*out1) = (in1-offset1);
	 (*out2) = (in2-offset2);
	 state = 0;
     }
    else
    {
    (*out1) = -(in1-offset1);
    (*out2) = -(in2-offset2);
    state = 1;
    }
 // Set offset
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }



// 16bit fir coefficients
// Banda de pas: fp=3.6kHz, atenuaci inferior a ap=1dB
// Banda atenuada: fa=4kHz, atenuaci superior a aa=40dB
// Freqncia de mostratge: fm=20kHz
// Coefficients order: a0,a1,a2,...

#define FIR_TAP 74
const q15_t fir[74] = {
-192,62,305,454,325,9,-188,-78,177,237
,-5,-269,-205,143,350,118,-307,-385,42,482
,343,-283,-637,-189,605,731,-126,-1012,-703,699
,1548,422,-1871,-2495,750,6799,11639,11639,6799,750
,-2495,-1871,422,1548,699,-703,-1012,-126,731,605
,-189,-637,-283,343,482,42,-385,-307,118,350
,143,-205,-269,-5,237,177,-78,-188,9,325
,454,305,62,-192};


// Implements a fir filter on both channels
void p2firFilter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;
 static q15_t buffer1[FIR_TAP];
 static q15_t buffer2[FIR_TAP];
 static int32_t pos = 0;

 int i,value1 = 0,value2 = 0;

 // Add samples to buffers
 buffer1[pos] = in1-offset1;
 buffer2[pos] = in2-offset2;

 // Calculate fir
 for(i=0;i<FIR_TAP;i++)
     {
	 value1 += buffer1[(FIR_TAP+pos-i)%FIR_TAP]*fir[i];
	 value2 += buffer2[(FIR_TAP+pos-i)%FIR_TAP]*fir[i];
     }

 // Scale output
 (*out1) = value1/32768;
 (*out2) = value2/32768;

 // Update position in buffer
 pos = (pos+1)%FIR_TAP;

 // Set offset when pushing SW1
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

/*** IIR Filter *******************************************************************/

// IIR coefficients
// Banda de pas: fp=3.6kHz, atenuaci inferior a ap=1dB
// Banda atenuada: fa=4kHz, atenuaci superior a aa=40dB
// Freqncia de mostratge: fm=20kHz
// Coefficients order: a0,a1,a2,...
// Note that filter is normalized

// Number of taps for the filter
#define IIR_TAP 7

// Q15 filter coefficients
const q15_t iirNumQ15[7] = {
1256,101,2623,550,2623,101,1256};
const q15_t iirDenQ15[7] = {
4096,-12098,20880,-22070,15545,-6685,1462};
const q15_t denScale = 8; // Den is scaled to fit Q15

// Int32 filter coefficients scaled to Q15_ONE
const int32_t iirNum[7] = {
1256,101,2623,550,2623,101,1256};
const int32_t iirDen[7] = {
32768,-96791,167045,-176562,124362,-53484,11698};


// Float32 filter coefficients
const float32_t iirFloatNum[7] = {
0.03835866,0.00310906,0.08006535,0.01679898,0.08006535,0.00310906,0.03835866};
const float32_t iirFloatDen[7] = {
1.0,-2.95385114,5.0978097,-5.38826857,3.79524319,-1.63221545,0.35700157};

// Float64 filter coefficients
const float64_t iirDoubleNum[7] = {
0.03835866,0.00310906,0.08006535,0.01679898,0.08006535,0.00310906,0.03835866};
const float64_t iirDoubleDen[7] = {
1.0,-2.95385114,5.0978097,-5.38826857,3.79524319,-1.63221545,0.35700157};

// Implements a fir filter on both channels using Q15 calculations
void p2iirQ15Filter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;
 static q15_t bufferIn1[IIR_TAP];
 static q15_t bufferIn2[IIR_TAP];
 static q15_t bufferOut1[IIR_TAP];
 static q15_t bufferOut2[IIR_TAP];

 static int32_t pos = 0;
 int i;
 int32_t value1 = 0.0, value2 = 0.0;

 // Add input samples to buffers
 bufferIn1[pos] = in1-offset1;
 bufferIn2[pos] = in2-offset1;

 // Calculate Num
 for(i=0;i<IIR_TAP;i++)
     {
	 value1 += bufferIn1[(IIR_TAP+pos-i)%IIR_TAP]*iirNum[i]/Q15_ONE;
	 value2 += bufferIn2[(IIR_TAP+pos-i)%IIR_TAP]*iirNum[i]/Q15_ONE;
     }
 // Calculate Den
 for(i=1;i<IIR_TAP;i++)
     {
	 value1 -= (bufferOut1[(IIR_TAP+pos-i)%IIR_TAP]*iirDen[i])/Q15_ONE;
	 value2 -= (bufferOut2[(IIR_TAP+pos-i)%IIR_TAP]*iirDen[i])/Q15_ONE;
     }

 // Store output value
 bufferOut1[pos] = value1;
 bufferOut2[pos] = value2;

 // Scale on output is not needed
 (*out1) = value1;
 (*out2) = value2;

 // Update position in buffer
 pos = (pos+1)%IIR_TAP;

 // Set offset when pushing SW1
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

// Implements a fir filter on both channels using Q15 calculations
// This version uses only Q15 coefficients
void p2iirQ15PureFilter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;
 static q15_t bufferIn1[IIR_TAP];
 static q15_t bufferIn2[IIR_TAP];
 static q15_t bufferOut1[IIR_TAP];
 static q15_t bufferOut2[IIR_TAP];

 static int32_t pos = 0;
 int i;
 int32_t value1 = 0.0, value2 = 0.0;

 // Add input samples to buffers
 bufferIn1[pos] = in1-offset1;
 bufferIn2[pos] = in2-offset1;

 // Calculate Num
 for(i=0;i<IIR_TAP;i++)
     {
	 value1 += bufferIn1[(IIR_TAP+pos-i)%IIR_TAP]*iirNumQ15[i]/Q15_ONE;
	 value2 += bufferIn2[(IIR_TAP+pos-i)%IIR_TAP]*iirNumQ15[i]/Q15_ONE;
     }
 // Calculate Den
 for(i=1;i<IIR_TAP;i++)
     {
	 value1 -= (bufferOut1[(IIR_TAP+pos-i)%IIR_TAP]*denScale*iirDenQ15[i])/Q15_ONE;
	 value2 -= (bufferOut2[(IIR_TAP+pos-i)%IIR_TAP]*denScale*iirDenQ15[i])/Q15_ONE;
     }

 // Store output value
 bufferOut1[pos] = value1;
 bufferOut2[pos] = value2;

 // Scale on output is not needed
 (*out1) = value1;
 (*out2) = value2;

 // Update position in buffer
 pos = (pos+1)%IIR_TAP;

 // Set offset when pushing SW1
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

// Implements a fir filter on both channels using int32_t calculations
void p2iirInt32Filter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;
 static q15_t bufferIn1[IIR_TAP];
 static q15_t bufferIn2[IIR_TAP];
 static int32_t bufferOut1[IIR_TAP];
 static int32_t bufferOut2[IIR_TAP];

 static int32_t pos = 0;
 int i;
 int32_t value1 = 0.0, value2 = 0.0;

 // Add input samples to buffers
 bufferIn1[pos] = in1-offset1;
 bufferIn2[pos] = in2-offset1;

 // Calculate Num
 for(i=0;i<IIR_TAP;i++)
     {
	 value1 += bufferIn1[(IIR_TAP+pos-i)%IIR_TAP]*iirNum[i];
	 value2 += bufferIn2[(IIR_TAP+pos-i)%IIR_TAP]*iirNum[i];
     }
 // Calculate Den
 for(i=1;i<IIR_TAP;i++)
     {
	 value1 -= (((int64_t)bufferOut1[(IIR_TAP+pos-i)%IIR_TAP])*iirDen[i])/Q15_ONE;
	 value2 -= (((int64_t)bufferOut2[(IIR_TAP+pos-i)%IIR_TAP])*iirDen[i])/Q15_ONE;
     }

 // Store output value
 bufferOut1[pos] = value1;
 bufferOut2[pos] = value2;

 // Scale output
 (*out1) = (q15_t)(value1/Q15_ONE);
 (*out2) = (q15_t)(value2/Q15_ONE);

 // Update position in buffer
 pos = (pos+1)%IIR_TAP;

 // Set offset when pushing SW1
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

// Implements a fir filter on both channels using float32_t calculations
void p2iirFloatFilter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;
 static float32_t bufferIn1[IIR_TAP];
 static float32_t bufferIn2[IIR_TAP];
 static float32_t bufferOut1[IIR_TAP];
 static float32_t bufferOut2[IIR_TAP];

 static int32_t pos = 0;
 int i;
 float32_t value1 = 0.0, value2 = 0.0;

 // Add input samples to buffers
 bufferIn1[pos] = ((float32_t)(in1-offset1))/Q15_ONE;
 bufferIn2[pos] = ((float32_t)(in2-offset2))/Q15_ONE;

 // Calculate Num
 for(i=0;i<IIR_TAP;i++)
     {
	 value1 += bufferIn1[(IIR_TAP+pos-i)%IIR_TAP]*iirFloatNum[i];
	 value2 += bufferIn2[(IIR_TAP+pos-i)%IIR_TAP]*iirFloatNum[i];
     }
 // Calculate Den
 for(i=1;i<IIR_TAP;i++)
     {
	 value1 -= bufferOut1[(IIR_TAP+pos-i)%IIR_TAP]*iirFloatDen[i];
	 value2 -= bufferOut2[(IIR_TAP+pos-i)%IIR_TAP]*iirFloatDen[i];
     }

 // Store output value
 bufferOut1[pos] = value1;
 bufferOut2[pos] = value2;

 // Scale output
 (*out1) = (q15_t)(value1*Q15_MAX);
 (*out2) = (q15_t)(value2*Q15_MAX);

 // Update position in buffer
 pos = (pos+1)%IIR_TAP;

 // Set offset when pushing SW1
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

// Implements a fir filter on both channels using float64_t calculations
void p2iirDoubleFilter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t offset1=0, offset2=0;
 static float64_t bufferIn1[IIR_TAP];
 static float64_t bufferIn2[IIR_TAP];
 static float64_t bufferOut1[IIR_TAP];
 static float64_t bufferOut2[IIR_TAP];

 static int32_t pos = 0;
 int i;
 float32_t value1 = 0.0, value2 = 0.0;

 // Add input samples to buffers
 bufferIn1[pos] = ((float64_t)(in1-offset1))/Q15_ONE;
 bufferIn2[pos] = ((float64_t)(in2-offset2))/Q15_ONE;

 // Calculate Num
 for(i=0;i<IIR_TAP;i++)
     {
	 value1 += bufferIn1[(IIR_TAP+pos-i)%IIR_TAP]*iirDoubleNum[i];
	 value2 += bufferIn2[(IIR_TAP+pos-i)%IIR_TAP]*iirDoubleNum[i];
     }
 // Calculate Den
 for(i=1;i<IIR_TAP;i++)
     {
	 value1 -= bufferOut1[(IIR_TAP+pos-i)%IIR_TAP]*iirDoubleDen[i];
	 value2 -= bufferOut2[(IIR_TAP+pos-i)%IIR_TAP]*iirDoubleDen[i];
     }

 // Store output value
 bufferOut1[pos] = value1;
 bufferOut2[pos] = value2;

 // Scale output
 (*out1) = (q15_t)(value1*Q15_MAX);
 (*out2) = (q15_t)(value2*Q15_MAX);

 // Update position in buffer
 pos = (pos+1)%IIR_TAP;

 // Set offset when pushing SW1
 if (SW1_ACTIVE)
    {
	offset1 = in1;
	offset2 = in2;
    }
 }

/**********************************************************************************

 OTHER EXAMPLE SAMPLE TO SAMPLE PROCESSING FUNCTIONS

 **********************************************************************************/

// Change sign function
// Changes the sign of every other sample
void changeSign(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static int32_t sign=0;

 sign = (sign+1)%2;

 if (sign)
      {
	  (*out1) = in1;
	  (*out2) = in2;
      }
     else
      {
      (*out1) = -in1;
      (*out2) = -in2;
      }
 }


// Implements a box filter on both channels
void boxFilter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t buffer1[BOX_TAP];
 static q15_t buffer2[BOX_TAP];
 static int32_t pos = 0;
 static int32_t value1 = 0,value2 = 0;

 // Remove samples from buffer and add new samples
 value1 = value1 - buffer1[pos] + in1;
 value2 = value2 - buffer2[pos] + in2;

 // Add samples to buffers
 buffer1[pos] = in1;
 buffer2[pos] = in2;

 // Scale output
 (*out1) = value1/BOX_TAP;
 (*out2) = value2/BOX_TAP;

 // Update position in buffer
 pos = (pos+1)%BOX_TAP;
 }

// Implements a box filter on both channels
// Bad version of the filter
void lameBoxFilter(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t buffer1[BOX_TAP];
 static q15_t buffer2[BOX_TAP];
 static int32_t pos = 0;
 int32_t i,value1 = 0,value2 = 0;

 // Add samples to buffers
 buffer1[pos] = in1;
 buffer2[pos] = in2;

 // Calculate addition
 for(i=0;i<BOX_TAP;i++)
     {
	 value1 += buffer1[i];
	 value2 += buffer2[i];
     }

 // Scale output
 (*out1) = value1/BOX_TAP;
 (*out2) = value2/BOX_TAP;

 // Update position in buffer
 pos = (pos+1)%BOX_TAP;
 }

// Length for ECHO processing
#define ECHO_LEN 6000 // 300ms

// Implements echo addition at -6dB
void echo(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t buffer1[ECHO_LEN];
 static q15_t buffer2[ECHO_LEN];
 static int32_t pos = 0;

 // Compute current sample
 (*out1) = in1 + buffer1[pos]/2;
 (*out2) = in2 + buffer2[pos]/2;

 // Add samples to buffers
 buffer1[pos] = (*out1);
 buffer2[pos] = (*out2);

 // Update position
 pos = (pos+1)%ECHO_LEN;
 }

extern volatile int32_t echoLen;
extern volatile int32_t echoMag;

// Implements echo addition with configuration delay and attenuation
// Must be used with echoLoop
void echoPot(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t buffer1[ECHO_LEN];
 static q15_t buffer2[ECHO_LEN];
 static int32_t pos = 0;

 // Compute current sample
 (*out1) = in1 + (echoMag*buffer1[pos])/256;
 (*out2) = in2 + (echoMag*buffer2[pos])/256;

 // Add samples to buffers
 buffer1[pos] = (*out1);
 buffer2[pos] = (*out2);

 // Update position
 pos = (pos+1)%echoLen;
 }


/**********************************************************************************

 BOARD TEST PROCESSING FUNCTION

 This is the function implemented in the boadTest firmware
 It operates in sample mode

 By default copies the input to the output

 If SW1 is active, a square wave is added to the output
 If SW2 is active, a triangle wave is added to the output
 IF SW3 is active, input is not copied to the output
 If SW4 is active, samples are sign changed at every other sample

 **********************************************************************************/

extern q15_t isquare;
extern q15_t isine;

void boardTest(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static int32_t cnt1 = 0;
 q15_t square;
 static int32_t tinc = 10;
 static q15_t triangular = 0;
 static q15_t buffer1[TEST_TAP];
 static q15_t buffer2[TEST_TAP];
 static int32_t boxPos = 0;
 static int32_t value1 = 0,value2 = 0;
 q15_t sine;
 static int32_t tsine = 0;

 q15_t add1,add2;

 /* Check Saturation on channel 1 */
 if ((in1 < SAT_LOW)||(in1 > SAT_HIGH))
	 PRO4_SET;
	 else
	 PRO4_CLEAR;

 /* 1kHz square signal */
 if (cnt1 < (1<<14))
	 square = 1600;
     else
     square = -1600;
 cnt1 = (cnt1+isquare)%(1<<15);

 /* 500Hz triangular signal  */
 triangular += tinc;
 if (triangular >=  1600) tinc = -160;
 if (triangular <= -1600) tinc =  160;

 /* 500Hz sine signal */
 sine = arm_sin_q15(tsine)/20;
 tsine = (tsine+isine)%(1<<15);

 /* Button check for adding signals */
 add1 = in1;
 add2 = in2;
 if (SW1_ACTIVE)
     {
	 add1+=square;
	 add2+=square;
     }
 if (SW2_ACTIVE)
	 {
	 //add1+=triangular;
	 //add2+=triangular;
	 add1+=sine;
	 add2+=sine;
	 }

 /* Box filter if SW3 is active */
 if (SW3_ACTIVE)
     {
	 // Remove samples from buffer and add new samples
	 value1 = value1 - buffer1[boxPos] + add1;
	 value2 = value2 - buffer2[boxPos] + add2;

	 // Add samples to buffers
	 buffer1[boxPos] = add1;
	 buffer2[boxPos] = add2;

	 // Scale output
	 (*out1) = value1/TEST_TAP;
	 (*out2) = value2/TEST_TAP;

	 // Update position in buffer
	 boxPos = (boxPos+1)%TEST_TAP;
     }
     else
     {
     (*out1) = add1;
     (*out2) = add2;
     }

 /* Mute Input if SW4 is pushed*/
 if (SW4_ACTIVE)
	 {
	 (*out1) = 0;
	 (*out2) = 0;
	 }
 }

/**********************************************************************************

 BLOCK GIVEN FUNCTIONS

 **********************************************************************************/

// This block processing example just copies the input to the output
//
// Global variables used:
//    inbuff1,inbuff2   : Input buffers
//    outbuff1,outbuff2 : Output buffers
//    blkStart          : Start position in buffer (can be 0 or HBLOCK_SIZE)
//
// The function shall fill the output buffers
// from blkStart to blkStart+HBLOCK_SIZE-1
//
// SW1 nulls the input offset error
//
void blockIdentity(void)
 {
 static q15_t offset1=0, offset2=0;
 int i;

 // For all samples in the block
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 outbuff1[blkStart+i] = inbuff1[blkStart+i]-offset1; //Copy In1 -> Out1
	 outbuff2[blkStart+i] = inbuff2[blkStart+i]-offset2; //Copy In2 -> Out2
     }

 // Set offset when SW1 is pushed
 if (SW1_ACTIVE)
    {
	offset1 = inbuff1[blkStart];
	offset2 = inbuff2[blkStart];
    }
 }

/**********************************************************************************

 BLOCK PROCESSING SOLUTIONS
 P2 : Introduction to Software

 **********************************************************************************/

/** FIR FILTER ********************************************************************/

static inline q15_t firSample(volatile q15_t *buff,uint32_t pos,uint32_t offset)
 {
 int32_t i,loc,value=0;

 // Calculate fir
 for(i=0;i<FIR_TAP;i++)
     {
	 loc = (2*HBLOCK_SIZE+pos-i+1)%(2*HBLOCK_SIZE);
	 value = value + (buff[loc]-offset)*(fir[i]);
	 };

 // Scale output
 value = value/Q15_ONE;

 // Return value
 return (q15_t)value;
 }

// Block mode FIR filter
// Uses the same coefficients as the sample to sample one
void p2blockFir(void)
 {
 static q15_t offset1=0, offset2=0;
 int i;

 // For all samples in the block
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 outbuff1[blkStart+i] = firSample(inbuff1,blkStart+i,offset1);
	 outbuff2[blkStart+i] = firSample(inbuff2,blkStart+i,offset2);
     }

 // Set offset when SW1 is pushed
 if (SW1_ACTIVE)
    {
	offset1 = inbuff1[blkStart];
	offset2 = inbuff2[blkStart];
    }
 }

/** IIR FILTER ********************************************************************/

// Implements a iir filter on both channels in block mode

static inline q15_t iirSample(volatile q15_t *buff_in,volatile q15_t *buff_out,uint32_t pos,uint32_t offset)
 {
 int32_t i,loc,value=0;

 // Calculate IIR Num
 for(i=0;i<IIR_TAP;i++)
     {
	 loc = (2*HBLOCK_SIZE+pos-i)%(2*HBLOCK_SIZE);
	 value = value + (buff_in[loc]-offset)*(iirNum[i])/Q15_ONE;
     }

 // Calculate IIR Den
 for(i=1;i<IIR_TAP;i++)
     {
	 loc = (2*HBLOCK_SIZE+pos-i)%(2*HBLOCK_SIZE);
	 value = value - buff_out[loc]*(iirDen[i])/Q15_ONE;
	 };

 // Return value
 return (q15_t)value;
 }

// Block mode IIR filter
// Uses the same coefficients as the sample to sample one
void p2blockIIR(void)
 {
 static q15_t offset1=0, offset2=0;
 int i;

 // For all samples in the block
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 outbuff1[blkStart+i] = iirSample(inbuff1,outbuff1,blkStart+i,offset1);
	 outbuff2[blkStart+i] = iirSample(inbuff2,outbuff2,blkStart+i,offset2);
     }

 // Set offset when SW1 is pushed
 if (SW1_ACTIVE)
    {
	offset1 = inbuff1[blkStart];
	offset2 = inbuff2[blkStart];
    }
 }

/**********************************************************************************

 BLOCK PROCESSING SOLUTIONS
 P3 : Detection

 **********************************************************************************/


// Chirp #1
const q15_t ChirpPulse1[167] = {
0,442,655,595,251,-354,-1181,-2169,-3253,-4365
,-5441,-6425,-7271,-7948,-8435,-8723,-8814,-8717,-8449,-8033
,-7492,-6854,-6146,-5392,-4619,-3850,-3105,-2405,-1766,-1204
,-735,-370,-124,-7,-32,-210,-551,-1066,-1764,-2653
,-3740,-5027,-6514,-8193,-10050,-12066,-14206,-16429,-18678,-20882
,-22958,-24807,-26321,-27381,-27863,-27648,-26624,-24702,-21822,-17968
,-13183,-7570,-1313,5331,12029,18382,23945,28260,30891,31469
,29747,25646,19296,11067,1567,-8376,-17778,-25590,-30815,-32657
,-30652,-24790,-15588,-4093,8042,18860,27028,31686,32495,29612
,23605,15323,5765,-4052,-13206,-20946,-26738,-30283,-31508,-30530
,-27617,-23143,-17534,-11231,-4654,1825,7899,13332,17961,21694
,24499,26395,27440,27722,27345,26424,25072,23400,21512,19498
,17437,15394,13424,11566,9852,8303,6933,5748,4751,3940
,3310,2854,2562,2426,2434,2576,2837,3207,3670,4211
,4815,5462,6133,6805,7457,8062,8594,9028,9337,9498
,9490,9298,8912,8332,7567,6640,5583,4440,3268,2128
,1087,212,-441,-832,-938,-769,-364};
const q15_t ChirpPulseSize1 = 167;


// Chirp #2
const q15_t ChirpPulse2[250] = {
0,-366,-811,-1225,-1509,-1585,-1407,-964,-276,610
,1631,2708,3765,4726,5530,6127,6485,6585,6426,6020
,5389,4561,3572,2458,1256,0,-1278,-2552,-3798,-4999
,-6140,-7213,-8211,-9133,-9979,-10750,-11448,-12078,-12643,-13146
,-13587,-13969,-14288,-14541,-14723,-14824,-14833,-14735,-14514,-14149
,-13619,-12901,-11970,-10805,-9383,-7689,-5710,-3445,-904,1888
,4893,8047,11269,14451,17467,20168,22390,23962,24715,24492
,23171,20674,16990,12188,6436,0,-6750,-13355,-19293,-24016
,-27000,-27811,-26165,-21992,-15489,-7141,2286,11792,20237,26474
,29517,28710,23880,15447,4440,-7576,-18703,-26992,-30803,-29159
,-22036,-10487,3427,16958,27187,31675,29091,19667,5327,-10610
,-24144,-31595,-30660,-21209,-5559,11967,26171,32515,28662,15452
,-3077,-20797,-31467,-30998,-19158,0,19158,30998,31467,20797
,3077,-15452,-28662,-32515,-26171,-11967,5559,21209,30660,31595
,24144,10610,-5327,-19667,-29091,-31675,-27187,-16958,-3427,10487
,22036,29159,30803,26992,18703,7576,-4440,-15447,-23880,-28710
,-29517,-26474,-20237,-11792,-2286,7141,15489,21992,26165,27811
,27000,24016,19293,13355,6750,0,-6436,-12188,-16990,-20674
,-23171,-24492,-24715,-23962,-22390,-20168,-17467,-14451,-11269,-8047
,-4893,-1888,904,3445,5710,7689,9383,10805,11970,12901
,13619,14149,14514,14735,14833,14824,14723,14541,14288,13969
,13587,13146,12643,12078,11448,10750,9979,9133,8211,7213
,6140,4999,3798,2552,1278,0,-1256,-2458,-3572,-4561
,-5389,-6020,-6426,-6585,-6485,-6127,-5530,-4726,-3765,-2708
,-1631,-610,276,964,1407,1585,1509,1225,811,366
};
const q15_t ChirpPulseSize2 = 250;


const q15_t SinePulse[134] = {
0,1543,3083,4617,6140,7649,9141,10614,12062,13484
,14876,16235,17557,18841,20083,21281,22431,23531,24579,25573
,26509,27387,28204,28959,29649,30273,30830,31319,31738,32087
,32364,32570,32703,32763,32751,32666,32509,32280,31978,31606
,31164,30652,30072,29426,28714,27939,27101,26204,25248,24236
,23170,22053,20887,19674,18418,17121,15786,14415,13013,11582
,10125,8646,7148,5633,4106,2570,1029,-514,-2057,-3595
,-5126,-6644,-8149,-9635,-11099,-12539,-13951,-15333,-16680,-17990
,-19260,-20487,-21669,-22803,-23886,-24916,-25891,-26809,-27666,-28463
,-29196,-29864,-30466,-31001,-31466,-31862,-32187,-32441,-32622,-32731
,-32768,-32731,-32622,-32441,-32187,-31862,-31466,-31001,-30466,-29864
,-29196,-28463,-27666,-26809,-25891,-24916,-23886,-22803,-21669,-20487
,-19260,-17990,-16680,-15333,-13951,-12539,-11099,-9635,-8149,-6644
,-5126,-3595,-2057,-514};
const q15_t SinePulseSize = 134;



// Select pulse to use

#define PULSE_ARRAY ChirpPulse1
#define PULSE_SIZE  ChirpPulseSize1

//#define PULSE_ARRAY ChirpPulse2
//#define PULSE_SIZE  ChirpPulseSize2

//#define PULSE_ARRAY SinePulse
//#define PULSE_SIZE  SinePulseSize

static inline q15_t detectSample(volatile q15_t *buff,uint32_t pos,int32_t offset)
 {
 int32_t i,loc,value = 0;

 // Calculate correlation
 for(i=0;i<PULSE_SIZE;i++)
     {
	 loc = (2*HBLOCK_SIZE+pos-PULSE_SIZE+i+1)%(2*HBLOCK_SIZE);
	 value = value + (((int)(buff[loc])-offset)*(int)(PULSE_ARRAY[i]))/(PULSE_SIZE/4);
	 };

 // Scale output
 value = value/(Q15_ONE);

 // Return value
 return (q15_t)value;
 }


// Block mode FIR filter
// Uses the same coefficients as the sample to sample one
void p3blockDetect(void)
 {
 static q15_t offset1=0, offset2=0;
 int i;

 // Detect on channel #1 for all samples in the block
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 outbuff1[blkStart+i] = detectSample(inbuff1,blkStart+i,offset1);
	 }

 // Replicate pulse on channel #2 output
 for(i=0;i<PULSE_SIZE;i++)
      {
 	 outbuff2[blkStart+i] = PULSE_ARRAY[i];
 	 }

 // Set offset when SW1 is pushed
 if (SW1_ACTIVE)
    {
	offset1 = inbuff1[blkStart];
	offset2 = inbuff2[blkStart];
    }
 }

/**********************************************************************************

 BLOCK PROCESSING SOLUTIONS
 P5 : Noise Cancelling

 **********************************************************************************/

/*
# Define a filter
fsize = len(filt2)
adFilter = np.zeros(fsize)
adFilter[0] = 1.0
#adFilter = filt2.copy()
mu = 0.01/fsize
#mu = 0.0

vSignal = np.zeros(fsize)
vError  = np.zeros(fsize)
vNoise  = np.zeros(fsize)
pos = 0
out = []
for s,n in zip(vD,filtNoise):
    vSignal[pos] = s
    vNoise[pos]  = n
    y = 0
    for i in range(0,fsize):
        y = y + vNoise[(fsize+pos-i)%fsize]*adFilter[i]
    e = s - y
    vError[pos] = e
    out.append(e)
    for i in range(0,fsize):
        vpos = (fsize+pos-i)%fsize
        adFilter[i] = adFilter[i] + mu * vNoise[vpos] * vError[vpos]
    pos = (pos + 1)%fsize

out = np.array(out)
*/

// We define a filter
#define FTAP 50
int32_t filter[FTAP];

// Noise canceling function
void p5noiseCancel(void)
 {
 const int32_t mu = 32;             // Mu Constant
 static int32_t reset = 1;          // First execution when 1
 static q15_t offset1=0, offset2=0; // Offset values
 int32_t signal;                    // Current signal value
 int32_t out;                       // Current output filter value
 int32_t error;                     // Current error value
 int32_t i,j;                       // Indexing variables

 // Execute only upon reset
 if(reset)
 	 {
	 reset = 0;  // Remove reset condition
	 // Filter seed
	 for(i=0;i<FTAP;i++)
		 filter[i]=0;
	 // Coefficient 1 is zero, rest of coefficients are 0
	 filter[0]=Q15_ONE;
 	 }

 // For all samples in the block
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 // Current signal
	 signal = inbuff1[blkStart+i]-offset1;

     // Filter calculation using current filter
	 out = 0;
	 for(j=0;j<FTAP;j++)
		 out = out + filter[j]*(inbuff2[(2*HBLOCK_SIZE+blkStart+i-j)%(2*HBLOCK_SIZE)]-offset2);
	 out = out/Q15_ONE;

	 // Error output computed as signal - out
	 error = signal - out;

	 if (SW2_ACTIVE)
	    {
		// If SW2 is active, just send error to both outputs
		outbuff1[blkStart+i] = error;
		outbuff2[blkStart+i] = error;
	    }
	    else
	    {
	    // If SW2 is inactive, just send signal to both outputs
	    outbuff1[blkStart+i] = signal;
	    outbuff2[blkStart+i] = signal;
	    }

	 // Filter adaptation
	 for(j=0;j<FTAP;j++)
		 filter[j] = ((int64_t) filter[j]) + ((int32_t)mu)
		                 * inbuff2[(2*HBLOCK_SIZE+blkStart+i-j)%(2*HBLOCK_SIZE)]
		                 * error //vNoise[(FTAP+pos-j)%FTAP]
		 			 	 / Q15_ONE / Q15_ONE;
     }

 // Set offset when SW1 is pushed
 if (SW1_ACTIVE)
    {
	offset1 = inbuff1[blkStart];
	offset2 = inbuff2[blkStart];
    }
 }

/**********************************************************************************

 EXAMPLE BLOCK PROCESSING FUNCTIONS

 **********************************************************************************/


// Instance for FFT
arm_cfft_radix4_instance_q15 sc15;

// FFT initialization to be performed when the program starts
void fftInit(void)
 {
 uint32_t result;

 result = arm_cfft_radix4_init_q15(&sc15,HBLOCK_SIZE,0,1);
 if (result != ARM_MATH_SUCCESS)
	 chprintf(SERIAL,"%sERROR : Cannot initializae FFT structure%s",BREAK,BREAK);
 }

// Array for the complexfft
q15_t cfft[2*HBLOCK_SIZE];

// This example just performs a FFT
// To be developed
void blockFFT(void)
 {
 int i;
 int32_t value,max,scale;

 // Put the samples in the FFT for left channel
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 cfft[2*i]   = inbuff1[blkStart+i];
	 cfft[2*i+1] = 0;
     }

 // Perform the FFT (Structure initialized in fftInit)
 arm_cfft_radix4_q15(&sc15,cfft);

 // Compute square of the modulo on output buffer
 max = 0;
 // Compute maximum
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 value = cfft[2*i]*cfft[2*i]+cfft[2*i+1]*cfft[2*i+1];
	 if (value > max) max = value;
 	 }
 // Compute scale
 scale = max/(1<<14);
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 outbuff1[blkStart+i] = (cfft[2*i]*cfft[2*i]+cfft[2*i+1]*cfft[2*i+1])/scale;
	 }

 // Put the samples in the FFT for right channel
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 cfft[2*i]   = inbuff2[blkStart+i];
	 cfft[2*i+1] = 0;
     }

 // Perform the FFT (Structure initialized in fftInit)
 arm_cfft_radix4_q15(&sc15,cfft);

 // Compute square of the modulo on output buffer
 max = 0;
 // Compute maximum
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 value = cfft[2*i]*cfft[2*i]+cfft[2*i+1]*cfft[2*i+1];
	 if (value > max) max = value;
 	 }
 // Compute scale
 scale = max/(1<<14);
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 outbuff2[blkStart+i] = (cfft[2*i]*cfft[2*i]+cfft[2*i+1]*cfft[2*i+1])/scale;
	 }
 }


/**********************************************************************************

 TONE CONTROL

 **********************************************************************************/



/* Low pass transition 1k-2k */
#define LP_FIR_TAP 27
const q15_t lpFIR[] = {
  -255,
  -260,
  -312,
  -288,
  -144,
  153,
  616,
  1233,
  1963,
  2739,
  3474,
  4081,
  4481,
  4620,
  4481,
  4081,
  3474,
  2739,
  1963,
  1233,
  616,
  153,
  -144,
  -288,
  -312,
  -260,
  -255
};

/* High pass transition 1k-2k */
#define HP_FIR_TAP 23
const q15_t hpFIR[] = {
  -2200,
  1757,
  1416,
  1117,
  675,
  -33,
  -1014,
  -2188,
  -3403,
  -4464,
  -5189,
  27320,
  -5189,
  -4464,
  -3403,
  -2188,
  -1014,
  -33,
  675,
  1117,
  1416,
  1757,
  -2200
};

extern volatile int32_t pot1value;
extern volatile int32_t pot2value;

/** FIR FILTERS ********************************************************************/

static inline q15_t firSampleLP(volatile q15_t *buff,uint32_t pos,uint32_t offset)
 {
 int32_t i,loc,value=0;

 // Calculate fir
 for(i=0;i<LP_FIR_TAP;i++)
     {
	 loc = (2*HBLOCK_SIZE+pos-i+1)%(2*HBLOCK_SIZE);
	 value = value + (buff[loc]-offset)*(lpFIR[i]);
	 };

 // Scale output
 value = value/Q15_ONE;

 // Return value
 return (q15_t)value;
 }

static inline q15_t firSampleHP(volatile q15_t *buff,uint32_t pos,uint32_t offset)
 {
 int32_t i,loc,value=0;

 // Calculate fir
 for(i=0;i<HP_FIR_TAP;i++)
     {
	 loc = (2*HBLOCK_SIZE+pos-i+1)%(2*HBLOCK_SIZE);
	 value = value + (buff[loc]-offset)*(hpFIR[i]);
	 };

 // Scale output
 value = value/Q15_ONE;

 // Return value
 return (q15_t)value;
 }

// Implements echo addition with configuration delay
#define EC_LEN 4000
void echoToneDemo(q15_t in1,q15_t in2,q15_t *out1,q15_t *out2)
 {
 static q15_t buffer1[EC_LEN];
 static q15_t buffer2[EC_LEN];
 static int32_t pos = 0;

 // Compute current sample
 (*out1) = in1 + (pot2value*buffer1[pos])/1000;
 (*out2) = in2 + (pot2value*buffer2[pos])/1000;

 // Add samples to buffers
 buffer1[pos] = (*out1);
 buffer2[pos] = (*out2);

 // Update position
 pos = (pos+1)%EC_LEN;
 }

// Block mode tone control demo
void demoTone(void)
 {
 static q15_t offset1=0, offset2=0;
 int i;
 int32_t low,high,l_in,r_in;

 // For all samples in the block
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 low  = firSampleLP(inbuff1,blkStart+i,offset1);
	 high = firSampleHP(inbuff1,blkStart+i,offset1);
	 l_in = ((low*pot1value)+(high*(1000-pot1value)))/1000;

	 low  = firSampleLP(inbuff2,blkStart+i,offset2);
	 high = firSampleHP(inbuff2,blkStart+i,offset2);
	 r_in = ((low*pot1value)+(high*(1000-pot1value)))/1000;

	 echoToneDemo(l_in,r_in,outbuff1+blkStart+i,outbuff2+blkStart+i);
     }

 // Set offset when SW1 is pushed
 if (SW1_ACTIVE)
    {
	offset1 = inbuff1[blkStart];
	offset2 = inbuff2[blkStart];
    }
 }




