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

#include "YLCStuff.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;
	double Normalizer = Instance->normalization;
	
	/* compute drift rate */
	if (N > 1) {
		Instance->Drift = (Drift * (N - 2) + 6 * (Freq - Mean) / N) / (N + 1);
		SetCtrlVal(Instance->AllanPanel, ALLANPANEL_DRIFT, Instance->Drift/Normalizer);
	}
	Instance->Mean = (Mean * (N - 1) + Freq) / N; 
	
	/* compute allan deviation */
	for (int i = 0; i < ALLAN_MAXPOINTSNUMBER; 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) ;
			} else { // ie if N=1, which is realized for the first completed block
				Instance->FirstMean[i] = CurrentMean;
			}
			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->FirstMean, 0, sizeof(Instance->FirstMean));
	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)
{
	int i, N;
	double X[ALLAN_MAXPOINTSNUMBER];
	double Y[ALLAN_MAXPOINTSNUMBER];
	double Normalizer = Instance->normalization;
	double DriftTau, FirstFreq, LastFreq, Error;
	int dedrift;
	
	GetCtrlVal(Instance->AllanPanel, ALLANPANEL_DEDRIFT, &dedrift);
	
	for (i = 0; i < ALLAN_MAXPOINTSNUMBER; i++) {
		X[i] = pow(2, i);
		if (dedrift) {
			DriftTau = Instance->Drift * X[i];
			N = Instance->BlocksNumber[i];
			if (N>=2) { // if enough data to calculate AVAR for tau = 2^i*tau0, and therefore, AllanVar[i] is meaningful
				FirstFreq = Instance->FirstMean[i];
				LastFreq = Instance->LastMean[i];
				Y[i] = sqrt(Instance->AllanVar[i]+DriftTau*(DriftTau/2-(LastFreq-FirstFreq)/(N-1)))/Normalizer;
				// Not totaly sure it works... to be checked thoroughly   
				}
			else {
				Y[i] = 0;
			}
		} else {
			Y[i] = sqrt(Instance->AllanVar[i])/Normalizer;
		}
	}
	
	DeleteGraphPlot(Instance->AllanPanel, ALLANPANEL_ALLANPLOT, -1, VAL_IMMEDIATE_DRAW); 
	PlotXY(Instance->AllanPanel, ALLANPANEL_ALLANPLOT, X, Y, ALLAN_MAXPOINTSNUMBER,
			VAL_DOUBLE, VAL_DOUBLE, VAL_SCATTER, VAL_SOLID_SQUARE, VAL_SOLID, 1, DATAPOINT_COLOR);
	
	for (i = 0; i < ALLAN_MAXPOINTSNUMBER; 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 YMin, YMax;
			GetCtrlVal(panel, ALLANPANEL_MIN, &YMin);
			GetCtrlVal(panel, ALLANPANEL_MAX, &YMax);
			if (YMin < YMax) {
				SetAxisScalingMode(panel, ALLANPANEL_ALLANPLOT, VAL_LEFT_YAXIS, VAL_MANUAL, pow(10,(double)YMin), pow(10,(double)YMax) );
				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->normalization));
			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;
}