// Welch, Wright, & Morrow,
// Real-time Digital Signal Processing, 2017

///////////////////////////////////////////////////////////////////////
// Filename: ISRs.c
//
// Synopsis: Interrupt service routine for codec data transmit/receive
//
///////////////////////////////////////////////////////////////////////

#include "DSP_Config.h"

// Data is received as 2 16-bit words (left/right) packed into one
// 32-bit word.  The union allows the data to be accessed as a single
// entity when transferring to and from the serial port, but still be
// able to manipulate the left and right channels independently.

#define LEFT  0
#define RIGHT 1

volatile union {
	Uint32 UINT;
	Int16 Channel[2];
} CodecDataIn, CodecDataOut;


/* add any global variables here */
/*
#define N 6		// IIR filter order

float B[N+1] = {0.1085, 0, -0.3255, 0, 0.3255, 0, -0.1085};  // numerator coefficients
float A[N+1] = {1.0, -1.1355, 0.9443, -0.6378, 0.5444, -0.1737, 0.0459};  // denominator coefficients
float x[N+1] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};   // input value (buffered)
float y[N+1] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};   // output values (buffered)


void iir_df_one(void){
    int i = 0;
    y[0] = 0.0;
    float sum = 0.0;
    for(i = 0; i < N + 1; i++){
        sum += (B[i] * x[i]) - (A[i] * y[i]);
    }
    y[0] = sum;
    int j = 0;
    for(j = N - 1; j > - 1; j--){
        y[j + 1] = y[j];
        x[j + 1] = x[j];
    }

}
*/

#define N 2     // IIR filter order
#define M 3       // number of biquad
float B[M][N+1] = {{1.0, 0.0, -1.0},{1.0, 0.0, -1.0},{1.0, 0.0, -1.0}};  // numerator coefficients
float A[M][N+1] = {{1.0, -1.2803, 0.6573},{1.0, 0.5139, 0.5305},{1.0, -0.3692, 0.1317}};  // denominator coefficients
float G[M+1] = {0.4999, 0.4999, 0.4342, 1.0};               // scale factors
float x[M][N+1] = {{0.0, 0.0, 0.0},{0.0, 0.0, 0.0},{0.0, 0.0, 0.0}};   // input value (buffered)
float y[M][N+1] = {{0.0, 0.0, 0.0},{0.0, 0.0, 0.0},{0.0, 0.0, 0.0}};   // output values (buffered)

void biquad(int i){
    y[i][0] = B[i][0]*x[i][0] + B[i][1]*x[i][1] + B[i][2]*x[i][2] - A[i][1]*y[i][1]- A[i][2]*y[i][2];
    int j = 0;
    for(j = M - 1; j > 0; j--){
        y[i][j] = y[i][j - 1];
        x[i][j] = x[i][j - 1];
    }
    y[i][0] = G[i + 1] * y[i][0];
}


interrupt void Codec_ISR()
///////////////////////////////////////////////////////////////////////
// Purpose:   Codec interface interrupt service routine
//
// Input:     None
//
// Returns:   Nothing
//
// Calls:     CheckForOverrun, ReadCodecData, WriteCodecData
//
// Notes:     None
///////////////////////////////////////////////////////////////////////
{
	/* add any local variables here */


 	if(CheckForOverrun())					// overrun error occurred (i.e. halted DSP)
		return;								// so serial port is reset to recover

  	CodecDataIn.UINT = ReadCodecData();		// get input data samples

	/* I added my IIR filter routine here */
	//x[0] = CodecDataIn.Channel[RIGHT];		// current input value
  	y[0][0] = G[0]*x[0][0];
  	x[0][0] = CodecDataIn.Channel[RIGHT]/32000.0;    // current input value
  	int i = 0;
  	for (i=0;i<M;i++){
     	    biquad(i);  // implement the i_th biquad
  	    if(i < M - 1) x[i + 1][0] = y[i][0];
  	}

	// iir_df_one();                                       // setup for the next input

	//CodecDataOut.Channel[LEFT]  = y[0];		// setup the LEFT value
  	CodecDataOut.Channel[LEFT]  = y[M-1][0]*32000.0;    // setup the LEFT value
    CodecDataOut.Channel[RIGHT]  = y[M-1][0]*32000.0;    // setup the LEFT value

    // CodecDataOut.Channel[RIGHT]  = y[0];     // setup the LEFT value
	/* end of my IIR filter routine	*/

	WriteCodecData(CodecDataOut.UINT);		// send output data to  port
}