/* SigLib - Program to read vibration data file and generate an order analysis
This program :
	Re-orders the data into an ordergram
*/


#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <conio.h>
#include <ctype.h>
#include <math.h>
#include <siglib.h>			/* SigLib DSP library */
#include "GraphFunctions.h"
#include "readdat.h"


#define SAMPLE_LENGTH				4096L					/* Length of array read from input file */
#define FFT_LENGTH					4096L
#define LOG2_FFT_LENGTH				12L

#define GRAPH_X_AXIS_LENGTH			((SLArrayIndex_t)240)

													/* Parameters for quick sinc look up table */
#define SINC_NUMBER_OF_ADJ_SAMPLES	3L				/* Number of adjacent samples */
#define SINC_LUT_LENGTH				((SLArrayIndex_t)1001)

#define RESULT_LENGTH				(FFT_LENGTH >> 1)		/* Only need to store the lower 1/2 of the FFT output */
#define OVERLAP_LENGTH				(SAMPLE_LENGTH >> 2)	/* 25 % overlap */

#define SAMPLE_PERIOD				(SIGLIB_ONE / SampleRate)	/* Sample rate of data in vibration data file */

#define MAX_NUMBER_OF_FRAMES		400L			/* Maximum number of frames processed */

#define FIRST_ORDER_FREQUENCY		20.0			/* Destination frequency of first order */
#define TENTH_ORDER_BIN				((SLArrayIndex_t)((FIRST_ORDER_FREQUENCY * 10.0 * ((SLData_t)FFT_LENGTH)) / ((SLData_t)SampleRate)))	/* Destination FFT bin of first order */

#define BASE_ORDER					10L				/* Base order */
#define NUMBER_OF_ORDERS			10L				/* Maximum number of orders to extract */
#define ORDER_NUMBER_OF_ADJ_SAMPLES	3L				/* Number of adjacent samples to search */

#define DB_SCALE					(1e5)			/* Input signal scaling - bar to dB*/
//#define LINEAR_SCALE				(1e-5)			/* Input signal scaling - dB to Bar */


#define NUMBER_OF_ORDERS_TO_SUM		 5				/* Number of orders to Sum */

SLData_t	LookUpTablePhase, LookUpTablePhaseGain;
SLData_t	SincLUT [SINC_LUT_LENGTH];
SLData_t	PreviousXValue;							/* Used to maintain continuity across array boundaries */
SLData_t	SincXData [SIGLIB_AI_FOUR*SINC_NUMBER_OF_ADJ_SAMPLES];

SLData_t	*pInputData, *pOverlapArray, *pWindowCoeffs;
SLData_t	*pOverlappedData, *pOrderAnalysisInternalArray, *pOrderMagnitudeResults, *pFFTCoeffs;
SLData_t	*pOrderArray, *pSumLevelArray, *pRealAverage, *pImagAverage, *pSpeed;

char		FileName[80];

void main (int argc, char *argv[]);

void main (int argc, char *argv[])
{
	GraphObject		*hOrderPlotGraph;				/* Declare graph objects */
	GraphObject		*hAverageSpectrumGraph;
	GraphObject		*hOrdergramGraph;

	SLArrayIndex_t	SampleCount;
	FILE			*pInputFile;
	SLArrayIndex_t	i, j;
	SLArrayIndex_t	FrameNumber = SIGLIB_AI_ZERO;	/* Number of frames processed */
	SLArrayIndex_t	OverlapSrcArrayIndex;
	SLArrayIndex_t	LogMagnitudeFlag, XAxisTimeFlag;

	SLData_t		WindowInverseCoherentGain = SIGLIB_ONE;
	SLData_t		Speed;
	SLData_t		SampleRate;


	pInputData = SUF_VectorArrayAllocate (SAMPLE_LENGTH);		/* Input data array */
	pOverlapArray = SUF_VectorArrayAllocate (OVERLAP_LENGTH);	/* Overlap data array */
	pWindowCoeffs = SUF_VectorArrayAllocate (FFT_LENGTH);		/* Window coeffs data array */
	pOverlappedData = SUF_VectorArrayAllocate (SAMPLE_LENGTH);	/* Overlapped data array */
	pOrderAnalysisInternalArray = SUF_OrderAnalysisArrayAllocate (FFT_LENGTH);	/* Order analysis internal processing array */
	pOrderMagnitudeResults = SUF_VectorArrayAllocate (RESULT_LENGTH);		/* Results data array */
	pFFTCoeffs = SUF_FftCoefficientAllocate (FFT_LENGTH);		/* FFT coefficient data array */
	pOrderArray = SUF_VectorArrayAllocate (NUMBER_OF_ORDERS*MAX_NUMBER_OF_FRAMES);	/* Order results data array */
	pSumLevelArray = SUF_VectorArrayAllocate (MAX_NUMBER_OF_FRAMES);	/* Sum level results data array */
	pRealAverage = SUF_VectorArrayAllocate (RESULT_LENGTH);		/* Average order spectrum data array */
	pImagAverage = SUF_VectorArrayAllocate (RESULT_LENGTH);		/* Average order spectrum data array */
	pSpeed = SUF_VectorArrayAllocate (MAX_NUMBER_OF_FRAMES);	/* Speed data array */

	if ((pInputData == NULL) || (pOverlapArray == NULL) || (pWindowCoeffs == NULL) ||
		(pOverlappedData == NULL) || (pOrderAnalysisInternalArray == NULL) || (pOrderMagnitudeResults == NULL) ||
		(pFFTCoeffs == NULL) || (pOrderArray == NULL) ||
		(pSumLevelArray == NULL) || (pRealAverage == NULL) || (pImagAverage == NULL) ||
		(pSpeed == NULL))
	{
		printf ("Memory allocation error\n");
		exit (0);
	}

													/* Reset the copy with overlap */
	SIF_CopyWithOverlap (&OverlapSrcArrayIndex);	/* Pointer to overlap source array index */


	SIF_OrderAnalysis (SincLUT,						/* Pointer to sinc LUT array */
					   &LookUpTablePhaseGain,		/* Pointer to phase gain */
					   SINC_NUMBER_OF_ADJ_SAMPLES,	/* Number of adjacent samples */
					   SINC_LUT_LENGTH,				/* Look up table length */
					   pWindowCoeffs,				/* Window coefficients pointer */
					   SIGLIB_BMAN_HARRIS,			/* Window type */
					   SIGLIB_ZERO,					/* Window coefficient */
					   &WindowInverseCoherentGain,	/* Window inverse coherent gain */
					   pFFTCoeffs,					/* Pointer to FFT coefficients */
					   SIGLIB_NULL_ARRAY_INDEX_PTR,	/* Pointer to bit reverse address table */
					   pRealAverage,				/* Pointer to real average array */
					   pImagAverage,				/* Pointer to imaginary average array */
					   FFT_LENGTH);					/* FFT Size */

// Debug
//SUF_ClearDebugfprintf();

	if (argc != 5)
	{
		printf ("\nUsage error  :\norder SampleRate D/L T/S filename\n");
		printf ("    D/L indicates dB or linear scaling\n");
		printf ("    T/S indicates time or total speed on x-axis\n\n");
		exit (1);									/* Exit - usage error */
	}

	SampleRate = atof(argv[1]);						/* Initilize sample rate */

	printf ("SampleRate = %1.1lf\n", SampleRate);

	if (toupper(argv[2][0]) == 'D')					/* Should we display in dB or linear */
	{
		LogMagnitudeFlag = SIGLIB_TRUE;
	}
	else if (toupper(argv[2][0]) == 'L')
	{
		LogMagnitudeFlag = SIGLIB_FALSE;
	}
	else
	{
		printf ("Error : Scaling option should be D or L for dB or linear\n");
		exit (1);
	}

	if (toupper(argv[3][0]) == 'T')					/* Should we display time or speed on the x-axis */
	{
		XAxisTimeFlag = SIGLIB_TRUE;
	}
	else if (toupper(argv[3][0]) == 'S')
	{
		XAxisTimeFlag = SIGLIB_FALSE;
	}
	else
	{
		printf ("Error : X-Axis option should be T or S for time or speed\n");
		exit (1);
	}

	strcpy (FileName, argv[4]);
	printf ("Source file = %s\n", FileName);

	if ((pInputFile = fopen(FileName, "r")) == NULL)	/* Note this file is text */
	{
		printf ("Error opening input vibration data file\n");
		exit (1);
	}

	if (LogMagnitudeFlag == SIGLIB_TRUE)			/* Create 3D graph with different scaling for log and linear plots */
	{
		hOrdergramGraph =							/* Initialize graph */
			Create3DGraph ("Machine Ordergram",		/* Graph title */
						   "Time",					/* X-Axis label */
						   "Order",					/* Y-Axis label */
						   GRAPH_X_AXIS_LENGTH,		/* X-axis length */
						   0.0,						/* Minimum signal magnitude */
						   100.0,					/* Maximum signal magnitude */
						   0.0,						/* Y-Axis minimum value */
						   SampleRate / SIGLIB_TWO,	/* Y-Axis maximum value */
						   SV_3D_COLOUR,			/* Graph mode */
						   "localhost");			/* Graph server */
		if (hOrdergramGraph == NULL)				/* Graph creation failed - e.g is server running ? */
		{
			printf ("\nGraph creation failure. Please check that the server is running\n");
			exit (1);
		}
	}
	else
	{
		hOrdergramGraph =							/* Initialize graph */
			Create3DGraph ("Machine Ordergram",		/* Graph title */
						   "Time",					/* X-Axis label */
						   "Order",					/* Y-Axis label */
						   GRAPH_X_AXIS_LENGTH,		/* X-axis length */
						   0.0,						/* Minimum signal magnitude */
						   0.1,						/* Maximum signal magnitude */
						   0.0,						/* Y-Axis minimum value */
						   SampleRate / SIGLIB_TWO,	/* Y-Axis maximum value */
						   SV_3D_COLOUR,			/* Graph mode */
						   "localhost");			/* Graph server */
		if (hOrdergramGraph == NULL)				/* Graph creation failed - e.g is server running ? */
		{
			printf ("\nGraph creation failure. Please check that the server is running\n");
			exit (1);
		}
	}

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

	hAverageSpectrumGraph =							/* Initialize graph */
		Create2DGraph ("Average Spectrum",			/* Graph title */
					   "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 (hOrderPlotGraph == NULL)					/* Graph creation failed - e.g is server running ? */
	{
		printf ("\nGraph creation failure. Please check that the server is running\n");
		exit (1);
	}

// Debug
//SUF_Debugfprintf ("Starting order analysis\n");

	while ((SampleCount =
				read_vibration_data (pInputData,
									 pInputFile,
									 &Speed,
									 SAMPLE_LENGTH))
					== SAMPLE_LENGTH)
	{
// Debug
//SUF_Debugfprintf ("FN1 = %ld\n", FrameNumber);

													/* Apply the overlap to the data */
		while (SDA_CopyWithOverlap (pInputData,				/* Pointer to source array */
									pOverlappedData,		/* Pointer to destination array */
									pOverlapArray,			/* Pointer to overlap array */
									&OverlapSrcArrayIndex,	/* Pointer to source array index */
									SAMPLE_LENGTH,			/* Source array length */
									OVERLAP_LENGTH,			/* Overlap length */
									SAMPLE_LENGTH)			/* Destination array length */
					< SAMPLE_LENGTH)
		{
			pSpeed [FrameNumber] = Speed;			/* Save speed */

// Debug
//SUF_Debugfprintf ("FrameNumber = %ld\n", FrameNumber);
//SUF_Debugfprintf ("Calling SDA_OrderAnalysis\n");
													/* Process and extract order information */
		    *(pSumLevelArray+FrameNumber) =
				SDA_OrderAnalysis (pOverlappedData,				/* Pointer to source array */	
								   pOrderAnalysisInternalArray,	/* Pointer to local processing array */
								   pOrderMagnitudeResults,		/* Pointer to destination array */
								   SincLUT,						/* Pointer to LUT array */
								   LookUpTablePhaseGain,		/* Look up table phase gain */
								   FIRST_ORDER_FREQUENCY,		/* First order frequency */
								   Speed / 60.0,				/* Speed - revolutions per second */
								   SINC_NUMBER_OF_ADJ_SAMPLES,	/* Number of adjacent samples for interpolation */
								   pWindowCoeffs,				/* Pointer to window coefficients */
								   WindowInverseCoherentGain,	/* Window inverse coherent gain */
								   pFFTCoeffs,					/* Pointer to FFT coefficients */
								   SIGLIB_NULL_ARRAY_INDEX_PTR,	/* Pointer to bit reverse address table */
								   pRealAverage,				/* Pointer to real average array */
								   pImagAverage,				/* Pointer to imaginary average array */
								   LogMagnitudeFlag,			/* Log magnitude flag */
								   pOrderArray+(FrameNumber * NUMBER_OF_ORDERS),	/* Pointer to order array */
								   BASE_ORDER,					/* Base order */
								   NUMBER_OF_ORDERS,			/* Number of orders to extract */
								   ORDER_NUMBER_OF_ADJ_SAMPLES,	/* Number of adjacent samples to search */
								   SAMPLE_PERIOD,				/* Sample period */
							 	   SIGLIB_SIGNAL_INCOHERENT,	/* Signal source type for summing orders */
								   DB_SCALE,					/* dB scale */
							 	   NUMBER_OF_ORDERS_TO_SUM,		/* Number of orders to sum */
								   SAMPLE_LENGTH,				/* Source array length */
								   FFT_LENGTH,					/* FFT size */
								   LOG2_FFT_LENGTH);			/* log2 FFT size */

														/* Plot results */
			Display3DGraph (hOrdergramGraph,			/* Graph handle */
							"Machine Spectrogram",		/* Title of the dataset */
							pOrderMagnitudeResults,		/* Array of Double dataset */
							RESULT_LENGTH);				/* Number of data points */

			FrameNumber++;
		}
	}

	fclose (pInputFile);							/* Close input file */


//Debug
//SUF_Debugfprintf ("Max frame number = %ld\n", FrameNumber);
//SUF_Debugfprintf ("Plotting orders\n");
													/* Plotting orders */
	for (i = 0; i < NUMBER_OF_ORDERS; i++)
	{
		if (XAxisTimeFlag == SIGLIB_TRUE)			/* Display time on x-axis */
		{
			SLData_t	PlotArray [500];
			for (j = 0; j < FrameNumber; j++)		/* Write out results for each order in turn */
			{
													/* Time (s), Order magnitude */
				PlotArray[j] = *(pOrderArray + (j * NUMBER_OF_ORDERS) + i);
			}
			Display2DGraph (hOrderPlotGraph,		/* Graph handle */
						    "Order results",		/* Title of the dataset */
						    PlotArray,				/* Array of Double dataset */
						    FrameNumber,			/* Number of data points */
							SV_GRAPH_LINE,			/* Graph type */
						    i,						/* Colour */
							SV_HIDE_MARKERS,		/* Marker enable / disable */
							SV_GRAPH_ADD);			/* New graph */
		}
		else										/* Display speed on x-axis */
		{
			SLData_t	PlotArray [500];
			for (j = 0; j < FrameNumber; j++)		/* Write out results for each order in turn */
			{
													/* Speed (rpm), Order magnitude */
				PlotArray[j] = *(pOrderArray + (j * NUMBER_OF_ORDERS) + i);
			}
			Display2DGraph (hOrderPlotGraph,		/* Graph handle */
						    "Order results",		/* Title of the dataset */
						    PlotArray,				/* Array of Double dataset */
						    FrameNumber,			/* Number of data points */
							SV_GRAPH_LINE,			/* Graph type */
						    i,						/* Colour */
							SV_HIDE_MARKERS,		/* Marker enable / disable */
							SV_GRAPH_ADD);			/* New graph */
		}
	}

//Debug
//SUF_Debugfprintf ("Plot speed\n");

	if (XAxisTimeFlag == SIGLIB_TRUE)				/* Only plot speed if we are plotting orders against time */
	{
													/* Plot speed - different scaling for variable and fixed speed */
		printf ("Maximum speed = %lf\n", SDA_Max (pSpeed, FrameNumber));

	if (LogMagnitudeFlag == SIGLIB_TRUE)			/* Plot ordergram graph with different speed scaling for log and linear plots */
		{
			SDA_Divide (pSpeed, 100.0, pSpeed, FrameNumber);

			Display2DGraph (hOrderPlotGraph,		/* Graph handle */
							"Speed x 100",			/* Title of the dataset */
							pSpeed,					/* Array of Double dataset */
							FrameNumber,			/* Number of data points */
							SV_GRAPH_LINE,			/* Graph type */
							SV_YELLOW,				/* Colour */
							SV_HIDE_MARKERS,		/* Marker enable / disable */
							SV_GRAPH_ADD);			/* New graph */
		}
		else
		{
			SDA_Divide (pSpeed, 10000.0, pSpeed, FrameNumber);

			Display2DGraph (hOrderPlotGraph,		/* Graph handle */
							"Speed x 10000",		/* Title of the dataset */
							pSpeed,					/* Array of Double dataset */
							FrameNumber,			/* Number of data points */
							SV_GRAPH_LINE,			/* Graph type */
							SV_YELLOW,				/* Colour */
							SV_HIDE_MARKERS,		/* Marker enable / disable */
							SV_GRAPH_ADD);			/* New graph */
		}
	}


	if (XAxisTimeFlag == SIGLIB_TRUE)				/* Display time on x-axis */
	{
		Display2DGraph (hOrderPlotGraph,			/* Graph handle */
						"Order Sum",				/* Title of the dataset */
						pSumLevelArray,				/* Array of Double dataset */
						FrameNumber,				/* Number of data points */
						SV_GRAPH_LINE,				/* Graph type */
						SV_RED,						/* Colour */
						SV_HIDE_MARKERS,			/* Marker enable / disable */
						SV_GRAPH_ADD);				/* New graph */
	}
	else											/* Display speed on x-axis */
	{
		Display2DGraph (hOrderPlotGraph,			/* Graph handle */
						"Order Sum",				/* Title of the dataset */
						pSumLevelArray,				/* Array of Double dataset */
						FrameNumber,				/* Number of data points */
						SV_GRAPH_LINE,				/* Graph type */
						SV_RED,						/* Colour */
						SV_HIDE_MARKERS,			/* Marker enable / disable */
						SV_GRAPH_ADD);				/* New graph */
	}


//Debug
//SUF_Debugfprintf ("Order sum calculated\n");

													/* Calculate average spectrum */
	SDA_ComplexScalarDivide (pRealAverage, 			/* Pointer to real source array */
							 pImagAverage,			/* Pointer to imaginary source array */
							 (SLData_t)FrameNumber,	/* Scalar divisor */
							 pRealAverage,			/* Pointer to real destination array */
							 pImagAverage,			/* Pointer to imaginary destination array */
							 RESULT_LENGTH);		/* Array lengths */

	if (LogMagnitudeFlag == SIGLIB_TRUE)
	{
													/* Calc real power fm complex */
		SDA_LogMagnitude (pRealAverage,				/* Pointer to real source array */
						  pImagAverage,				/* Pointer to imaginary source array */
						  pOrderMagnitudeResults,	/* Pointer to log magnitude destination array */
						  RESULT_LENGTH);			/* Array length */
	}
	else
	{
													/* Calc real power fm complex */
		SDA_Magnitude (pRealAverage,				/* Pointer to real source array */
					   pImagAverage,				/* Pointer to imaginary source array */
					   pOrderMagnitudeResults,		/* Pointer to log magnitude destination array */
					   RESULT_LENGTH);				/* Array length */
	}


//Debug
//SUF_Debugfprintf ("Calculating spectrum average\n");

	printf ("Spectrum average\n");
	for (i = 1; i <= NUMBER_OF_ORDERS; i++)			/* Extract the required orders from the results */
	{
		printf ("Order %ld = %lf\n", BASE_ORDER*i,
				SDA_ExtractOrder(pOrderMagnitudeResults,		/* Pointer to source array */
								 BASE_ORDER*i,					/* Order to extract */
								 ORDER_NUMBER_OF_ADJ_SAMPLES,	/* Number of samples to search either side of centre */
								 FIRST_ORDER_FREQUENCY,			/* First order frequency */
								 FFT_LENGTH,					/* FFT length */
								 SAMPLE_PERIOD,					/* Sample period */
								 RESULT_LENGTH));				/* Input array length */
	}
	printf ("\n");


//Debug
//SUF_Debugfprintf ("Plot average spectrum\n");

													/* Plot average spectrum */
	Display2DGraph (hAverageSpectrumGraph,			/* Graph handle */
					"Average spectrum",				/* Title of the dataset */
					pRealAverage,					/* Array of Double dataset */
					RESULT_LENGTH,					/* Number of data points */
					SV_GRAPH_LINE,					/* Graph type */
					SV_RED,							/* Colour */
					SV_HIDE_MARKERS,				/* Marker enable / disable */
					SV_GRAPH_NEW);					/* New graph */


// Debug
//SUF_Debugfprintf ("Number of frames processed = %ld\n", FrameNumber);
	printf ("Number of frames processed = %ld\n", FrameNumber);


	free (pInputData);								/* Free memory */
	free (pOverlapArray);
	free (pWindowCoeffs);
	free (pOverlappedData);
	free (pOrderAnalysisInternalArray);
	free (pOrderMagnitudeResults);
	free (pFFTCoeffs);
	free (pOrderArray);
	free (pSumLevelArray);
	free (pRealAverage);
	free (pImagAverage);
	free (pSpeed);
}