/* SigLib Deconvolution Example */

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

#define	FFT_SIZE		((SLArrayIndex_t)128)
#define	LOG_FFT_SIZE	((SLArrayIndex_t)7)

#define DATA_SET_11				1		/* Select '1' or '0' to choose between test sequences */
#define ADD_NOISE				0		/* Select '1' to add noise */
#define GAUS_NOISE_OFFSET		SIGLIB_ZERO
#define GAUS_NOISE_VARIANCE		0.00001

#if DATA_SET_11
#define	INPUT_LENGTH			27L		/* Input array length */
#define	IMPULSE_LENGTH			10L		/* Impulse response array length */
SLData_t	Input[] = {					/* Input data */
	0.0, -0.1, -0.3, -0.45, -0.55, -0.45, -0.3, -0.1, 0.0,
	0.1, 0.3, 0.6, 0.9, 1.0, 0.9, 0.6, 0.3, 0.1,
	0.0, -0.1, -0.2, -0.3, -0.35, -0.3, -0.2, -0.1, 0.0
	};

SLData_t	Impulse[] = {				/* Impulse response data */
	0.0, 0.0, 0.3, 1.0, 0.3, -0.2, -0.3, -0.2, 0.0, 0.0
	};
#endif

														/* Result array length */
#define	RESULT_LENGTH			((SLArrayIndex_t)(INPUT_LENGTH + IMPULSE_LENGTH - 1))

SLData_t DistortedInput [RESULT_LENGTH];

SLData_t *pSrcRealData, *pSrcImagData, *pImpulseRealData, *pImpulseImagData, *pFFTCoeffs;

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

#if ADD_NOISE
	SLData_t	GaussianNoisePhase, GaussianNoiseValue;	/* Variables for injecting noise */
#endif

													/* Allocate memory */
	pSrcRealData = SUF_VectorArrayAllocate (FFT_SIZE);
	pSrcImagData = SUF_VectorArrayAllocate (FFT_SIZE);
	pImpulseRealData = SUF_VectorArrayAllocate (FFT_SIZE);
	pImpulseImagData = SUF_VectorArrayAllocate (FFT_SIZE);
	pFFTCoeffs = SUF_FftCoefficientAllocate (FFT_SIZE);


	h2DGraph =										/* Initialize graph */
		Create2DGraph ("Deconvolution",				/* Graph title */
					   "Time / Frequency",			/* 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 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 */

	Display2DGraph (h2DGraph,						/* Graph handle */
				    "System Response - Prior To Smearing By Transducer",	/* Title of the dataset */
				    Input,							/* Array of Double dataset */
				    INPUT_LENGTH,					/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
				    SV_BLUE,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nSystem Response - Prior To Smearing By Transducer\nPlease hit <Carriage Return> to continue . . ."); getchar ();

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

	SDA_ConvolveLinear (Input,						/* Pointer to input array */
						Impulse,					/* Pointer to impulse response data */
						DistortedInput,				/* Pointer to destination array */
						INPUT_LENGTH,				/* Input data length */
						IMPULSE_LENGTH);			/* Impulse response length */

#if ADD_NOISE
									/* Inject some noise into the signal */
									/* Be careful with this because it can make the division unstable in the deconvolution */
	GaussianNoisePhase = SIGLIB_ZERO;
	SDA_SignalGenerate (DistortedInput,				/* Pointer to destination array */
						SIGLIB_GAUSSIAN_NOISE,		/* Signal type - Gaussian noise */
						SIGLIB_ZERO,				/* Signal peak level - Unused */
						SIGLIB_ADD,					/* Fill (overwrite) or add to existing array contents */
						SIGLIB_ZERO,				/* Signal frequency - Unused */
						GAUS_NOISE_OFFSET,			/* D.C. Offset */
						GAUS_NOISE_VARIANCE,		/* Gaussian noise variance */
						SIGLIB_ZERO,				/* Signal end value - Unused */
						&GaussianNoisePhase,		/* Pointer to gaussian signal phase - should be initialised to zero */
						&GaussianNoiseValue,		/* Gaussian signal second sample - should be initialised to zero */
						RESULT_LENGTH);				/* Output array length */
#endif

	Display2DGraph (h2DGraph,						/* Graph handle */
				    "Distorted (smeared) Input",	/* Title of the dataset */
				    DistortedInput,					/* Array of Double dataset */
				    RESULT_LENGTH,					/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
				    SV_BLUE,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nDistorted (smeared) Input\nPlease hit <Carriage Return> to continue . . ."); getchar ();

							/* Zero pad distorted input and impuslse response */
	SDA_Lengthen (DistortedInput,					/* Pointer to source array */
				  pSrcRealData,						/* Pointer to destination array */
				  RESULT_LENGTH,					/* Source array size */
				  FFT_SIZE);						/* Destination array size */
	SDA_Lengthen (Impulse,							/* Pointer to source array */
				  pImpulseRealData,					/* Pointer to destination array */
				  IMPULSE_LENGTH,					/* Source array size */
				  FFT_SIZE);						/* Destination array size */

	SDA_Deconvolution (pSrcRealData,				/* Pointer to real source array 1 */
					   pSrcImagData,				/* Pointer to imaginary source array 1 */
					   pImpulseRealData,			/* Pointer to real source array 2 */
					   pImpulseImagData,			/* Pointer to imaginary source array 2 */
					   SIGLIB_DATA_CLOSE_TO_ZERO,	/* Minimum value to avoid divide by zero */
					   pFFTCoeffs,					/* FFT coefficients */
					   SIGLIB_NULL_ARRAY_INDEX_PTR,	/* FFT Bit reversed addressing look up table */
					   FFT_SIZE,					/* FFT length */
					   LOG_FFT_SIZE);				/* Log2 FFT length */

	Display2DGraph (h2DGraph,						/* Graph handle */
				    "Real Result",					/* Title of the dataset */
				    pSrcRealData,					/* Array of Double dataset */
				    FFT_SIZE,						/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
				    SV_BLUE,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nReal Result\nPlease hit <Carriage Return> to continue . . ."); getchar ();

	Display2DGraph (h2DGraph,						/* Graph handle */
					"Imaginary Result",				/* Title of the dataset */
					pSrcImagData,					/* Array of Double dataset */
					FFT_SIZE,						/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
					SV_BLUE,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nImaginary Result\nPlease hit <Carriage Return> to continue . . ."); getchar ();

	SDA_Subtract2 (Input, pSrcRealData, Input, INPUT_LENGTH);	/* Calculate difference between input and deconvolution */

	Display2DGraph (h2DGraph,						/* Graph handle */
				    "Difference Between Input And Deconvolution",	/* Title of the dataset */
				    Input,							/* Array of Double dataset */
				    INPUT_LENGTH,					/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
				    SV_BLUE,						/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */
	printf ("\nDifference Between Input And Deconvolution\n");

	SUF_MemoryFree (pSrcRealData);					/* Free memory */
	SUF_MemoryFree (pSrcImagData);
	SUF_MemoryFree (pImpulseRealData);
	SUF_MemoryFree (pImpulseImagData);
	SUF_MemoryFree (pFFTCoeffs);
}