/*	**************************************************************  */
/*
 *	Module MOVEZ
 *	This module contains functions for moving and getting the position
 *	of a single stage.
 *
 *	Zachary Wolf
 *	2/2/00
 */ 

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

/*	INCLUDES  */
#include <utility.h>
#include <gpib.h>
#include <formatio.h>
#include <ansi_c.h>
#include "movez.h"
#include "cm2100.h"
#include "mc4.h"

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

/*  PRIVATE FUNCTIONS  */
void movez_message(char* msg);
void movez_error(char* msg);
void movez_log_device_type(enum movez_device_type_enum device_type);
void movez_log_abs_move(double pos);
void movez_log_get_pos(double pos);
void movez_log_zero(void);

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

/*  PRIVATE GLOBAL VARIABLES  */
static int cm2100_ID;
static int mc4_ID;
static double current_nominal_position;

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

/*  PRIVATE PARAMETERS  */
static char log_file[100];
static struct movez_param_struct movez_param;

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

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  movez_init
 *  This function opens the device, queries for ID, and
 *  initializes the device to a known state.
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_init(char log_file_in[], struct movez_param_struct movez_param_in)
{
	/* Save the module parameters */
	strcpy(log_file, log_file_in);
	movez_param = movez_param_in;
	
	/* Initialize the appropriate devices */
	if (movez_param.device_type == MOVEZ_CM2100_CM2100)
	{
		cm2100_init(movez_param.board_addr, movez_param.cm2100_addr, &cm2100_ID);
		cm2100_set_num_steps_per_rev(movez_param.cm2100_num_steps_per_rev);
	}
	else if (movez_param.device_type == MOVEZ_MC4_MC4)
	{
		mc4_init(movez_param.board_addr, movez_param.mc4_addr, &mc4_ID);
	}
	else if (movez_param.device_type == MOVEZ_MANUAL_NONE);
	else if (movez_param.device_type == MOVEZ_NONE_NONE);
	else movez_error("Unknown device type.");
	
	/* Zero the Z position system */
	movez_zero();
	
	/* Log the device type */
	movez_log_device_type(movez_param.device_type);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movez_abs_move
 *  This function moves the stage to the specified position.
 *
 *	Input:
 *	pos, final stage position (m)
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_abs_move(double pos)
{
	/* Declare variables */
	double cm2100_init_pos_rev, cm2100_init_pos_m;
	double cm2100_dist_to_move_m, cm2100_dist_to_move_rev;
	double cm2100_acc_rev_sec_sec, cm2100_vel_rev_sec;
	double cm2100_final_pos_rev, cm2100_final_pos_m;
	char buf[80];
	
	/* Move the stage to the specified position */
	if (movez_param.device_type == MOVEZ_CM2100_CM2100)
	{
		cm2100_get_abs_position(cm2100_ID, &cm2100_init_pos_rev);
		cm2100_init_pos_m = cm2100_init_pos_rev * movez_param.cm2100_meters_per_rev;
		cm2100_dist_to_move_m = pos - cm2100_init_pos_m;
		cm2100_dist_to_move_rev = cm2100_dist_to_move_m / movez_param.cm2100_meters_per_rev;
		cm2100_acc_rev_sec_sec = movez_param.acc / movez_param.cm2100_meters_per_rev;
		cm2100_vel_rev_sec = movez_param.vel / movez_param.cm2100_meters_per_rev;
		cm2100_rel_move(cm2100_ID, cm2100_acc_rev_sec_sec, cm2100_vel_rev_sec, cm2100_dist_to_move_rev);
		cm2100_get_abs_position(cm2100_ID, &cm2100_final_pos_rev);
		cm2100_final_pos_m = cm2100_final_pos_rev * movez_param.cm2100_meters_per_rev;
		if (fabs(cm2100_final_pos_m - pos) > .0001) movez_message("The stage position is different from the desired position by more than .0001 m");
	}
	else if (movez_param.device_type == MOVEZ_MC4_MC4)
	{
		mc4_abs_stage_move(mc4_ID, movez_param.mc4_axis, movez_param.acc, movez_param.vel, pos);
	}
	else if (movez_param.device_type == MOVEZ_MANUAL_NONE)
	{
		printf("\nPlease move the stage to %f m\n", pos);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movez_param.device_type == MOVEZ_NONE_NONE);
	else movez_error("Unknown device type.");
	
	/* Save the position */
	current_nominal_position = pos;
	
	/* Log the move */
	movez_log_abs_move(pos);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movez_get_pos
 *  This function gets the position of the stage.
 *
 *	Output:
 *	pos, stage position (m)
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_get_pos(double* pos)
{
	/* Declare variables */
	double cm2100_pos_rev;
	
	/* Get the position of the stage */
	if (movez_param.device_type == MOVEZ_CM2100_CM2100)
	{
		cm2100_get_abs_position(cm2100_ID, &cm2100_pos_rev);
		*pos = cm2100_pos_rev * movez_param.cm2100_meters_per_rev;
	}
	else if (movez_param.device_type == MOVEZ_MC4_MC4)
	{
		mc4_get_stage_pos(mc4_ID, movez_param.mc4_axis, pos);
	}
	else if (movez_param.device_type == MOVEZ_MANUAL_NONE)
	{
		*pos = current_nominal_position;
	}
	else if (movez_param.device_type == MOVEZ_NONE_NONE);
	else movez_error("Unknown device type.");
	
	/* Log the position */
	movez_log_get_pos(*pos);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movez_zero
 *  This function zeros the move device and the read device.
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_zero(void)
{
	/* Declare variables */
	char buf[80];
    
    /* Have the user move to the zero position, if required */
	if (movez_param.device_type == MOVEZ_CM2100_CM2100 || movez_param.device_type == MOVEZ_MC4_MC4)
	{
    	printf("\nPosition Zero\n");
    	printf("Please move to the zero position.\n");
    	printf("The Z position scale will be zeroed.\n");
    	printf("Press ENTER when ready.");
    	fgets(buf, 80, stdin);
    }
	
	/* Zero the move device and the read device */
	if (movez_param.device_type == MOVEZ_CM2100_CM2100)
	{
		cm2100_zero(cm2100_ID);
    	printf("The Z position zero has been set.\n");
	}
	else if (movez_param.device_type == MOVEZ_MC4_MC4)
	{
		mc4_zero(mc4_ID);
    	printf("The Z position zero has been set.\n");
	}
	else if (movez_param.device_type == MOVEZ_MANUAL_NONE)
	{
		current_nominal_position = 0.;
	}
	else if (movez_param.device_type == MOVEZ_NONE_NONE);
	else movez_error("Unknown device type.");
	
	/* Log the zero */
	movez_log_zero();
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movez_exit
 *  This function configures the MOVEZ for program exit and
 *	closes the devices.
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_exit(void)
{
	/* Exit all devices */
	if (movez_param.device_type == MOVEZ_CM2100_CM2100)
	{
		cm2100_exit(cm2100_ID);
	}
	else if (movez_param.device_type == MOVEZ_MC4_MC4)
	{
		mc4_exit(mc4_ID);
	}
	else if (movez_param.device_type == MOVEZ_MANUAL_NONE);
	else if (movez_param.device_type == MOVEZ_NONE_NONE);
	else movez_error("Unknown device type.");
    
    /* Done */
    return;
}

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

/*	PRIVATE FUNCTIONS  */

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

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

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

void movez_error(char* message)
{
	/* Declare variables */
	char buf[80];
	
	/* Notify the operator of the error */
	printf("\nMOVEZ 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);
}

/*	**************************************************************	*/
/*
 *	movez_log_device_type
 *	This function logs the types of devices being used.
 *
 *	Input:
 *	device_type, types of devices
 *
 *	Zachary Wolf
 *	2/2/00
 */

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

	/* Put the device type in a string */
	if      (device_type == MOVEZ_CM2100_CM2100) strcpy(type, "Move Device = CM2100, Read Device = CM2100");
	else if (device_type == MOVEZ_MC4_MC4) strcpy(type, "Move Device = MC4, Read Device = MC4");
	else if (device_type == MOVEZ_MANUAL_NONE) strcpy(type, "Move Device = MANUAL, Read Device = NONE");
	else if (device_type == MOVEZ_NONE_NONE) strcpy(type, "Move Device = NONE, Read Device = NONE");
	else movez_error("Unknown device type in log device function.");
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movez_message("Unable to open log file");
		return;
	}
	
	/* Write the types of devices to the log file */
	fprintf(file_ptr, "%s  MoveZ Devices:  %s\n", TimeStr(), type);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movez_log_abs_move
 *	This function logs a move.
 *
 *	Input:
 *	pos, final stage position (m)
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_log_abs_move(double pos)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movez_message("Unable to open log file");
		return;
	}
	
	/* Write the final position to the log file */
	fprintf(file_ptr, "%s  Move Stage, position = %f\n", TimeStr(), pos);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movez_log_get_pos
 *	This function logs the result of a position measurement.
 *
 *	Input:
 *	pos, stage position (m)
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_log_get_pos(double pos)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movez_message("Unable to open log file");
		return;
	}
	
	/* Write the measured value to the log file */
	fprintf(file_ptr, "%s  Stage Position Measurement, pos = %f m\n", TimeStr(), pos);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movez_log_zero
 *	This function logs the zeroing of the move device and the read device.
 *
 *	Zachary Wolf
 *	2/2/00
 */

void movez_log_zero(void)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movez_message("Unable to open log file");
		return;
	}
	
	/* Write the measured value to the log file */
	fprintf(file_ptr, "%s  Stage Zeroed\n", TimeStr());
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}