#include <ansi_c.h>
#include <userint.h>

#include "utils.h"
#include "FXAllan.h"
#include "Allan.h"

#define DATAPOINT_COLOR VAL_RED
#define ERRORBAR_COLOR VAL_RED


static void Allan_Reset(Allan_Data * Instance);
static void Allan_Display(Allan_Data * Instance);


void Allan_InitPanel(Allan_Data * Instance, const char *title, double normalization, int parent, int control) 
{
	if ((Instance->AllanPanel = LoadPanel (0, "FXAllan.uir", ALLANPANEL)) < 0)
		return;
	
	Allan_Reset(Instance);
	Instance->normalization = normalization;
	Instance->autoscale = FALSE;
	Instance->active = TRUE;
	Instance->parent = parent;
	Instance->control = control;
	
	SetPanelAttribute(Instance->AllanPanel, ATTR_TITLE, title); 
	SetPanelAttribute (Instance->AllanPanel, ATTR_CALLBACK_DATA, (void *)Instance);
	DisplayPanel (Instance->AllanPanel);
	SetCtrlVal(Instance->AllanPanel, ALLANPANEL_NORMALIZER, normalization);
}


void Allan_ClosePanel(Allan_Data * Instance)
{
	Instance->active = FALSE;
	SetCtrlVal(Instance->parent, Instance->control, FALSE);
	DiscardPanel(Instance->AllanPanel);
}


void Allan_AddFrequency(Allan_Data * Instance, double Freq)
{
	/* total number of points used. used to calculate the drift rate */
	int N = 1 + Instance->BlocksNumber[0];
	double Mean = Instance->Mean;
	double Drift = Instance->Drift;
	
	/* compute drift rate */
	if (N > 1)
		Instance->Drift = (Drift * (N - 2) + 6 * (Freq - Mean) / N) / (N + 1);
	Instance->Mean = (Mean * (N - 1) + Freq) / N; 
	
	/* compute allan deviation */
	for (int i = 0; i < ALLAN_NUM_DATAPOINTS; i++) {
		Instance->CurrentAverage[i] = (Instance->CurrentAverage[i]*Instance->NbCurrentAverage[i] + Freq)
											/(Instance->NbCurrentAverage[i]+1);
		Instance->NbCurrentAverage[i] +=1;
		
		if (Instance->NbCurrentAverage[i] >= pow(2,i) ) {
			double CurrentMean = Instance->CurrentAverage[i];
			double LastMean = Instance->LastMean[i];
			N = Instance->BlocksNumber[i];
			N++;
			Instance->CurrentAverage[i] = 0;
			Instance->NbCurrentAverage[i] = 0;
			if (N > 1) {
				Instance->AllanVar[i] = (Instance->AllanVar[i]*(N-2)
										+0.5*pow(CurrentMean-LastMean,2))/(N-1) ;
			}
			Instance->LastMean[i] = CurrentMean;
			Instance->BlocksNumber[i] = N;
		}
	}
	
	Allan_Display(Instance);
}


/* private */

static void Allan_Reset(Allan_Data *Instance)
{
	memset(Instance->AllanVar, 0, sizeof(Instance->AllanVar));
	memset(Instance->LastMean, 0, sizeof(Instance->LastMean));
	memset(Instance->BlocksNumber, 0, sizeof(Instance->BlocksNumber));
	memset(Instance->CurrentAverage, 0, sizeof(Instance->CurrentAverage));
	memset(Instance->NbCurrentAverage, 0, sizeof(Instance->NbCurrentAverage));
	Instance->Drift = 0;
	Instance->Mean = 0;
}

	
static void Allan_Display(Allan_Data *Instance)
{
	double x[ALLAN_NUM_DATAPOINTS];
	double y[ALLAN_NUM_DATAPOINTS];
	double error;
	int dedrift;
	int i;
	
	GetCtrlVal(Instance->AllanPanel, ALLANPANEL_DEDRIFT, &dedrift);
	SetCtrlVal(Instance->AllanPanel, ALLANPANEL_DRIFT, Instance->Drift);
	
	for (i = 0; i < ALLAN_NUM_DATAPOINTS; i++) {
		if (Instance->AllanVar[i] == 0.0)
			break;
		x[i] = pow(2, i);
		/* remove  linear drift estimated contribution to the allan variance */
		if (dedrift) {
			double corr = 0.5 * (Instance->Drift * x[i]) * (Instance->Drift * x[i]);
			/* uncertainty in the estimate of the drift rate may over correct
			   the estimated allan variance and result in a negative value. in
			   this case we take the absolute value of the corrected value as
			   the best estimator of the allan variance */
			y[i] = sqrt(fabs(Instance->AllanVar[i] - corr)) / Instance->normalization;
		} else {
			y[i] = sqrt(Instance->AllanVar[i]) / Instance->normalization;
		}
	}
	
	DeleteGraphPlot(Instance->AllanPanel, ALLANPANEL_ALLANPLOT, -1, VAL_IMMEDIATE_DRAW); 
	PlotXY(Instance->AllanPanel, ALLANPANEL_ALLANPLOT, x, y, ALLAN_NUM_DATAPOINTS,
			VAL_DOUBLE, VAL_DOUBLE, VAL_SCATTER, VAL_SOLID_SQUARE, VAL_SOLID, 1, DATAPOINT_COLOR);
	
	for (i = 0; i < ALLAN_NUM_DATAPOINTS; i++) {
		error = 1 / sqrt(Instance->BlocksNumber[i]);
		PlotLine(Instance->AllanPanel, ALLANPANEL_ALLANPLOT,
			x[i], y[i] * (1 - error), x[i], y[i] * (1 + error), ERRORBAR_COLOR);
	}
}


/* callbacks */

int  CVICALLBACK Allan_CB_Reset(int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch(event)
	{
		case EVENT_COMMIT:
			Allan_Data *data;
			GetPanelAttribute(panel, ATTR_CALLBACK_DATA, &data);
			Allan_Reset(data);
			break;
	}
	return 0;
}


int CVICALLBACK Allan_CB_ChangeYLim (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			int pmin, pmax;
			GetCtrlVal(panel, ALLANPANEL_MIN, &pmin);
			GetCtrlVal(panel, ALLANPANEL_MAX, &pmax);
			if (pmin < pmax) {
				SetAxisScalingMode(panel, ALLANPANEL_ALLANPLOT, VAL_LEFT_YAXIS, VAL_MANUAL, pow(10, pmin), pow(10, pmax));
				SetCtrlVal(panel, ALLANPANEL_CHECKBOX_AUTOSCALE, FALSE);
			}
			break;
	}	
	return 0;
}


int CVICALLBACK Allan_CB_ChangeAutoScale (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{  
	switch (event)
	{
		case EVENT_COMMIT:
			Allan_Data *data;
			GetPanelAttribute(panel, ATTR_CALLBACK_DATA, &data);
			GetCtrlVal(panel, control, &(data->autoscale));
			Allan_Display(data);
			break;
	}
	return 0; 
} 


int CVICALLBACK Allan_CB_ChangeNormalization (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			Allan_Data *data;
			GetPanelAttribute(panel, ATTR_CALLBACK_DATA, &data);
			GetCtrlVal(panel, control, &(data->normalization));
			Allan_Display(data);
			break;
	}
	return 0; 
} 


int CVICALLBACK CB_GeneralAllanPanel (int panel, int event, void *callbackData,
		int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_CLOSE:
			Allan_Data *data;
			GetPanelAttribute(panel, ATTR_CALLBACK_DATA, &data);
			Allan_ClosePanel(data);
			break;
	}
	return 0;
}