/*************************************************************************
 * This code is written by N. Kehtarnavaz and N. Kim as part of the 
 * textbook "Digital Signal Processing System-Level Design Using LabVIEW".
 *************************************************************************/
 
#include <rtdx.h>                      /* RTDX                      */
#include "target.h"                    /* TARGET_INITIALIZE()       */

#define kBUFFER_SIZE 128

float a1[2]={-0.955505, 0.834882};
float b1[3]={0.545337, -0.735242, 0.545337}; 

float a2[2]={0.954255, 0.834810};
float b2[3]={0.545337, 0.734702, 0.545337};

float a3[2]={-0.000622, 0.372609};
float b3[3]={0.545337, 0, -0.545337};

float IIRwindow1[3] = {0,0,0}; 
float y_prev1[2] = {0,0};

float IIRwindow2[3] = {0,0,0}; 
float y_prev2[2] = {0,0};

float IIRwindow3[3] = {0,0,0}; 
float y_prev3[2] = {0,0};

RTDX_CreateInputChannel(cinput);
RTDX_CreateOutputChannel(coutput);

void main()
{	
	float input[kBUFFER_SIZE];
	float output[kBUFFER_SIZE];
	int i, n;
	float ASUM, BSUM;
	
	// Target initialization for RTDX
	TARGET_INITIALIZE();

	/*enable RTDX channels*/
	RTDX_enableInput(&cinput);
	RTDX_enableOutput(&coutput);
	
	for (;;)	/* Infinite message loop. */
	{
		while(!RTDX_read(&cinput, input, sizeof(input)));

		// IIR filtering

		for(i=0; i<kBUFFER_SIZE; i++)
		{

			// Stage #1

		    for(n=2;n>0;n--)
		    	IIRwindow1[n] = IIRwindow1[n-1];
		    
		    IIRwindow1[0] = input[i];
		        
		    BSUM = 0;
		    for(n = 0; n <= 2; n++)
		    {
		       	BSUM += b1[n] * IIRwindow1[n];
		    }
		    
		    ASUM = 0;
		    for(n = 0;n <= 1; n++)
		    {
		    	ASUM += a1[n] * y_prev1[n];
		    }                                           
		    
	    	y_prev1[1] = y_prev1[0];	    
		    y_prev1[0] = BSUM - ASUM;

			// Stage #2
			
		    for(n=2;n>0;n--)
		    	IIRwindow2[n] = IIRwindow2[n-1];
		    
		    IIRwindow2[0] = y_prev1[0];
		        
		    BSUM = 0;
		    for(n = 0; n <= 2; n++)
		    {
		       	BSUM += b2[n] * IIRwindow2[n];
		    }
		    
		    ASUM = 0;
		    for(n = 0;n <= 1; n++)
		    {
		    	ASUM += a2[n] * y_prev2[n];
		    }                                           
		    
	    	y_prev2[1] = y_prev2[0];	    
		    y_prev2[0] = BSUM - ASUM; 
			
			// Stage #3
			
		    for(n=2;n>0;n--)
		    	IIRwindow3[n] = IIRwindow3[n-1];
		    
		    IIRwindow3[0] = y_prev2[0];
		        
		    BSUM = 0;
		    for(n = 0; n <= 2; n++)
		    {
		       	BSUM += b3[n] * IIRwindow3[n];
		    }
		    
		    ASUM = 0;
		    for(n = 0;n <= 1; n++)
		    {
		    	ASUM += a3[n] * y_prev3[n];
		    }                                           
		    
			output[i] = BSUM - ASUM;
	    	y_prev3[1] = y_prev3[0];	    
		    y_prev3[0] = output[i];

		}
		
		/* Write data back to host. */
		RTDX_write(&coutput, output, sizeof(output));
	}
}
