/* SigLib Phase Locked Loop Example */

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

/* Define constants */
#define	SAMPLE_LENGTH					512L
#define	SAMPLE_RATE						((SLData_t)8000.0)
#define	PLL_LOOP_FILTER_LENGTH			41L					/* PLL loop filter length */
#define CARRIER_FREQ					((SLData_t)200.0)	/* Carrier frequency */
#define LPF_CUTOFF_FREQUENCY			((SLData_t)100.0)	/* LPF cut-off frequency */
#define	VCO_MODULATION_INDEX			((SLData_t)0.05)	/* Modulation index */

#define	PLL_HILBERT_TFORM_LENGTH		41L					/* PLL Hilbert transform filter length */
#define	VCO_SINE_TABLE_SIZE				((SLArrayIndex_t)1024)	/* Look up table for fast sine calculation */

SLData_t	*pData;
SLData_t	*pPLLLoopFilterCoeffs, *pPLLLoopFilterState;	/* PLL loop filter coefficient pointer */

SLArrayIndex_t	PLLLoopFilterIndex;					/* PLL loop filter index */
SLData_t	PLLVCOPhase;							/* PLL VCO phase */
SLData_t	PLLSavedSample;							/* PLL saved sample for next iteration */

SLData_t	*pPLLHilbertTformFilterCoeffs, *pPLLHilbertTformFilterState;
SLArrayIndex_t	PLLHilbertTformFilterIndex;

SLData_t	*pVCOLookUpTable;						/* For fast sine lookup */
SLData_t	SinePhase;

void   main(void)

{
	GraphObject *h2DGraph;							/* Declare graph object */

	pData = SUF_VectorArrayAllocate (SAMPLE_LENGTH);
	pPLLLoopFilterCoeffs = SUF_VectorArrayAllocate (PLL_LOOP_FILTER_LENGTH);
	pPLLLoopFilterState = SUF_VectorArrayAllocate (PLL_LOOP_FILTER_LENGTH);
	pPLLHilbertTformFilterCoeffs = SUF_VectorArrayAllocate (PLL_HILBERT_TFORM_LENGTH);
	pPLLHilbertTformFilterState = SUF_VectorArrayAllocate (PLL_HILBERT_TFORM_LENGTH);
	pVCOLookUpTable = SUF_VectorArrayAllocate (VCO_SINE_TABLE_SIZE);

	h2DGraph =										/* Initialize graph */
		Create2DGraph ("Phase Locked Loop",			/* 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);
	}

	SinePhase = 1.2;								/* Arbitrary phase - radians */

	SIF_PhaseLockedLoop (&PLLVCOPhase,							/* VCO phase */
						 pVCOLookUpTable,						/* VCO look up table */
						 VCO_SINE_TABLE_SIZE,					/* VCO look up table size */
						 LPF_CUTOFF_FREQUENCY / SAMPLE_RATE,	/* Carrier frequency */
						 pPLLLoopFilterState,					/* Pointer to loop filter state */
						 pPLLLoopFilterCoeffs,					/* Pointer to loop filter coefficients */
						 &PLLLoopFilterIndex,					/* Pointer to loop filter index */
						 PLL_LOOP_FILTER_LENGTH,				/* Loop filter length */
						 pPLLHilbertTformFilterState,			/* Pointer to Hilbert transformer filter state */
						 pPLLHilbertTformFilterCoeffs,			/* Pointer to Hilbert transformer filter coefficients */
						 &PLLHilbertTformFilterIndex,			/* Pointer to Hilbert transformer filter index */
						 PLL_HILBERT_TFORM_LENGTH,				/* Hilbert transformer filter length */
						 &PLLSavedSample);						/* Pointer to delayed sample */

	printf ("\n\nSigLib Phase Locked Loop Example\n");
	printf ("Hit <CR> to move continue with the same input signal\n");
	printf ("Hit 'x' to exit\n");
	printf ("Please hit <CR> to continue . . .\n");
	getchar ();

	do
	{
		SDA_SignalGenerate (pData,						/* Output array pointer */
							SIGLIB_SINE_WAVE,			/* Signal type - Sine wave */
							SIGLIB_ONE,					/* Signal peak level */
							SIGLIB_FILL,				/* Fill (overwrite) or add to existing array contents */
							CARRIER_FREQ / SAMPLE_RATE,	/* Signal frequency */
							SIGLIB_ZERO,				/* D.C. Offset */
							SIGLIB_ZERO,				/* Unused */
							SIGLIB_ZERO,				/* Signal end value - Unused */
							&SinePhase,					/* Signal phase - maintained across array boundaries */
							SIGLIB_NULL_DATA_PTR,		/* Unused */
							SAMPLE_LENGTH);				/* Output array length */

		Display2DGraph (h2DGraph,					/* Graph handle */
					    "Original Sine Wave",		/* Title of the dataset */
					    pData,						/* Array of Double dataset */
					    SAMPLE_LENGTH,				/* Number of data points */
						SV_GRAPH_LINE,				/* Graph type */
					    SV_BLUE,					/* Colour */
						SV_HIDE_MARKERS,			/* Marker enable / disable */
						SV_GRAPH_NEW);				/* New graph */

		SDA_PhaseLockedLoop (pData,							/* Pointer to source array */
							 pData,							/* Pointer to destination array */
							 &PLLVCOPhase,					/* VCO phase */
							 VCO_MODULATION_INDEX,			/* VCO modulation index */
							 pVCOLookUpTable,				/* VCO look up table */
							 VCO_SINE_TABLE_SIZE,			/* VCO look up table size */
							 CARRIER_FREQ / SAMPLE_RATE,	/* Carrier frequency */
							 pPLLLoopFilterState,			/* Pointer to loop filter state */
							 pPLLLoopFilterCoeffs,			/* Pointer to loop filter coefficients */
							 &PLLLoopFilterIndex,			/* Pointer to loop filter index */
							 PLL_LOOP_FILTER_LENGTH,		/* Loop filter length */
							 pPLLHilbertTformFilterState,	/* Pointer to Hilbert transformer filter state */
							 pPLLHilbertTformFilterCoeffs,	/* Pointer to Hilbert transformer filter coefficients */
							 &PLLHilbertTformFilterIndex,	/* Pointer to Hilbert transformer filter index */
							 PLL_HILBERT_TFORM_LENGTH,		/* Hilbert transformer filter length */
							 &PLLSavedSample,				/* Pointer to delayed sample */
							 SAMPLE_LENGTH);				/* Length of input array */

		Display2DGraph (h2DGraph,					/* Graph handle */
					    "PLL Output",				/* Title of the dataset */
					    pData,						/* Array of Double dataset */
					    SAMPLE_LENGTH,				/* Number of data points */
						SV_GRAPH_LINE,				/* Graph type */
					    SV_RED,						/* Colour */
						SV_HIDE_MARKERS,			/* Marker enable / disable */
						SV_GRAPH_ADD);				/* New graph */

	} while (tolower (getchar ()) != 'x');


	SUF_MemoryFree (pData);							/* Free memory */
	SUF_MemoryFree (pPLLLoopFilterCoeffs);
	SUF_MemoryFree (pPLLLoopFilterState);
	SUF_MemoryFree (pPLLHilbertTformFilterCoeffs);
	SUF_MemoryFree (pPLLHilbertTformFilterState);
	SUF_MemoryFree (pVCOLookUpTable);

}