//==============================================================================
//
// Title:		Pr_Align
// Purpose:		A short description of the application.
//
// Created on:	1/28/2015 at 1:37:25 PM by Arnaud Madur.
// Copyright:	LBNL. All Rights Reserved.
//
//==============================================================================

//==============================================================================
// Include files

#include <ansi_c.h>
#include <cvirte.h>		
#include <userint.h>
#include "Pr_Align.h"
#include "InitStatus.h"
#include "toolbox.h"
#include "Utilities.h"
#include "Hp3458.h"
#include "umac.h"
#include "umacParam.h"
#include "Param_umac.h" 
#include "Spline.h"

//==============================================================================
// Constants
#define PRMFILE "w:\\LCLS-II\\data\\ZScan.prm"		    
#define UNDPRMFILE "w:\\LCLS-II\\data\\align.prm"
#define ZGAUSSFILE1 "w:\\LCLS-II\\data\\ZGauss1.log"
#define ZGAUSSFILE2  "w:\\LCLS-II\\data\\ZGauss2.log"
#define RMPRMFILE  "w:\\LCLS-II\\data\\RefMag.prm"
#define X0 -9.0
#define NCOLORS 5
#define NPMX 1000
#define MINMOVE 1e-3

//==============================================================================
// Types

//==============================================================================
// Static global variables

static int panelHandle = 0;
static int panelHandle_IN = 0;
static int umac_ID;  

//==============================================================================
// Static functions

//==============================================================================
// Global variables
double posx, posy, posz;
double posrx, posry, posrz;
double poswz, posvy;
double X_Scan, Y_Scan;
double XL, YL, ZL, RXL, RYL, RZL;
double Z_RefMag;
unsigned int LED_toggle = TRUE;
char zgauss1_file[FILENAME]= ZGAUSSFILE1;
char zgauss2_file[FILENAME]= ZGAUSSFILE2;
unsigned int plot_num = 0;
int colors[] = {VAL_RED, VAL_GREEN, VAL_BLUE, VAL_CYAN, VAL_MAGENTA};
BOOL Do_HScans = TRUE;
BOOL Do_VScans = TRUE;

// UMAC variables
UMAC_PARAM_STRUCT nParam[9];
int ID =-1;
static  DWORD UMAC_ID;
char align_dir[FILENAME];
FILE *fpl;	// pointer to log file

//==============================================================================
// Global functions
void ZScan(void);
int findPeaks (double z[], double B[], unsigned int npts, double fltr, double zp[], double Bp[], unsigned int *pnpks);
void LS_Quad (double x[], double y[], unsigned int npts, double c[]);

/// HIFN The main entry-point function.
int main (int argc, char *argv[])
{
	int error = 0;
	char und_prm_file[FILENAME] = UNDPRMFILE;
	char param_file[FILENAME] = PRMFILE;
	char ref_mag_prm_file[FILENAME] = RMPRMFILE;
	char buffer[LINE];
	FILE *fpi;
	
	/* initialize and load resources */
	nullChk (InitCVIRTE (0, argv, 0));
	errChk (panelHandle = LoadPanel (0, "Pr_Align.uir", PANEL)); // Main panel
	errChk (panelHandle_IN = LoadPanel (0, "InitStatus.uir", IN_PANEL)); // Initialization panel
	
	/* display the panel */
	errChk (DisplayPanel (panelHandle));
	errChk (DisplayPanel (panelHandle_IN));

	sprintf (buffer, "Read Parameters\n");
	SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, buffer);
	sprintf (buffer, "Read in undulator definition from %s\n", und_prm_file);
	SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, buffer);

	// Read in data directory
	fpi = fopen (und_prm_file, "r");
	fgets (und_dir, FILENAME, fpi);
	sprintf (buffer, "Data Directory: %s\n", und_dir);
	SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, buffer);
	fclose (fpi);
	
	// Read parameters
	sprintf (buffer, "Read in parameters from %s and setup Parameter Panel\n", param_file);
	SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, buffer);
	RdParamFile (param_file);
	
	// Set nominal center position
	X_Scan = x_scan;
	Y_Scan = y_scan;
	
	// Get z position for reference magnet
	fpi = fopen (ref_mag_prm_file, "r");
	fgets (buffer, LINE, fpi);
	while (buffer[0] == '#') 
		fgets (buffer, LINE, fpi);
	sscanf (buffer, "%lf", &Z_RefMag);
	fclose (fpi);
	
	SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_X, x_scan);
	SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_Y, y_scan);
	SetCtrlVal (panelHandle, PANEL_NUMERIC_SC_Z1, Sc_strt);
	SetCtrlVal (panelHandle, PANEL_NUMERIC_SC_Z2, Sc_stp);
	sprintf (buffer, "GPIB Addresses: %d, %d\n", GPIBDVM[0], GPIBDVM[1]);
	SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, buffer);
	sprintf (buffer, "Hall Calibration File: %s\n", HP_cal_file);
	SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, buffer);
	
	sprintf (buffer, "\nBEGIN HARDWARE INITIALIZATION BY HITTING THE BUTTON AT THE TOP OF PANEL\n\n");
	SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, buffer);

	errChk (RunUserInterface ());

Error:
	/* clean up */
	if (panelHandle > 0)
		DiscardPanel (panelHandle);
		DiscardPanel (panelHandle_IN);
	return 0;
}

//==============================================================================
// UI callback function prototypes

/// HIFN Exit when the user dismisses the panel.
int CVICALLBACK panelCB (int panel, int event, void *callbackData,
		int eventData1, int eventData2)
{
	BOOL res;
	double xm;
	if (event == EVENT_CLOSE)  {
		// Retract probe
		xm = X0/1000;
		res = umac_abs_move(umac_ID, 3, UMAC_AXIS3_ACCELERATION, UMAC_AXIS3_VELOCITY, xm);   
		umac_exit(0);   
		QuitUserInterface (0);
	}
	return 0;
}

int CVICALLBACK Initialize (int panel, int control, int event,
							void *callbackData, int eventData1, int eventData2)
							
{
	int resp = 0;
	double xm, ym;
	unsigned int move = 0;
	BOOL res;
	char StatBuffer[LINE];
	
	switch (event)
	{
		case EVENT_COMMIT:  // Begin initializing hardware
			// Turn off LED timer
			SuspendTimerCallbacks ();
			// Turn off LED
			SetCtrlVal (panelHandle_IN, IN_PANEL_LED, 0);
			
			
			// Initialize HP3458s
			sprintf (StatBuffer, "Initialize HP3458 @ GPIB:%d.\n", GPIBDVM[0]);
			SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			hp3458_init (0, GPIBDVM[0], &hp3458_ID[0]);	//Bx
			sprintf (StatBuffer, "Initialize HP3458 @ GPIB:%d.\n\n", GPIBDVM[1]);
			SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			hp3458_init (0, GPIBDVM[1], &hp3458_ID[1]);	//By

			// Initialize Kugler controls
			sprintf (StatBuffer, "Initialize Kugler Controls.\n");
			SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			umac_data_fill_param_struct(nParam); 
		    UMAC_ID = umac_init(0, &ID, nParam);
			if (UMAC_ID == -1) { // Shutdown program for restart  
				QuitUserInterface (0);
				return -1;
			}
			umac_ID=ID;
			// Acquire initial Kugler axes position
			sprintf (StatBuffer, "Determine Axes Positions.\n");
			SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			if (umac_get_mot_position(umac_ID, 1, &posz)) {  //position in [m]
				ZL = posz*1000;	// set last position indicator for z
				sprintf (StatBuffer, "Motor 1 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
				SetCtrlVal (panelHandle, PANEL_NUMERIC_PZ, ZL);
			}
			if (umac_get_mot_position(umac_ID, 2, &posy)) {   //position in [m]
				sprintf (StatBuffer, "Motor 2 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			}
			if (umac_get_mot_position(umac_ID, 3, &posx)) {   //position in [m]
				sprintf (StatBuffer, "Motor 3 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			}
			if (umac_get_mot_position(umac_ID, 5, &posrx)) {   //position in [m]
				RXL = posrx*1000;	// set last position indicator for rx
				sprintf (StatBuffer, "Motor 5 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
				SetCtrlVal (panelHandle, PANEL_NUMERIC_RX, RXL);
			}
			if (umac_get_mot_position(umac_ID, 8, &posry)) {   //position in [m] 
				RYL = posry*1000;	// set last position indicator for ry
				sprintf (StatBuffer, "Motor 8 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
				SetCtrlVal (panelHandle, PANEL_NUMERIC_RY, RYL);
			}
			if (umac_get_mot_position(umac_ID, 9, &posrz)) {   //position in [m]
				RZL = posrz*1000;	// set last position indicator for rz
				sprintf (StatBuffer, "Motor 9 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
				SetCtrlVal (panelHandle, PANEL_NUMERIC_RZ, RZL);
			}
			if (umac_get_mot_position(umac_ID, 6, &poswz)) {   //position in [m]  
				sprintf (StatBuffer, "Motor 6 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			}
			if (umac_get_mot_position(umac_ID, 7, &posvy)) {   //position in [m] 
				sprintf (StatBuffer, "Motor 7 is Ready.\n");
				SetCtrlVal (panelHandle_IN, IN_PANEL_TEXTBOX_STATUS, StatBuffer);
			}
			posz=posz*1000;   posy=posy*1000;  posx=posx*1000; //convert position to [mm] 
			
			sprintf (StatBuffer, "\nBEFORE RUNNING SCAN\n");
			SetCtrlVal (panelHandle, PANEL_TEXTBOX, StatBuffer);
			sprintf (StatBuffer, "MAKE SURE x AND y PROBE POSITIONS ARE SET TO CLEAR OBSTACLES\n\n");
			SetCtrlVal (panelHandle, PANEL_TEXTBOX, StatBuffer);
			
			// Read calibration file
			RdCalFile (HP_cal_file);

			// Get operator
			if (!operator_set) {
				PromptPopup ("Operator", "Must Specify Operator", operator, NAME-1);
				operator_set = TRUE;
			}
			
			// Prompt to proceed
			resp = ConfirmPopup ("Continue Program", "Proceed?");
			if (resp) {
				DiscardPanel (panelHandle_IN);
				DisplayPanel (panelHandle);
				// Start timer
				ResumeTimerCallbacks ();
				
			}
			
			// Go to nominal x positions for scan
			SetCtrlVal (panelHandle, PANEL_TEXTBOX, "Move probe to scan x position\n");  
			xm = X_Scan/1000;
			XL = X_Scan;	// set last position indicator for x
			res = umac_abs_move(umac_ID, 3, UMAC_AXIS3_ACCELERATION, UMAC_AXIS3_VELOCITY, xm);
			Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 3);
			while (move) {
				Delay(1.0);
				move = umac_AxisIsMoving(umac_ID, 3);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_X, X_Scan);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_PX, X_Scan);
			
			// Go to nominal y position for scan
			SetCtrlVal (panelHandle, PANEL_TEXTBOX, "Move probe to scan y position\n");  
			ym = Y_Scan/1000;
			YL = Y_Scan;	// set last position indicator for y
			res = umac_abs_move(umac_ID, 2, UMAC_AXIS2_ACCELERATION, UMAC_AXIS2_VELOCITY, ym);
			Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 2);
			while (move) {
				Delay(1.0);
				move = umac_AxisIsMoving(umac_ID, 2);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_Y, Y_Scan);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_PY, Y_Scan);

			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Init_timer (int panel, int control, int event,
							void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_TIMER_TICK:
			SetCtrlVal (panelHandle_IN, IN_PANEL_LED, LED_toggle);
			if (LED_toggle)
				LED_toggle = FALSE;
			else
				LED_toggle = TRUE;

			break;
	}
	return 0;
}

int CVICALLBACK Begin (int panel, int control, int event,
					   void *callbackData, int eventData1, int eventData2)
{
	unsigned int scan, nhscans, nvscans;
	unsigned int i, npks;
	double *xp, *pkx;
	double *yp, *pky;
	double zp[NPMX], Bp[NPMX];
	double fltr;
	float x0, y0;
	double xm, ym;
	double dq, xq[21], yq[21];
	float dx, dy;
	double c[4];
	double new_x0, new_y0;
	BOOL res;
	unsigned int ln;
	unsigned int move = 0;
	char buffer[LINE];
	char file_pk[LINE];
	FILE *fpk;

	switch (event)
	{
		case EVENT_COMMIT:
			
			SuspendTimerCallbacks ();
			
			GetCtrlVal (panelHandle, PANEL_NUMERIC_FLTR, &fltr);
			GetCtrlVal (panelHandle, PANEL_NUMERIC_NUMX, &nhscans);
			GetCtrlVal (panelHandle, PANEL_NUMERIC_NUMY, &nvscans);
			
			xp = calloc (nhscans, sizeof(double));
			pkx = calloc (nhscans, sizeof(double));
			yp = calloc (nvscans, sizeof(double));
			pky = calloc (nvscans, sizeof(double));

			// Horizontal Scans
			if (Do_HScans) {
				SetCtrlVal (panelHandle, PANEL_TEXTBOX, "Begin Horizontal Scans\n");  
				GetCtrlVal (panelHandle, PANEL_NUMERIC_NUMX, &nhscans);
			
			// Go to nominal y position for scan
			SetCtrlVal (panelHandle, PANEL_TEXTBOX, "Move probe to scan y position\n");  
			ym = Y_Scan/1000;
			YL = Y_Scan;	// set last position indicator for y
			res = umac_abs_move(umac_ID, 2, UMAC_AXIS2_ACCELERATION, UMAC_AXIS2_VELOCITY, ym);
			Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 2);
			while (move) {
				Delay(1.0);
				move = umac_AxisIsMoving(umac_ID, 2);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_Y, Y_Scan);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_PY, Y_Scan);
			GetCtrlVal (panelHandle, PANEL_NUMERIC_DX, &dx);

			x0 = X_Scan - dx*(nhscans - 1)/2.0;
			
			// Clear graph
			DeleteGraphPlot (panelHandle, PANEL_GRAPH, -1, VAL_IMMEDIATE_DRAW);
			
			for (scan = 0, x_scan = x0; scan < nhscans; scan++, x_scan += dx) {
				// Move to scan x position
				sprintf (buffer, "Move to x = %7.3f, for scan %u\n", x_scan, scan);
				SetCtrlVal (panelHandle, PANEL_TEXTBOX, buffer);  
				xm = x_scan/1000;
				xp[scan] = x_scan;
				XL = x_scan;
				res = umac_abs_move(umac_ID, 3, UMAC_AXIS3_ACCELERATION, UMAC_AXIS3_VELOCITY, xm);
				Delay(0.5);
				move = umac_AxisIsMoving(umac_ID, 3);
				while (move) {
					Delay(1.0);
					move = umac_AxisIsMoving(umac_ID, 3);
				}
				SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_X, x_scan);
       			SetCtrlVal (panelHandle, PANEL_NUMERIC_PX, x_scan);
				ZScan();
				
				// Plot data
				num_pnts1 = num_pnts - 1;
				if (plot_num >= NCOLORS)
					plot_num = 0;
					PlotXY (panelHandle, PANEL_GRAPH, z, By, num_pnts1, VAL_DOUBLE, VAL_DOUBLE, VAL_THIN_LINE, 
							VAL_EMPTY_SQUARE, VAL_SOLID, 1, colors[plot_num]);
					plot_num++;

					// Create data file name
					ln = strlen (und_dir);
					ln--;	// last character is EOL - don't copy
					for (i = 0; i < ln; i++)
						und_file[i] = und_dir[i];
					und_file[i] = '\0'; 
					sprintf (buffer, "h%u", scan);
					strcat (und_file, buffer);
					strcat (und_file, ".dat");
					// Write to data file
					WriteDataFile ();
	
					findPeaks (z, By, num_pnts, fltr, zp, Bp, &npks);
					// Calculate average peak value
					for (i = 0, pkx[scan] = 0.0; i < npks; i++)
						pkx[scan] += fabs(Bp[i]);
					pkx[scan] /= npks;
				
					// write peak values to file
					strncpy (file_pk, " ", LINE);
					for (i = 0; und_file[i] != '.'; i++)
						file_pk[i] = und_file[i];
					strcat (file_pk, ".pk");
					fpk = fopen (file_pk, "w");
					fprintf (fpk, "%u  %f  %f\n", npks, xp[scan], pkx[scan]);
					for (i = 0; i < npks; i++)
						fprintf (fpk, "%f  %f\n", zp[i], Bp[i]);
					fclose (fpk);
				
					// Plot peak vs. x
					PlotXY (panelHandle, PANEL_GRAPH_PKX, &xp[scan], &pkx[scan], 1, VAL_DOUBLE, VAL_DOUBLE, 
							VAL_SCATTER, VAL_EMPTY_SQUARE, VAL_SOLID, 1, VAL_RED);
				
				}
 
				// Find max
				LS_Quad (xp, pkx, nhscans, c);
				new_x0 = c[3];
				sprintf (buffer, "Maximum found at %8.3f\n", new_x0);
				SetCtrlVal (panelHandle, PANEL_TEXTBOX, buffer);
				SetCtrlVal (panelHandle, PANEL_NUMERIC_X0, new_x0);
				// Plot Quad fit
				dq = (xp[nhscans-1] - xp[0])/20;
				for (i = 0; i < 21; i++) {
					xq[i] = xp[0] + i*dq;
					yq[i] = c[0] + c[1]*xq[i] + c[2]*xq[i]*xq[i];
				}
				PlotXY (panelHandle, PANEL_GRAPH_PKX, xq, yq, 21, VAL_DOUBLE, VAL_DOUBLE, 
						VAL_THIN_LINE, VAL_EMPTY_SQUARE, VAL_SOLID, 1, VAL_RED);
			}
			
			// Vertical Scans
			if (Do_VScans) {
				SetCtrlVal (panelHandle, PANEL_TEXTBOX, "\nBegin Vertical Scans\n");  
				GetCtrlVal (panelHandle, PANEL_NUMERIC_NUMY, &nvscans);
 				GetCtrlVal (panelHandle, PANEL_NUMERIC_DY, &dy);

			// Go to nominal x positions for scan
			SetCtrlVal (panelHandle, PANEL_TEXTBOX, "Move probe to scan x position\n");  
			xm = X_Scan/1000;
			XL = X_Scan;	// set last position indicator for x
			res = umac_abs_move(umac_ID, 3, UMAC_AXIS3_ACCELERATION, UMAC_AXIS3_VELOCITY, xm);
			Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 3);
			while (move) {
				Delay(1.0);
				move = umac_AxisIsMoving(umac_ID, 3);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_X, X_Scan);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_PX, X_Scan);
			y0 = Y_Scan - dy*(nvscans - 1)/2.0;

			// Clear graph
			DeleteGraphPlot (panelHandle, PANEL_GRAPH, -1, VAL_IMMEDIATE_DRAW);
			plot_num = 0;
			
			for (scan = 0, y_scan = y0; scan < nvscans; scan++, y_scan += dy) {
				// Move to scan y position
				sprintf (buffer, "Move to y = %7.3f, for scan %u\n", y_scan, scan);
				SetCtrlVal (panelHandle, PANEL_TEXTBOX, buffer);  
				ym = y_scan/1000;
				yp[scan] = y_scan;
				YL = y_scan;
				res = umac_abs_move(umac_ID, 2, UMAC_AXIS2_ACCELERATION, UMAC_AXIS2_VELOCITY, ym);
				Delay(0.5);
				move = umac_AxisIsMoving(umac_ID, 2);
				while (move) {
					Delay(1.0);
					move = umac_AxisIsMoving(umac_ID, 2);
				}
				SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_Y, y_scan);
       			SetCtrlVal (panelHandle, PANEL_NUMERIC_PY, y_scan);
				ZScan();

				// Plot data
				num_pnts1 = num_pnts - 1;
				if (plot_num >= NCOLORS)
					plot_num = 0;
					PlotXY (panelHandle, PANEL_GRAPH, z, By, num_pnts1, VAL_DOUBLE, VAL_DOUBLE, VAL_THIN_LINE, 
							VAL_EMPTY_SQUARE, VAL_SOLID, 1, colors[plot_num]);
					plot_num++;

					// Create data file name
					ln = strlen (und_dir);
					ln--;	// last character is EOL - don't copy
					for (i = 0; i < ln; i++)
						und_file[i] = und_dir[i];
					und_file[i] = '\0'; 
					sprintf (buffer, "v%u", scan);
					strcat (und_file, buffer);
					strcat (und_file, ".dat");
					// Write to data file
					WriteDataFile ();
	
					findPeaks (z, By, num_pnts, fltr, zp, Bp, &npks);
					// Calculate average peak value
					for (i = 0, pky[scan] = 0.0; i < npks; i++)
						pky[scan] += fabs(Bp[i]);
					pky[scan] /= npks;
				
					// write peak values to file
					strncpy (file_pk, " ", LINE);
					for (i = 0; und_file[i] != '.'; i++)
						file_pk[i] = und_file[i];
					strcat (file_pk, ".pk");
					fpk = fopen (file_pk, "w");
					fprintf (fpk, "%u  %f  %f\n", npks, yp[scan], pky[scan]);
					for (i = 0; i < npks; i++)
						fprintf (fpk, "%f  %f\n", zp[i], Bp[i]);
					fclose (fpk);
					// Plot peak vs. y
					PlotXY (panelHandle, PANEL_GRAPH_PKY, &yp[scan], &pky[scan], 1, VAL_DOUBLE, VAL_DOUBLE, VAL_SCATTER, 
							VAL_EMPTY_SQUARE, VAL_SOLID, 1, VAL_RED);
				}

				// Find min
				LS_Quad (yp, pky, nvscans, c);
				new_y0 = c[3];
				sprintf (buffer, "Minimum found at %8.3f\n", new_y0);
				SetCtrlVal (panelHandle, PANEL_TEXTBOX, buffer);
				SetCtrlVal (panelHandle, PANEL_NUMERIC_Y0, new_y0);
				// Plot Quad fit
				dq = (yp[nvscans-1] - yp[0])/20;
				for (i = 0; i < 21; i++) {
					xq[i] = yp[0] + i*dq;
					yq[i] = c[0] + c[1]*xq[i] + c[2]*xq[i]*xq[i];
				}
				PlotXY (panelHandle, PANEL_GRAPH_PKY, xq, yq, 21, VAL_DOUBLE, VAL_DOUBLE, 
						VAL_THIN_LINE, VAL_EMPTY_SQUARE, VAL_SOLID, 1, VAL_RED);
			}
			
			free (xp);
			free (pkx);
			free (yp);
			free (pky);
			
			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

void ZScan (void)
{
	int	i;
	double trigval[5]={trig_axis,trig_spc,Sc_strt,Sc_stp,Sc_V};
	char umacCom[120];
	char umacRes[100];
	double maxPulse=-2;
	double Vx_ave, Vy_ave, Vx_rms, Vy_rms;
	int velval;
	FILE *fpi;
	
	// Move to start location
	// Setup triggering
	SetCtrlVal (panelHandle, PANEL_TEXTBOX, "Go to start position and do scan\n");  
	hp3458_setup_triggered_voltage_samples (hp3458_ID[0], DCV[0], int_tm, num_pnts);	// Bx
	hp3458_setup_triggered_voltage_samples (hp3458_ID[1], DCV[1], int_tm, num_pnts);	// By
	
	for (i = 0; i < num_pnts; i++) {
		z[i]=(Sc_strt+i*trig_spc)/1000.0;	// values in m
	}

	// Start motion
	sprintf(umacCom,"q0=%f",trigval[0]); //set the trigger source      (Kugler manual p.60)
 	umac_command(umac_ID, umacRes, 0, umacCom);

 	sprintf(umacCom,"q1=%f",trigval[1]); //set the trigger pitch in mm (Kugler manual p.60)
 	umac_command(umac_ID, umacRes, 0, umacCom);

 	sprintf(umacCom,"p1=%f",trigval[2]); //set init position of the carriage (motion program 8)
 	umac_command(umac_ID, umacRes, 0, umacCom);  

 	sprintf(umacCom,"p2=%f",trigval[3]);
 	umac_command(umac_ID, umacRes, 0, umacCom); //set final position of the carriage (motion program 8)
 	velval=floor(trigval[0]);
 	trigval[4]=trigval[4]*20;
 	sprintf(umacCom,"i%i22=%f",velval,trigval[4]);
 	umac_command(umac_ID, umacRes, 0, umacCom); //set the trigger process velocity (motion program 8)

 	umac_command(umac_ID, umacRes, 0, "q10=1"); //set the flexible start pos var to 1 (enable)

 	sprintf(umacCom,"q11=%f",trigval[2]);
 	umac_command(umac_ID, umacRes, 0, umacCom); // set the position of the 1st trigger

 	maxPulse=fabs(floor((trigval[3]-trigval[2])/trigval[1]));

 	sprintf(umacCom,"q13=%f",maxPulse);
 	umac_command(umac_ID, umacRes, 0, umacCom); // set the max number of trigger pulses

 	umac_command(umac_ID, umacRes, 0, "q14=0"); //select the trigger edge 0=rising edge  

 	umac_command(umac_ID, umacRes, 0, "b8r");	   //launch triggering process
 // Delay(30.0);
	  
  // Start scan triggers
	hp3458_start_triggered_voltage_samples (hp3458_ID[0]); 	// Bx
	hp3458_start_triggered_voltage_samples (hp3458_ID[1]); 	// By
	Delay(1.0);
	// Retrieve data
	hp3458_collect_triggered_voltage_samples (hp3458_ID[0], num_pnts, Vx);	// Bx
	hp3458_collect_triggered_voltage_samples (hp3458_ID[1], num_pnts, Vy);	// By
	Delay(1.0);

	// Assume no zero-Gauss reading
	fpi = fopen (zgauss2_file, "r");
 	ReadZGauss (fpi, &Vx_ave, &Vy_ave, &Vx_rms, &Vy_rms);
	fclose (fpi);
	
	// Convert to Bx, By
	// Get V offset correction - average first and second readings
	Vx_offset = Vx_ave;
	Vy_offset = Vy_ave;
	for (i = 0; i < num_pnts; i++) {
		Vx_corr[i] = Vx[i] - Vx_offset;
		Vy_corr[i] = Vy[i] - Vy_offset;
	}
	for (i = 0; i < num_pnts; i++) {
		Bx[i] = HP_Calibrate (Vx_corr[i], HP_cal_nx, HP_cal_px, ncalx);
		By[i] = HP_Calibrate (Vy_corr[i], HP_cal_ny, HP_cal_py, ncaly);
	}
	
	return;
}
	

int CVICALLBACK Set_x (int panel, int control, int event,
					   void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_X, x_scan);

			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Set_y (int panel, int control, int event,
					   void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			SetCtrlVal (panelHandle, PANEL_NUMERIC_ST_Y, y_scan);

			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Set_z1 (int panel, int control, int event,
						void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			GetCtrlVal (panelHandle, PANEL_NUMERIC_SC_Z1, &Sc_strt);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_SC_Z1, Sc_strt);

			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Set_z2 (int panel, int control, int event,
						void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			GetCtrlVal (panelHandle, PANEL_NUMERIC_SC_Z2, &Sc_stp);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_SC_Z2, Sc_stp);

			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Quit (int panel, int control, int event,
					  void *callbackData, int eventData1, int eventData2)
{
	BOOL res;
	double xm;

	switch (event)
	{
		case EVENT_COMMIT:

			// Retract probe
			xm = X0/1000;
			res = umac_abs_move(umac_ID, 3, UMAC_AXIS3_ACCELERATION, UMAC_AXIS3_VELOCITY, xm);   
			umac_exit(0);   
			QuitUserInterface (0);
			break;
	}
	return 0;
}

int findPeaks (double z[], double B[], unsigned int npts, double fltr, double zp[], double Bp[], unsigned int *pnpks)
{
	unsigned int i, np;
	double Bmax, Bt;
	double *y2, *Bd;
	double dz, dBd;
	
	// Find peak 
	for (i = 1, Bmax = fabs(B[0]); i < npts; i++)
		if ((Bt = fabs(B[i])) > Bmax)
			Bmax = Bt;
	fltr *= Bmax;
	
	y2 = calloc (npts, sizeof(double));
	Bd = calloc (npts, sizeof(double));
	// Calculate B'
	spline (z, B, npts, y2);
	spld1 (z, B, y2, npts, z, Bd, npts);
	
	// Search for peaks of B as zero-crossings of B'
	for (i = 0, np = 0; i < npts-1; i++) {
		if ( ((Bt = fabs(B[i]))) > fltr && (Bd[i]*Bd[i+1] < 0.0) ) {	// found a peak
			dz = z[i+1] - z[i];
			dBd = Bd[i+1] - Bd[i];
			zp[np] = z[i] - Bd[i]*dz/dBd;	// get location by linear interpolation
			np++;
			if (np >= NPMX)
				return 1;
		}
	}
	
	// Calculate peak values
	splint (z, B, y2, npts, zp, Bp, np);
	*pnpks = np;
	
	free (y2);
	free (Bd);
	
	return 0;
}

void LS_Quad (double x[], double y[], unsigned npts, double c[])
{
	unsigned int i;
	double a00, a01, a02, b0;
	double a10, a11, a12, b1;
	double a20, a21, a22, b2;
	double xi, xi2, xi3, xi4;
	double yi, yixi, yixi2;
	double fact;
	
	xi = xi2 = xi3 = xi4 = yi = yixi = yixi2 = 0.0;
	for (i = 0; i < npts; i++) {
		xi += x[i];
		xi2 += x[i]*x[i];
		xi3 += x[i]*x[i]*x[i];
		xi4 += x[i]*x[i]*x[i]*x[i];
		yi += y[i];
		yixi += y[i]*x[i];
		yixi2 += y[i]*x[i]*x[i];
	}
	a00 = npts;
	a01 = a10 = xi;
	a02 = a11 = a20 = xi2;
	a12 = a21 = xi3;
	a22 = xi4;
	b0 = yi;
	b1 = yixi;
	b2 = yixi2;
	
	// matrix reduction #1
	fact = a00/a10;
	a11 = fact*a11 - a01;
	a12 = fact*a12 - a02;
	b1 = fact*b1 - b0;
	fact = a00/a20;
	a21 = fact*a21 - a01;
	a22 = fact*a22 - a02;
	b2 = fact*b2 - b0;
	
	// matrix reduction #2
	fact = a11/a21;
	a22 = fact*a22 - a12;
	b2 = fact*b2 - b1;
	
	c[2] = b2/a22;
	c[1] = (b1 - a12*c[2])/a11;
	c[0] = (b0 - a01*c[1] - a02*c[2])/a00;
	c[3] = -c[1]/2/c[2];
	
	return;
}


int CVICALLBACK Mod_PFile (int panel, int control, int event,
						   void *callbackData, int eventData1, int eventData2)
{
	unsigned short i;
	double x0, y0;
	char buffer[LINE];
	char tfile[LINE];
	FILE *fpi, *fpt;
	switch (event)
	{
		case EVENT_COMMIT:

			GetCtrlVal (panelHandle, PANEL_NUMERIC_X0, &x0);
			GetCtrlVal (panelHandle, PANEL_NUMERIC_Y0, &y0);
	
			fpi = fopen (PRMFILE, "r");
			sprintf (tfile, "%s.m", PRMFILE); 
			fpt = fopen (tfile, "w");
	
			// skip over first 14 lines
			for (i = 0; i < 14; i++) {	
				fgets (buffer, LINE, fpi);
				fprintf (fpt, "%s", buffer);
			}
	
			// get x, write new value
			fgets (buffer, LINE, fpi);
			fprintf (fpt, "%.3f		# x position for scan\n", x0);
			// get y, write new value
			fgets (buffer, LINE, fpi);
			fprintf (fpt, "%.3f		# y position for scan\n", y0);
	
			// get next 11 lines
			for (i = 0; i < 11; i++) {	
				fgets (buffer, LINE, fpi);
				fprintf (fpt, "%s", buffer);
			}
	
			fclose (fpi);
			fclose (fpt);
	
			break;
	}
	return 0;
}


int CVICALLBACK Move_Probe (int panel, int control, int event,
							  void *callbackData, int eventData1, int eventData2)
{
	float drx, dry, drz;
	float rxm, rym, rzm;
	float dx, dy, dz;
	float xm, ym, zm;
	double vx, vy, bx, by;
	BOOL res;
	unsigned int move = 0;

	switch (event)
	{
		case EVENT_COMMIT:
			GetCtrlVal (panelHandle, PANEL_NUMERIC_DPX, &dx);	// Get specified x in mm
			GetCtrlVal (panelHandle, PANEL_NUMERIC_DPY, &dy);	// Get specified y in mm
			GetCtrlVal (panelHandle, PANEL_NUMERIC_DPZ, &dz);	// Get specified z in mm
			xm = (XL + dx)/1000;
			res = umac_abs_move(umac_ID, 3, UMAC_AXIS3_ACCELERATION, UMAC_AXIS3_VELOCITY, xm);
      Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 3);
			while (move) {
				Delay(0.1);
				move = umac_AxisIsMoving(umac_ID, 3);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_PX, XL+dx);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_DPX, 0.0);
			XL += dx;

      ym = (YL + dy)/1000; 
			res = umac_abs_move(umac_ID, 2, UMAC_AXIS2_ACCELERATION, UMAC_AXIS2_VELOCITY, ym);
      Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 2);
			while (move) {
				Delay(0.1);
				move = umac_AxisIsMoving(umac_ID, 2);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_PY, YL + dy);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_DPY, 0.0);
			YL += dy;

			zm = (ZL + dz)/1000;
      Delay(0.5);
			res = umac_abs_move(umac_ID, 1, UMAC_AXIS1_ACCELERATION, UMAC_AXIS1_VELOCITY, zm);    
			move = umac_AxisIsMoving(umac_ID, 1);
			while (move) {
				Delay(0.1);
				move = umac_AxisIsMoving(umac_ID, 1);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_PZ, ZL+dz);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_DPZ, 0.0);
			ZL += dz;

			GetCtrlVal (panelHandle, PANEL_NUMERIC_DRX, &drx);	// Get specified drx in mrad
			GetCtrlVal (panelHandle, PANEL_NUMERIC_DRY, &dry);	// Get specified dry in mrad
			GetCtrlVal (panelHandle, PANEL_NUMERIC_DRZ, &drz);	// Get specified drz in mrad

      		rxm = (RXL+drx)/1000; 
			res = umac_abs_move(umac_ID, 5, UMAC_AXIS5_ACCELERATION, UMAC_AXIS5_VELOCITY, rxm);
      Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 5);
			while (move) {
				Delay(0.1);
				move = umac_AxisIsMoving(umac_ID, 5);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_RX, RXL+drx);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_DRX, 0.0);
			RXL += drx;

			rym = (RYL+dry)/1000;  
			res = umac_abs_move(umac_ID, 8, UMAC_AXIS8_ACCELERATION, UMAC_AXIS8_VELOCITY, rym); 
      Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 8);
			while (move) {
				Delay(0.1);
				move = umac_AxisIsMoving(umac_ID, 8);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_RY, RYL+dry);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_DRY, 0.0);
			RYL += dry;

      rzm = (RZL+drz)/1000;  
			res = umac_abs_move(umac_ID, 9, UMAC_AXIS9_ACCELERATION, UMAC_AXIS9_VELOCITY, rzm);
      Delay(0.5);
			move = umac_AxisIsMoving(umac_ID, 9);
			while (move) {
				Delay(0.1);
				move = umac_AxisIsMoving(umac_ID, 9);
			}
			SetCtrlVal (panelHandle, PANEL_NUMERIC_RZ, RZL+drz);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_DRZ, 0.0);
			RZL += drz;


			// Measure votages	
			hp3458_get_voltage(hp3458_ID[0], &vx);
			hp3458_get_voltage(hp3458_ID[1], &vy);
			// Convert to B
			bx = HP_Calibrate (vx, HP_cal_nx, HP_cal_px, ncalx);
			by = HP_Calibrate (vy, HP_cal_ny, HP_cal_py, ncaly);
			// Display Bx and By
			SetCtrlVal (panelHandle, PANEL_NUMERIC_BX, bx);
			SetCtrlVal (panelHandle, PANEL_NUMERIC_BY, by);

			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Move_to_RefMag (int panel, int control, int event,
								void *callbackData, int eventData1, int eventData2)
{
	double zm;
	BOOL res;
	unsigned int move = 0;
	switch (event)
	{
		case EVENT_COMMIT:
			zm = Z_RefMag/1000;
			res = umac_abs_move(umac_ID, 1, UMAC_AXIS1_ACCELERATION, UMAC_AXIS1_VELOCITY, zm);
			move = umac_AxisIsMoving(umac_ID, 1);
			while (move) {
				Delay(0.1);
				move = umac_AxisIsMoving(umac_ID, 1);
			}

			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Set_HScans (int panel, int control, int event,
							void *callbackData, int eventData1, int eventData2)
{
	unsigned int on;
	switch (event)
	{
		case EVENT_COMMIT:
			GetCtrlVal (panelHandle, PANEL_BINARYSWITCH_HS, &on);
			if (on) {
				Do_HScans = TRUE;
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_NUMX, ATTR_DIMMED, 0);
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_DX, ATTR_DIMMED, 0);
				SetCtrlAttribute (panelHandle, PANEL_GRAPH_PKX, ATTR_DIMMED, 0);
			}
			else {
				Do_HScans = FALSE;
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_NUMX, ATTR_DIMMED, 1);
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_DX, ATTR_DIMMED, 1);
				SetCtrlAttribute (panelHandle, PANEL_GRAPH_PKX, ATTR_DIMMED, 1);
			}
			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}

int CVICALLBACK Set_VScans (int panel, int control, int event,
							void *callbackData, int eventData1, int eventData2)
{
	unsigned int on;
	switch (event)
	{
		case EVENT_COMMIT:
			GetCtrlVal (panelHandle, PANEL_BINARYSWITCH_VS, &on);
			if (on) {
				Do_VScans = TRUE;
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_NUMY, ATTR_DIMMED, 0);
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_DY, ATTR_DIMMED, 0);
				SetCtrlAttribute (panelHandle, PANEL_GRAPH_PKY, ATTR_DIMMED, 0);
			}
			else {
				Do_VScans = FALSE;
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_NUMY, ATTR_DIMMED, 1);
				SetCtrlAttribute (panelHandle, PANEL_NUMERIC_DY, ATTR_DIMMED, 1);
				SetCtrlAttribute (panelHandle, PANEL_GRAPH_PKY, ATTR_DIMMED, 1);
			}
			break;
		case EVENT_RIGHT_CLICK:

			break;
	}
	return 0;
}