/* Bilinear Transform IIR Filter Example.
Generates a low pass filter */

#include <stdio.h>
#include <siglib.h>
#include <nhl.h>
#include "GraphFunctions.h"

/* Define constants */
#define	SAMPLE_RATE				((SLData_t)10.0)
#define PREWARP_MATCH_FREQUENCY	SIGLIB_TWO
#define	REQUIRED_FILTER_GAIN	((SLData_t)10.0)

#define	NUMBER_OF_ZEROS			5L		/* Can be changed for any number between 1 and 5 */
#define	NUMBER_OF_POLES			5L		/* Can be changed for any number between 1 and 5 */
#define	IIR_FILTER_STAGES		NUMBER_OF_POLES		/* The greater of the number of poles or zeros */
#define	NUMBER_OF_FILTER_COEFFS	(SIGLIB_IIR_COEFFS_PER_BIQUAD*IIR_FILTER_STAGES)	/* Total number of IIR filter coefficients */

#define	IMPULSE_RESPONSE_LENGTH	((SLArrayIndex_t)1024)

#define	FFT_SIZE				IMPULSE_RESPONSE_LENGTH
#define	LOG_FFT_SIZE			((SLArrayIndex_t)10)

#define	PLOT_LENGTH				((SLArrayIndex_t)(IMPULSE_RESPONSE_LENGTH/2))

/* Declare global variables */
					/* Allocate arrays for the poles and zeros. For test pupRealDataoses, the array
					lengths are 1 larger than necessary, so that array lengths of 0 can be used */
SLComplexRect_s SPlaneZeros [NUMBER_OF_ZEROS+1];
SLComplexRect_s SPlanePoles [NUMBER_OF_POLES+1];
SLComplexRect_s ZPlaneZeros [NUMBER_OF_POLES+1];		/* NOTE - THIS ARRAY LENGTH IS SET TO THE SAME LENGTH
											AS THE NUMBER OF POLES BECAUSE WHEN A FILTER IS SPECIFIED
											IN THE S-DOMAIN WITH LESS ZEROS THEN POLES THE BILINEAR
											TRANSFORM ADDS ZEROS AT Fs/2 */
SLComplexRect_s ZPlanePoles [NUMBER_OF_POLES+1];

SLData_t	pFilterState[IIR_FILTER_STAGES * SIGLIB_IIR_DELAY_SIZE];
SLData_t	*pIIRCoeffs;
SLData_t	*pRealData, *pImagData, *pResults, *pStepData, *pFFTCoeffs;

void main (void)
{
	GraphObject *h2DGraph;								/* Declare graph objects */
	GraphObject *hPZGraph;

	SLFixData_t	i;
	SLData_t	Max;
	SLFixData_t	NumberOfZeros = NUMBER_OF_ZEROS;	/* Used for initialising correct number */
	SLFixData_t	NumberOfPoles = NUMBER_OF_POLES;	/* of  s-plane poles and zeros */

														/* Allocate memory */
	pIIRCoeffs = SUF_IirCoefficientAllocate (IIR_FILTER_STAGES);
	pImagData = SUF_VectorArrayAllocate (FFT_SIZE);
	pResults = SUF_VectorArrayAllocate (FFT_SIZE);
	pRealData = SUF_VectorArrayAllocate (IMPULSE_RESPONSE_LENGTH);
	pStepData = SUF_VectorArrayAllocate (IMPULSE_RESPONSE_LENGTH);
	pFFTCoeffs = SUF_FftCoefficientAllocate (FFT_SIZE);

	if ((pIIRCoeffs == NULL) || (pImagData == NULL) || (pResults == NULL) ||
		(pRealData == NULL) || (pStepData == NULL) || (pFFTCoeffs == NULL))
	{
		printf ("\n\nMemory allocation failure\n\n");
		exit (0);
	}

	if (NumberOfZeros >= 1)
		SPlaneZeros[0] = SCV_Rectangular (((SLData_t)0.0), ((SLData_t)3.0));	/* Initialise s-plane zeros */
	if (NumberOfZeros >= 2)
		SPlaneZeros[1] = SCV_Rectangular (((SLData_t)0.0), ((SLData_t)3.0));
	if (NumberOfZeros >= 3)
		SPlaneZeros[2] = SCV_Rectangular (((SLData_t)0.0), ((SLData_t)3.0));
	if (NumberOfZeros >= 4)
		SPlaneZeros[3] = SCV_Rectangular (((SLData_t)0.0), ((SLData_t)3.0));
	if (NumberOfZeros >= 5)
		SPlaneZeros[4] = SCV_Rectangular (((SLData_t)0.0), ((SLData_t)3.0));

	if (NumberOfPoles >= 1)
		SPlanePoles[0] = SCV_Rectangular (((SLData_t)-0.2), ((SLData_t)0.0));	/* Initialise s-plane poles */
	if (NumberOfPoles >= 2)
		SPlanePoles[1] = SCV_Rectangular (((SLData_t)-0.3), ((SLData_t)0.0));
	if (NumberOfPoles >= 3)
		SPlanePoles[2] = SCV_Rectangular (((SLData_t)-0.6), ((SLData_t)0.7));
	if (NumberOfPoles >= 4)
		SPlanePoles[3] = SCV_Rectangular (((SLData_t)-0.8), ((SLData_t)0.6));
	if (NumberOfPoles >= 5)
		SPlanePoles[4] = SCV_Rectangular (((SLData_t)-0.9), ((SLData_t)0.3));


	printf ("\nComplex s-plane zeros\n");		/* Print s-plane poles and zeros */
	for (i = 0; i < NumberOfZeros; i++)
	{
		print_rectangular (SPlaneZeros[i]);
	}

	printf ("\nComplex s-plane poles\n");
	for (i = 0; i < NUMBER_OF_POLES; i++)
	{
		print_rectangular (SPlanePoles[i]);
	}

	printf ("Hit any key to continue . . ."); getchar ();

													/* Convert z-plane poles and zeros to s-plane */
													/* using the bilinear transform */
	SDA_BilinearTransform (SPlaneZeros,				/* S-plane zeros */
						   SPlanePoles,				/* S-plane poles */
						   ZPlaneZeros,				/* Z-plane zeros */
						   ZPlanePoles,				/* Z-plane poles */
						   SAMPLE_RATE,				/* Sample rate */
						   PREWARP_MATCH_FREQUENCY,	/* Pre-warp frequency */
						   SIGLIB_OFF,				/* Pre-warp switch */
						   NumberOfZeros,			/* Number of zeros */
						   NUMBER_OF_POLES);		/* Number of poles */

	NumberOfZeros = (SLArrayIndex_t)NUMBER_OF_POLES;	/* If number of s-plane zeros < number of poles,
															additional zeros places at Fs/2 */

	printf ("\nComplex z-plane zeros\n");			/* Print z-plane poles and zeros */
	for (i = 0; i < NumberOfZeros; i++)
	{
		print_rectangular (ZPlaneZeros[i]);
	}

	printf ("\nComplex z-plane poles\n");
	for (i = 0; i < NUMBER_OF_POLES; i++)
	{
		print_rectangular (ZPlanePoles[i]);
	}

	hPZGraph =										/* Initialize graph */
		CreatePoleZeroGraph("Pole-Zero Plot",		/* Graph title */
							"Real",					/* X-Axis label */
							"Imaginary",			/* Y-Axis label */
							SV_COMPLEX_ZERO,		/* PZ Data type */
							"localhost");			/* Graph server */
	if (hPZGraph == NULL)							/* Graph creation failed - e.g is server running ? */
	{
		printf ("\nGraph creation failure. Please check that the server is running\n");
		exit (1);
	}

	DisplayPZPlot(hPZGraph,							/* Graph object */
				  "Poles",							/* Title of dataset (for use on legends) */
				  (Complex_s *)ZPlanePoles,			/* Array of complex values */
				  NUMBER_OF_POLES,					/* Items of data */
				  SV_BLUE,							/* Colour */
				  SV_GRAPH_NEW,						/* Add / new mode */
				  SV_CONJUGATE_POLE);				/* Pole-zero mode */
	DisplayPZPlot(hPZGraph,							/* Graph object */
				  "Zeros",							/* Title of dataset (for use on legends) */
				  (Complex_s *)ZPlaneZeros,			/* Array of complex values */
				  NumberOfZeros,					/* Items of data */
				  SV_RED,							/* Colour */
				  SV_GRAPH_ADD,						/* Add / new mode */
				  SV_CONJUGATE_ZERO);				/* Pole-zero mode */
	printf ("Pole-zero plot\nHit any key to continue . . ."); getchar ();


													/* Convert poles and zeros to coefficients */
	SDA_IirZplaneToCoeffs (ZPlaneZeros,				/* Z-plane zeros */
						   ZPlanePoles,				/* Z-plane zeros */
						   pIIRCoeffs,				/* IIR filter coefficients */
						   NumberOfZeros,			/* Number of zeros */
						   NUMBER_OF_POLES);		/* Number of poles */

													/* Apply filter gain function */
	SDA_ModifyIirFilterGain (pIIRCoeffs,			/* Pointer to source IIR filter coefficients */
							 pIIRCoeffs,			/* Pointer to modified IIR filter coefficients */
							 SIGLIB_ZERO,			/* Centre frequency normalised to 1 Hz */
							 REQUIRED_FILTER_GAIN,	/* Desired gain */
							 IIR_FILTER_STAGES);	/* Number of biquads */

													/* Print filter coefficients */
	printf ("\nIIR filter coefficients : b(0), b(1), b(2), a(1), a(2)\n\n");
	
	for (i = 0; i < IIR_FILTER_STAGES; i++)
	{
		printf ("%le, %le, %le, %le, %le\n", *(pIIRCoeffs+(i*SIGLIB_IIR_COEFFS_PER_BIQUAD)),
				*(pIIRCoeffs+1+(i*SIGLIB_IIR_COEFFS_PER_BIQUAD)), *(pIIRCoeffs+2+(i*SIGLIB_IIR_COEFFS_PER_BIQUAD)),
				*(pIIRCoeffs+3+(i*SIGLIB_IIR_COEFFS_PER_BIQUAD)), *(pIIRCoeffs+4+(i*SIGLIB_IIR_COEFFS_PER_BIQUAD)));
	}

	printf ("Hit any key to see impulse, step and frequency response . . ."); getchar ();

	h2DGraph =										/* Initialize graph */
		Create2DGraph ("Bilinear Transform",		/* Graph title */
					   "Time",						/* X-Axis label */
					   "Magnitude",					/* Y-Axis label */
					   SV_AUTO_SCALE,				/* Scaling mode */
					   SV_SIGNED,					/* Sign mode */
					   SV_GRAPH_LINE,				/* Graph type */
					   "localhost");				/* Graph server */
	if (h2DGraph == NULL)							/* Graph creation failed - e.g is server running ? */
	{
		printf ("\nGraph creation failure. Please check that the server is running\n");
		exit (1);
	}

													/* Initialise IIR filter operation */
	SIF_Iir (pFilterState,							/* Pointer to filter state array */
			 IIR_FILTER_STAGES);					/* Number of second order stages */

													/* Generate impulse response */
	SDA_SignalGenerate (pRealData,					/* Pointer to destination array */
						SIGLIB_IMPULSE,				/* Signal type - Impulse function */
						SIGLIB_ONE,					/* Signal peak level */
						SIGLIB_FILL,				/* Fill (overwrite) or add to existing array contents */
						SIGLIB_ZERO,				/* Signal frequency - Unused */
						SIGLIB_ZERO,				/* D.C. Offset */
						SIGLIB_ZERO,				/* Unused */
						SIGLIB_ZERO,				/* Signal end value - Unused */
						SIGLIB_NULL_DATA_PTR,		/* Unused */
						SIGLIB_NULL_DATA_PTR,		/* Unused */
						IMPULSE_RESPONSE_LENGTH)	/* Output array length */;

													/* Apply iir filter and store filtered data */
	SDA_Iir (pRealData,								/* Input array to be filtered */
			 pRealData,								/* Filtered output array */
			 pFilterState,							/* Pointer to filter state array */
			 pIIRCoeffs,							/* Pointer to filter coefficients array */
			 IIR_FILTER_STAGES,						/* Number of stages */
			 IMPULSE_RESPONSE_LENGTH);				/* Array length */

	Display2DGraph (h2DGraph,						/* Graph handle */
				    "Impulse response",				/* Title of the dataset */
				    pRealData,						/* Array of Double dataset */
				    PLOT_LENGTH,					/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
				    SV_MAGENTA,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nImpulse response\nPlease hit <Carriage Return> to continue . . ."); getchar ();


													/* Re-initialise IIR filter operation */
	SIF_Iir (pFilterState,							/* Pointer to filter state array */
			 IIR_FILTER_STAGES);					/* Number of second order stages */

    												/* Generate step response */
	SDA_SignalGenerate (pStepData,					/* Pointer to destination array */
						SIGLIB_STEP,				/* Signal type - Step function */
						SIGLIB_ONE,					/* Signal peak level */
						SIGLIB_FILL,				/* Fill (overwrite) or add to existing array contents */
						SIGLIB_ZERO,				/* Signal frequency - Unused */
						SIGLIB_ZERO,				/* D.C. Offset */
						SIGLIB_ZERO,				/* Unused */
						SIGLIB_ZERO,				/* Signal end value - Unused */
						SIGLIB_NULL_DATA_PTR,		/* Unused */
						SIGLIB_NULL_DATA_PTR,		/* Unused */
						IMPULSE_RESPONSE_LENGTH);	/* Output array length */
                        
	SDA_Iir (pStepData,								/* Input array to be filtered */
			 pStepData,								/* Filtered output array */
			 pFilterState,							/* Pointer to filter state array */
			 pIIRCoeffs,							/* Pointer to filter coefficients array */
			 IIR_FILTER_STAGES,						/* Number of stages */
			 IMPULSE_RESPONSE_LENGTH);				/* Array length */

	Display2DGraph (h2DGraph,						/* Graph handle */
				    "Step response",				/* Title of the dataset */
				    pStepData,						/* Array of Double dataset */
				    PLOT_LENGTH,					/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
				    SV_MAGENTA,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nStep response\nPlease hit <Carriage Return> to continue . . ."); getchar ();


													/* Initialise FFT */
	SIF_Fft (pFFTCoeffs,							/* Pointer to FFT coefficients */
			 SIGLIB_NULL_ARRAY_INDEX_PTR,			/* Pointer to bit reverse address table - NOT USED */
			 FFT_SIZE);								/* FFT Size */

													/* Perform real FFT */
	SDA_Rfft (pRealData,							/* Pointer to real array */
			  pImagData,							/* Pointer to imaginary array */
			  pFFTCoeffs,							/* Pointer to FFT coefficients */
			  SIGLIB_NULL_ARRAY_INDEX_PTR,			/* Pointer to bit reverse address table - NOT USED */
			  FFT_SIZE,								/* FFT size */
			  LOG_FFT_SIZE);						/* log2 FFT size */

													/* Calculate real power from complex */
	SDA_LogMagnitude (pRealData,					/* Pointer to real source array */
					  pImagData,					/* Pointer to imaginary source array */
					  pResults,						/* Pointer to log magnitude destination array */
					  PLOT_LENGTH);					/* Array length */

	Max = SDA_AbsMax (pResults,						/* Pointer to source array */
					  PLOT_LENGTH);					/* Array length */
	SDA_Offset (pResults,							/* Pointer to source array */
				Max,								/* D.C. offset */
				pResults,							/* Pointer to destination array */
				PLOT_LENGTH);						/* Array length */

	Display2DGraph (h2DGraph,						/* Graph handle */
				    "Frequency response",			/* Title of the dataset */
				    pResults,						/* Array of Double dataset */
				    PLOT_LENGTH,					/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
				    SV_MAGENTA,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nFrequency response\nPlease hit <Carriage Return> to continue . . ."); getchar ();

	DeleteGraph (h2DGraph);							/* Delete the graphs */
	DeleteGraph (hPZGraph);

	SUF_MemoryFree (pRealData);						/* Free memory */
	SUF_MemoryFree (pIIRCoeffs);
	SUF_MemoryFree (pImagData);
	SUF_MemoryFree (pResults);
	SUF_MemoryFree (pFFTCoeffs);
}