#include <analysis.h>

/*	**************************************************************  */
/*
 *	Module WIRPOS
 *	This module contains functions for automating measurements with
 *	wire position detectors.  The detector is moved until its signal
 *	is centered on the wire.  A coordinate measuring machine, or some
 *	other device, then locates tooling balls on the detector.  This is
 *	used to get the wire position without touching the wire.
 *
 *	Zachary Wolf
 *	6/3/05
 */ 

/*	**************************************************************  */

/*	INCLUDES  */
#include <utility.h>
#include <gpib.h>
#include <formatio.h>
#include <ansi_c.h>
#include "wirpos.h"
#include "wirposui.h"
#include "esp7000.h"
#include "hp34970_sw.h"

/*	**************************************************************  */

/*  PRIVATE FUNCTIONS  */
void wirpos_message(char* msg);
void wirpos_error(char* msg);
void wirpos_log_device_type(enum wirpos_device_type_enum device_type);
void wirpos_log_move_home(int det_num);
void wirpos_log_rel_move(int det_num, double dis);
void wirpos_log_abs_move(int det_num, double pos);
void wirpos_log_get_pos(int det_num, double pos);
void wirpos_log_get_volt(int det_num, double v);
void wirpos_log_set_zero_pos(int det_num);

/*	**************************************************************  */

/*  PRIVATE GLOBAL VARIABLES  */
static int esp7000_ID;
static int wirpos_hp34970_ID;
static double nominal_position[WIRPOS_MAX_NUM_WIR_DET];
static char cmd[WIRPOS_MAX_NUM_CHAR];
static char msg[WIRPOS_MAX_NUM_CHAR];

/*	**************************************************************  */

/*  PRIVATE PARAMETERS  */
static char log_file[100];
static struct wirpos_param_struct wirpos_param;

/*	**************************************************************  */

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  wirpos_init
 *  This function opens the device, queries for ID, and
 *  initializes the device to a known state.
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_init(char log_file_in[], struct wirpos_param_struct wirpos_param_in)
{
	/* Declare variables */
	int i;
	
	/* Save the module parameters */
	strcpy(log_file, log_file_in);
	wirpos_param = wirpos_param_in;
	
	/* Initialize the appropriate devices */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		hp34970_init(wirpos_param.board_addr, wirpos_param.hp34970_addr, &wirpos_hp34970_ID);
		esp7000_init(wirpos_param.board_addr, wirpos_param.esp7000_addr, &esp7000_ID);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		hp34970_init(wirpos_param.board_addr, wirpos_param.hp34970_addr, &wirpos_hp34970_ID);
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
	
	/* Move each detector to its home position */
	if (wirpos_param.device_type != WIRPOS_NONE_NONE)
	{
		printf("\nMove detectors to their home position:\n");
		for (i = 0; i < wirpos_param.num_wire_det; i++) wirpos_move_home(i);
		    
		//Turn off power to the active axes
    	for (i = 1; i <= ESP7000_MAX_NUM_AXES; i++)
			 esp7000_deactivate_axis(esp7000_ID, i);
	}
	
	/* Log the device type */
	wirpos_log_device_type(wirpos_param.device_type);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_manual_move_to_wire
 *  This function requests the operator to move the stages to the position
 *	where the detector is roughly centered on the wire.  The DMM monitors
 *	the detector output to aid in moving.
 *
 *	Input:
 *	det_num, detector number (0, 1, 2, ...)
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_manual_move_to_wire(int det_num)
{
	/* Declare variables */
	char buf[80];
	
	/* Check input parameters */
    if (det_num < 0 || det_num >= wirpos_param.num_wire_det)
    {
    	Fmt(msg, "%s<det_num = %i, out of range", det_num);
    	wirpos_error(msg);
    	return;
    }
    
    /* Have the DMM monitor the detector output */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		hp34970_monitor_chan(wirpos_hp34970_ID, wirpos_param.det_info[det_num].meas_channel, 'v');
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		hp34970_monitor_chan(wirpos_hp34970_ID, wirpos_param.det_info[det_num].meas_channel, 'v');
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
    
    /* Have the user move to the wire position */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970 || wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
    	printf("\nManual Move To Wire\n");
    	printf("Please move detector %i (%s, axis %i) to the wire position.\n", det_num, wirpos_param.det_info[det_num].name, wirpos_param.det_info[det_num].move_axis);
    	printf("Press ENTER when ready.");
    	fgets(buf, 80, stdin);
    }
    
    /* Turn off the DMM monitoring */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		hp34970_monitor_off(wirpos_hp34970_ID);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		hp34970_monitor_off(wirpos_hp34970_ID);
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_move_home
 *  This function moves a detector to its home position.
 *
 *	Input:
 *	det_num, number of the detector to move (0, 1, 2, ...)
 *
 *	Zachary Wolf
 *	7/25/05
 */

void wirpos_move_home(int det_num)
{
	/* Declare variables */
	char buf[80];
	
	/* Check input parameters */
    if (det_num < 0 || det_num >= wirpos_param.num_wire_det)
    {
    	Fmt(msg, "%s<det_num = %i, out of range", det_num);
    	wirpos_error(msg);
    	return;
    }
	
	/* Move the detector to the home position */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		esp7000_move_home(esp7000_ID, wirpos_param.det_info[det_num].move_axis);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		printf("\nPlease move detector %i (%s) to its home position.\n", det_num, wirpos_param.det_info[det_num].name);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
	
	/* Log the move */
	wirpos_log_move_home(det_num);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_rel_move
 *  This function moves a detector a specified amount.
 *
 *	Input:
 *	det_num, number of the detector to move (0, 1, 2, ...)
 *	dis, distance (m), can be + or -
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_rel_move(int det_num, double dis)
{
	/* Declare variables */
	char buf[80];
	
	/* Check input parameters */
    if (det_num < 0 || det_num >= wirpos_param.num_wire_det)
    {
    	Fmt(msg, "%s<det_num = %i, out of range", det_num);
    	wirpos_error(msg);
    	return;
    }
	
	/* Move the detector the specified amount */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		esp7000_rel_stage_move(esp7000_ID, wirpos_param.det_info[det_num].move_axis, wirpos_param.acc, wirpos_param.vel, dis);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		printf("\nPlease move detector %i (%s) by %f m.\n", det_num, wirpos_param.det_info[det_num].name, dis);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
	
	/* Save the position */
	nominal_position[det_num] += dis;
	
	/* Log the move */
	wirpos_log_rel_move(det_num, dis);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_abs_move
 *  This function moves a detector to the specified position.
 *
 *	Input:
 *	det_num, number of the detector to move (0, 1, 2, ...)
 *	pos, position to move to (m), can be + or -
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_abs_move(int det_num, double pos)
{
	/* Declare variables */
	char buf[80];
	
	/* Check input parameters */
    if (det_num < 0 || det_num >= wirpos_param.num_wire_det)
    {
    	Fmt(msg, "%s<det_num = %i, out of range", det_num);
    	wirpos_error(msg);
    	return;
    }
	
	/* Move the detector to the specified position */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		esp7000_abs_stage_move(esp7000_ID, wirpos_param.det_info[det_num].move_axis, wirpos_param.acc, wirpos_param.vel, pos);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		printf("\nPlease move detector %i (%s) to position %f m.\n", det_num, wirpos_param.det_info[det_num].name, pos);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
	
	/* Save the position */
	nominal_position[det_num] = pos;
	
	/* Log the move */
	wirpos_log_abs_move(det_num, pos);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_set_zero_pos
 *  This function makes the present position the zero position.
 *
 *	Input:
 *	det_num, detector number (0, 1, 2, ...)
 *
 *	Zachary Wolf
 *	7/25/05
 */

void wirpos_set_zero_pos(int det_num)
{
	/* Check input parameters */
    if (det_num < 0 || det_num >= wirpos_param.num_wire_det)
    {
    	Fmt(msg, "%s<det_num = %i, out of range", det_num);
    	wirpos_error(msg);
    	return;
    }
	
	/* Zero the move device */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		esp7000_zero(esp7000_ID, wirpos_param.det_info[det_num].move_axis);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970);
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
	
	/* Set the nominal position to zero */
	nominal_position[det_num] = 0.;
	
	/* Message */
    printf("The position zero has been set.\n");
	
	/* Log the zero */
	wirpos_log_set_zero_pos(det_num);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_get_pos
 *  This function gets the position of the specified stage.
 *
 *	Input:
 *	det_num, detector number (0, 1, ...)
 *
 *	Output:
 *	pos, stage position (m)
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_get_pos(int det_num, double* pos)
{
	/* Get the position of the stage */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		esp7000_get_pos(esp7000_ID, wirpos_param.det_info[det_num].move_axis, pos);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		*pos = nominal_position[det_num];
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
	
	/* Log the position */
	wirpos_log_get_pos(det_num, *pos);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_get_volt
 *  This function gets the output voltage from the detector.
 *
 *	Input:
 *	det_num, detector number (0, 1, ...)
 *
 *	Output:
 *	v, detector output voltage (V)
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_get_volt(int det_num, double* v)
{
	/* Get the detector output voltage */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		hp34970_get_chan_volt(wirpos_hp34970_ID, wirpos_param.det_info[det_num].meas_channel, v);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		hp34970_get_chan_volt(wirpos_hp34970_ID, wirpos_param.det_info[det_num].meas_channel, v);
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
	
	/* Log the voltage */
	wirpos_log_get_volt(det_num, *v);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_find_wire_coarse
 *  This function moves a detector through its range in evenly
 *	spaced steps starting from the home position.
 *	At each step, it measures the detector output
 *	voltage.  It interpolates to find the wire position.
 *
 *	Input:
 *	det_num, number of the detector to move (0, 1, 2, ...)
 *
 *	Output:
 *	coarse_wire_pos, wire position, rough estimate (m)
 *
 *	Zachary Wolf
 *	7/25/05
 */

void wirpos_find_wire_coarse(int det_num, double* coarse_wire_pos)
{
	/* Declare variables */
	double v_det[WIRPOS_COARSE_NUM_POS];
	double v_det_abs[WIRPOS_COARSE_NUM_POS];
	double x_det[WIRPOS_COARSE_NUM_POS];
	int i, v_min_index;
	int found_wire;
	int index_wire;
	int num_pos;
	double v_min, v_max;
	double v_mid;
	double x_left, x_right;
	
	/* Check input parameters */
    if (det_num < 0 || det_num >= wirpos_param.num_wire_det)
    {
    	Fmt(msg, "%s<det_num = %i, out of range", det_num);
    	wirpos_error(msg);
    	return;
    }
    
    /* Initialize variables */
    found_wire = 0;
    index_wire = 0;
    
    /* Initialize the user interface */
	wirposui_clear_graph();
	wirposui_set_det_num(det_num);
	
	/* Message */
	printf("\nFind Wire Coarse:\n");
    
    /* Move the detector to its home position */
	wirpos_move_home(det_num);
	
	/* Move by the amount indicated in parameter file */
	wirpos_abs_move(det_num, wirpos_param.det_info[det_num].zero_shift);
	
	/* Measure the detector voltage in the home position, index 0, no shadow */
	wirpos_get_volt(det_num, &v_det[0]);
	v_det_abs[0] = fabs(v_det[0]);
	wirpos_get_pos(det_num, &x_det[0]);
	num_pos = 1;
	
	/* Setup the plot for the measurements */
	wirposui_setup_graph(wirpos_param.det_info[det_num].zero_shift, wirpos_param.det_info[det_num].zero_shift+WIRPOS_COARSE_NUM_POS * WIRPOS_COARSE_STEP_SIZE, 0., v_det_abs[0] * 1.3);
	
	/* Plot the measurement, use absolute values of the voltage */
	wirposui_plot_x_v_samp(x_det[0], v_det_abs[0]);
    
    /* Loop over steps */
    for (i = 1; i < WIRPOS_COARSE_NUM_POS; i++)
    {
    	/* Do a relative move by the step size */
		wirpos_rel_move(det_num, WIRPOS_COARSE_STEP_SIZE);
	    /* Measure the detector output voltage */
		wirpos_get_volt(det_num, &v_det[i]);
		v_det_abs[i] = fabs(v_det[i]);
		/* Get the detector position */
		wirpos_get_pos(det_num, &x_det[i]);
		/* Plot the measurement */
		wirposui_plot_x_v_samp(x_det[i], v_det_abs[i]);
		/* Increment the number of measured positions */
		num_pos++;
		/* See if the wire is found */
		if (fabs(v_det_abs[i] - v_det_abs[0]) > WIRPOS_SENS * v_det_abs[0])
		{
			found_wire = 1;
			index_wire = i;
		}
		/* If the wire is found, exit early to save time */
		if (found_wire == 1 && (i - index_wire) > 3) break;
	}
    
    /* Find the minimum and maximum output voltage */
    v_min = v_det_abs[0];
    v_min_index = 0;
    v_max = v_det_abs[0];
    for (i = 1; i < num_pos; i++)
    {
    	if (v_det_abs[i] < v_min)
    	{
    		v_min = v_det_abs[i];
    		v_min_index = i;
    	}
    	if (v_det_abs[i] > v_max) v_max = v_det_abs[i];
    }
    
    /* Determine the midpoint voltage level */
    v_mid = (v_min + v_max) / 2.;
    
    /* Draw a line at the midpoint voltage level */
	wirposui_plot_line(0., v_mid, WIRPOS_COARSE_NUM_POS * WIRPOS_COARSE_STEP_SIZE, v_mid);
    
    /* Interpolate on the left side, starting from the minimum */
    x_left = 0.;
    for (i = v_min_index; i >= 0; i--)
    {
    	if (v_det_abs[i] >= v_mid && v_det_abs[i+1] < v_mid)
    	{
    		x_left = x_det[i] + ((v_mid - v_det_abs[i]) / (v_det_abs[i+1] - v_det_abs[i])) * (x_det[i+1] - x_det[i]);
    		break;
    	}
    }
    
    /* Plot the left interpolation point */
	wirposui_plot_x_v_thresh(x_left, v_mid);
    
    /* Interpolate on the right side */
    x_right = 0.;
    for (i = v_min_index; i < num_pos - 1; i++)
    {
    	if (v_det_abs[i] <= v_mid && v_det_abs[i+1] > v_mid)
    	{
    		x_right = x_det[i] + ((v_mid - v_det_abs[i]) / (v_det_abs[i+1] - v_det_abs[i])) * (x_det[i+1] - x_det[i]);
    		break;
    	}
    }
    
    /* Plot the right interpolation point */
	wirposui_plot_x_v_thresh(x_right, v_mid);
    
    /* Average to get the wire position */
    *coarse_wire_pos = (x_left + x_right) / 2.;
    
    /* Check */
    if (found_wire == 0 || x_left == 0. || x_right == 0.)
    {
    	wirpos_message("wirpos_find_wire_coarse:  Could not find wire.");
    	*coarse_wire_pos = 0.;
    }
    
    /* Plot the wire position */
	wirposui_plot_x_v_wire(*coarse_wire_pos, v_mid);
	
	/* Move to the wire position */
	printf("Moving to the coarse wire position...\n");
	wirpos_abs_move(det_num, *coarse_wire_pos);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_find_wire_fine
 *  This function moves a detector through a small range in evenly
 *	spaced steps starting from a coarse initial estimate of the
 *	wire position.  At each step, it measures the detector output
 *	voltage.  A final scan is used to get fit information to
 *	determine the wire position.
 *
 *	Input:
 *	det_num, number of the detector to move (0, 1, 2, ...)
 *	coarse_wire_pos, initial estimate of the wire position (m)
 *
 *	Output:
 *	fine_wire_pos, wire position (m)
 *
 *	Zachary Wolf
 *	7/25/05
 */

void wirpos_find_wire_fine(int det_num, double coarse_wire_pos, double* fine_wire_pos)
{
	/* Declare variables */
	double x_det_start;
	double v_det[WIRPOS_FINE_NUM_POS];
	double v_det_abs[WIRPOS_FINE_NUM_POS];
	double x_det[WIRPOS_FINE_NUM_POS];
	int i, j, v_min_index;
	double v_min, v_max;
	double v_mid;
	double x_left, x_right;
	double v_fit[WIRPOS_FIT_NUM_POS];
	double v_fit_abs[WIRPOS_FIT_NUM_POS];
	double x_fit[WIRPOS_FIT_NUM_POS];
	double z[WIRPOS_FIT_NUM_POS], slope, intercept, mse;
        double sum;
	
	/* Check input parameters */
    if (det_num < 0 || det_num >= wirpos_param.num_wire_det)
    {
    	Fmt(msg, "%s<det_num = %i, out of range", det_num);
    	wirpos_error(msg);
    	return;
    }
    
    /* Initialize the user interface */
	wirposui_clear_graph();
	wirposui_set_det_num(det_num);
	
	/* Message */
	printf("\nFind Wire Fine:\n");
	
	/* Move to the initial position of the fine scan */
	x_det_start = coarse_wire_pos - (WIRPOS_FINE_NUM_POS - 1) * WIRPOS_FINE_STEP_SIZE / 2.;
	if (x_det_start < 0.)
	{
		printf("Coarse wire position too close to zero, exiting...");
		*fine_wire_pos = 0.;
		return;
	}
	wirpos_abs_move(det_num, x_det_start);
	
	/* Measure the detector voltage in the initial position */
	wirpos_get_volt(det_num, &v_det[0]);
	v_det_abs[0] = fabs(v_det[0]);
	wirpos_get_pos(det_num, &x_det[0]);
	
	/* Setup the plot for the measurements */
	wirposui_setup_graph(x_det_start - WIRPOS_FINE_STEP_SIZE, x_det_start + WIRPOS_FINE_NUM_POS * WIRPOS_FINE_STEP_SIZE, 0., v_det_abs[0] * 1.5);
	
	/* Plot the measurement, use absolute values of the voltage */
	wirposui_plot_x_v_samp(x_det[0], v_det_abs[0]);
    
    /* Loop over steps */
    for (i = 1; i < WIRPOS_FINE_NUM_POS; i++)
    {
    	/* Do a relative move by the step size */
		wirpos_rel_move(det_num, WIRPOS_FINE_STEP_SIZE);
	    /* Measure the detector output voltage */
		Delay(0.2);
		wirpos_get_volt(det_num, &v_det[i]);
		v_det_abs[i] = fabs(v_det[i]);
		/* Get the detector position */
		wirpos_get_pos(det_num, &x_det[i]);
		/* Plot the measurement */
		wirposui_plot_x_v_samp(x_det[i], v_det_abs[i]);
	}
    
    //Check that the voltages are not too close to 0.
    for (i = 9; i < WIRPOS_FINE_NUM_POS; i++)
    {
        sum=0;
        for(j=i-9; j<i; j++)
	    	sum+=v_det_abs[j];
		if(sum < 0.3)
		{
	    	*fine_wire_pos=0.;
	    	return;
        }
    }

    /* Find the minimum and maximum output voltage */
    v_min = v_det_abs[0];
    v_max = v_det_abs[0];
    v_min_index=0;
    for (i = 1; i < WIRPOS_FINE_NUM_POS; i++)
    {
    	if (v_det_abs[i] < v_min)
    	{
    		v_min = v_det_abs[i];
    		v_min_index=i;
    	}
    	if (v_det_abs[i] > v_max) v_max = v_det_abs[i];
    }
    
    /* Determine the midpoint voltage level */
    v_mid = (v_min + v_max) / 2.;
    
    /* Draw a line at the midpoint voltage level */
	wirposui_plot_line(x_det_start - WIRPOS_FINE_STEP_SIZE, v_mid, x_det_start + WIRPOS_FINE_NUM_POS * WIRPOS_FINE_STEP_SIZE, v_mid);
    
    /* Interpolate on the left side */
    x_left = 0.;
    for (i = v_min_index; i >= 0; i--)
    {
    	if (v_det_abs[i] >= v_mid && v_det_abs[i+1] < v_mid)
    	{
    		x_left = x_det[i] + ((v_mid - v_det_abs[i]) / (v_det_abs[i+1] - v_det_abs[i])) * (x_det[i+1] - x_det[i]);
    		break;
    	}
    }
	
	/* Move to the initial position of the fit scan on the left side */
	x_det_start = x_left - ((WIRPOS_FIT_NUM_POS - 1) * WIRPOS_FIT_STEP_SIZE / 2.) - WIRPOS_FIT_STEP_SIZE;
	if (x_det_start < 0.)
	{
		printf("Problem finding the wire, exiting...");
		*fine_wire_pos = 0.;
		return;
	}
	wirpos_abs_move(det_num, x_det_start - 0.000040);
	wirpos_abs_move(det_num, x_det_start);
	
	/* Do a fit scan around x_left */
    for (i = 0; i < WIRPOS_FIT_NUM_POS; i++)
    {
    	/* Do a relative move by the step size */
		wirpos_rel_move(det_num, WIRPOS_FIT_STEP_SIZE);
	    /* Measure the detector output voltage */
		wirpos_get_volt(det_num, &v_fit[i]);
		v_fit_abs[i] = fabs(v_fit[i]);
		/* Get the detector position */
		wirpos_get_pos(det_num, &x_fit[i]);
		/* Plot the measurement */
		wirposui_plot_x_v_samp(x_fit[i], v_fit_abs[i]);
	}
	
	/* Fit the scan to find a new measurement of x_left */
	LinFit(x_fit, v_fit_abs, WIRPOS_FIT_NUM_POS, z, &slope, &intercept, &mse);
	x_left = (v_mid - intercept) / slope;
    
    /* Plot the left interpolation point */
	wirposui_plot_x_v_thresh(x_left, v_mid);
    
    /* Interpolate on the right side */
    x_right = 0.;
    for (i = v_min_index; i < WIRPOS_FINE_NUM_POS - 1; i++)
    {
    	if (v_det_abs[i] <= v_mid && v_det_abs[i+1] > v_mid)
    	{
    		x_right = x_det[i] + ((v_mid - v_det_abs[i]) / (v_det_abs[i+1] - v_det_abs[i])) * (x_det[i+1] - x_det[i]);
    		break;
    	}
    }
	
	/* Move to the initial position of the fit scan on the right side */
	x_det_start = x_right - ((WIRPOS_FIT_NUM_POS - 1) * WIRPOS_FIT_STEP_SIZE / 2.) - WIRPOS_FIT_STEP_SIZE;
	if (x_det_start < 0.)
	{
		printf("Problem finding the wire, exiting...");
		*fine_wire_pos = 0.;
		return;
	}
	wirpos_abs_move(det_num, x_det_start);
	
	/* Do a fit scan around x_right */
    for (i = 0; i < WIRPOS_FIT_NUM_POS; i++)
    {
    	/* Do a relative move by the step size */
		wirpos_rel_move(det_num, WIRPOS_FIT_STEP_SIZE);
	    /* Measure the detector output voltage */
		wirpos_get_volt(det_num, &v_fit[i]);
		v_fit_abs[i] = fabs(v_fit[i]);
		/* Get the detector position */
		wirpos_get_pos(det_num, &x_fit[i]);
		/* Plot the measurement */
		wirposui_plot_x_v_samp(x_fit[i], v_fit_abs[i]);
	}
	
	/* Fit the scan to find a new measurement of x_right */
	LinFit(x_fit, v_fit_abs, WIRPOS_FIT_NUM_POS, z, &slope, &intercept, &mse);
	x_right = (v_mid - intercept) / slope;
    
    /* Plot the right interpolation point */
	wirposui_plot_x_v_thresh(x_right, v_mid);
    
    /* Average to get the wire position */
    *fine_wire_pos = (x_left + x_right) / 2.;
    
    /* Plot the wire position */
	wirposui_plot_x_v_wire(*fine_wire_pos, v_mid);
	
	/* Move to the wire position */
	printf("Moving to the fine wire position...\n");
	wirpos_abs_move(det_num, *fine_wire_pos - 0.000050);
	wirpos_abs_move(det_num, *fine_wire_pos);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_find_wire
 *  This function first makes a coarse measurement of the wire position.
 *	It then does a fine measurement to get the final position.
 *	It leaves the detector centered on the wire.
 *
 *	Input:
 *	det_num, number of the detector to move (0, 1, 2, ...)
 *
 *	Output:
 *	wire_pos, wire position (m)
 *
 *	Zachary Wolf
 *	8/4/05
 */

void wirpos_find_wire(int det_num, double* wire_pos)
{
	/* Declare variables */
        double coarse_wire_pos;
        int i=0,done=0;
	
		//turn on the axis
		esp7000_activate_axis(esp7000_ID, det_num+1);  //not the best place for it, but it works
		
		/* Make an initial coarse measurement of the wire position */
        do
        {
	    i++;
            wirpos_find_wire_coarse(det_num, &coarse_wire_pos);
            
            if(coarse_wire_pos!=0.)
                done=1;
            else
            {
                printf("\nFailed to find coarse wire position.\n");
                if(i<WIRPOS_COARSE_TRIES)
                    printf("Retrying...\n");
                else
                    done=1;
            }
        }while(!done);
	
	/* Make a final fine measurement of the wire position */
        done=0;
        i=0;
        do
        {
	    i++;
            wirpos_find_wire_fine(det_num, coarse_wire_pos, wire_pos);
            
            if(*wire_pos!=0.)
                done=1;
            else
            {
                printf("\nFailed to find fine wire position.\n");
                if(i<WIRPOS_FINE_TRIES)
                    printf("Retrying...\n");
                else
                {
                    done=1;
                    wirpos_error("Out of tries finding wire position.");
                }
            }
        }while(!done);

    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	wirpos_exit
 *  This function configures the WIRPOS for program exit and
 *	closes the devices.
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_exit(void)
{
	/* Exit all devices */
	if (wirpos_param.device_type == WIRPOS_ESP7000_HP34970)
	{
		esp7000_exit(esp7000_ID);
		hp34970_exit(wirpos_hp34970_ID);
	}
	else if (wirpos_param.device_type == WIRPOS_MANUAL_HP34970)
	{
		hp34970_exit(wirpos_hp34970_ID);
	}
	else if (wirpos_param.device_type == WIRPOS_NONE_NONE);
	else wirpos_error("Unknown device type.");
    
    /* Done */
    return;
}

/*	**************************************************************  */

/*	PRIVATE FUNCTIONS  */

/*	**************************************************************  */
/*
 *	wirpos_message
 *  This function handles messages about the WIRPOS.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	12/20/99
 */

void wirpos_message(char* message)
{
	/* Print the message */
	printf("%s\n", message);
	
	/* Done */
	return;
}

/*	**************************************************************  */
/*
 *	wirpos_error
 *  This function handles error messages for the WIRPOS.
 *
 *	Input:
 *	message, string to display in standard I/O
 *
 *	Zachary Wolf
 *	12/20/99
 */

void wirpos_error(char* message)
{
	/* Declare variables */
	char buf[80];
	
	/* Notify the operator of the error */
	printf("\nWIRPOS ERROR: %s\n", message);
	Beep();
	Delay(.5);
	Beep();
	
	/* Terminate the program if the operator desires */
	printf("Press ENTER to continue.\nPress any key then ENTER to terminate program.\n");
	fgets(buf, 80, stdin);
	if (buf[0] == '\n') return;
	else exit(0);
}

/*	**************************************************************	*/
/*
 *	wirpos_log_device_type
 *	This function logs the types of devices being used.
 *
 *	Input:
 *	device_type, types of devices
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_log_device_type(enum wirpos_device_type_enum device_type)
{
	/* Declare variables */
	FILE* file_ptr;
	char type[50];

	/* Put the device type in a string */
	if      (device_type == WIRPOS_ESP7000_HP34970) strcpy(type, "Move Device = ESP7000, Measure Device = HP34970");
	else if (device_type == WIRPOS_MANUAL_HP34970) strcpy(type, "Move Device = MANUAL, Measure Device = HP34970");
	else if (device_type == WIRPOS_NONE_NONE) strcpy(type, "Move Device = NONE, Measure Device = NONE");
	else wirpos_error("Unknown device type in log device function.");
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		wirpos_message("Unable to open log file");
		return;
	}
	
	/* Write the types of devices to the log file */
	fprintf(file_ptr, "%s  Wirpos Devices:  %s\n", TimeStr(), type);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	wirpos_log_move_home
 *	This function logs a move to the home position.
 *
 *	Input:
 *	det_num, detector number (0, 1, ...)
 *
 *	Zachary Wolf
 *	7/25/05
 */

void wirpos_log_move_home(int det_num)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		wirpos_message("Unable to open log file");
		return;
	}
	
	/* Write the distance moved to the log file */
	fprintf(file_ptr, "%s  Detector %i moved to its home position\n", TimeStr(), det_num);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	wirpos_log_rel_move
 *	This function logs a move.
 *
 *	Input:
 *	det_num, detector number (0, 1, ...)
 *	dis, distance stage is moved (m)
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_log_rel_move(int det_num, double dis)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		wirpos_message("Unable to open log file");
		return;
	}
	
	/* Write the distance moved to the log file */
	fprintf(file_ptr, "%s  Rel move of detector %i by %f m\n", TimeStr(), det_num, dis);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	wirpos_log_abs_move
 *	This function logs a move.
 *
 *	Input:
 *	det_num, detector number (0, 1, ...)
 *	pos, position stage is moved (m)
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_log_abs_move(int det_num, double pos)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		wirpos_message("Unable to open log file");
		return;
	}
	
	/* Write the distance moved to the log file */
	fprintf(file_ptr, "%s  Abs move of detector %i to %f m\n", TimeStr(), det_num, pos);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	wirpos_log_get_pos
 *	This function logs the result of a position measurement.
 *
 *	Input:
 *	det_num, detector number (0, 1, ...)
 *	pos, stage position (m)
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_log_get_pos(int det_num, double pos)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		wirpos_message("Unable to open log file");
		return;
	}
	
	/* Write the measured value to the log file */
	fprintf(file_ptr, "%s  Detector %i position measurement, pos = %f m\n", TimeStr(), det_num, pos);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	wirpos_log_get_volt
 *	This function logs the result of a detector output voltage measurement.
 *
 *	Input:
 *	det_num, detector number (0, 1, ...)
 *	v, detector output voltage (V)
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_log_get_volt(int det_num, double v)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		wirpos_message("Unable to open log file");
		return;
	}
	
	/* Write the measured value to the log file */
	fprintf(file_ptr, "%s  Detector %i voltage measurement, V = %f V\n", TimeStr(), det_num, v);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	wirpos_log_set_zero_pos
 *	This function logs the zeroing of the motion device for the given detector.
 *
 *	Input:
 *	det_num, detector number
 *
 *	Zachary Wolf
 *	6/3/05
 */

void wirpos_log_set_zero_pos(int det_num)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		wirpos_message("Unable to open log file");
		return;
	}
	
	/* Write the measured value to the log file */
	fprintf(file_ptr, "%s  Detector %i stage zeroed\n", TimeStr(), det_num);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}