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

   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


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

 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;
    }
 }

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

 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];
    }
 }


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

 P3 : CHIRP SIGNAL DATA

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

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;


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

 P4 : BLOCK FFT EXAMPLE

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


// 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 complex fft
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;
	 }

 // Fill the right channel data
 for(i=0;i<HBLOCK_SIZE;i++)
     {
	 if (i<HBLOCK_SIZE/2)
		 outbuff2[blkStart+i] = Q15_ONE - 100;
	     else
	     outbuff2[blkStart+i] = 100 - Q15_ONE;
     }
 }






