/*	**************************************************************  */
/*
 *	Module MOVEMAG
 *	This module contains functions for moving and getting the position
 *	of a magnet.
 *
 *	Zachary Wolf
 *	9/7/05
 */ 

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

/*	INCLUDES  */
#include <utility.h>
#include <gpib.h>
#include <formatio.h>
#include <ansi_c.h>
#include "movemag_cam.h"
#include "cm6k6_g.h"

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

/*  PRIVATE FUNCTIONS  */
void movemag_message(char* msg);
void movemag_error(char* msg);
void movemag_log_device_type(enum movemag_device_type_enum device_type);
void movemag_log_rel_move_x(double x);
void movemag_log_rel_move_y(double y);
void movemag_log_rel_move_z(double z);
void movemag_log_rel_move_pitch(double pitch);
void movemag_log_rel_move_yaw(double yaw);
void movemag_log_rel_move_roll(double roll);
void movemag_log_abs_move_x(double x);
void movemag_log_abs_move_y(double y);
void movemag_log_abs_move_z(double z);
void movemag_log_abs_move_pitch(double pitch);
void movemag_log_abs_move_yaw(double yaw);
void movemag_log_abs_move_roll(double roll);
void movemag_log_get_pos(struct mag_pos_struct pos);
void movemag_log_zero(void);

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

/*  PRIVATE GLOBAL VARIABLES  */
static struct mag_pos_struct cur_nom_pos;
static int dev_ID;

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

/*  PRIVATE PARAMETERS  */
static char log_file[100];
static struct movemag_param_struct movemag_param;

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

/*	PUBLIC FUNCTIONS  */

/*	**************************************************************  */
/*
 *  movemag_init
 *  This function opens the device, queries for ID, and
 *  initializes the device to a known state.
 *
 *	Zachary Wolf
 *	9/7/05
 *  Heavily modified by Michael Levashov
 *  07/23/2007
 */

void movemag_init(char log_file_in[], struct movemag_param_struct movemag_param_in)
{
	/* Save the module parameters */
	strcpy(log_file, log_file_in);
	movemag_param = movemag_param_in;
	
	/* Initialize the appropriate devices */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE);
	else if (movemag_param.device_type == MOVEMAG_CM6K6_CM6K6)
	{
		cm6k6_init(movemag_param.port_num, movemag_param.cm6k6_addr, &dev_ID);
		/* Make the current position the zero position */
		/*if (movemag_param.axis_x > 0) esp7000_zero(dev_ID, movemag_param.axis_x);		
		if (movemag_param.axis_y > 0) esp7000_zero(dev_ID, movemag_param.axis_y);
		if (movemag_param.axis_z > 0) esp7000_zero(dev_ID, movemag_param.axis_z);
		if (movemag_param.axis_pitch > 0) esp7000_zero(dev_ID, movemag_param.axis_pitch);
		if (movemag_param.axis_yaw > 0) esp7000_zero(dev_ID, movemag_param.axis_yaw);
		if (movemag_param.axis_roll > 0) esp7000_zero(dev_ID, movemag_param.axis_roll);*/
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Log the device type */
	movemag_log_device_type(movemag_param.device_type);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_rel_move_x
 *  This function moves the magnet in the x direction by the specified
 *	amount.
 *
 *	Input:
 *	x, position to move to in the x direction (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_rel_move_x(double x)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet the specified amount */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do a relative move of the magnet in the x direction by %f m\n", x);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_x > 0)
			esp7000_rel_stage_move(dev_ID, movemag_param.axis_x, movemag_param.acc, movemag_param.vel, x);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.x += x;
	
	/* Log the move */
	movemag_log_rel_move_x(x);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_rel_move_y
 *  This function moves the magnet in the y direction by the specified
 *	amount.
 *
 *	Input:
 *	y, position to move to in the y direction (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_rel_move_y(double y)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet the specified amount */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do a relative move of the magnet in the y direction by %f m\n", y);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_y > 0)
			esp7000_rel_stage_move(dev_ID, movemag_param.axis_y, movemag_param.acc, movemag_param.vel, y);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.y += y;
	
	/* Log the move */
	movemag_log_rel_move_y(y);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_rel_move_z
 *  This function moves the magnet in the z direction by the specified
 *	amount.
 *
 *	Input:
 *	z, position to move to in the z direction (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_rel_move_z(double z)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet the specified amount */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do a relative move of the magnet in the z direction by %f m\n", z);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_z > 0)
			esp7000_rel_stage_move(dev_ID, movemag_param.axis_z, movemag_param.acc, movemag_param.vel, z);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.z += z;
	
	/* Log the move */
	movemag_log_rel_move_z(z);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_rel_move_pitch
 *  This function changes the pitch of the magnet by the specified
 *	amount.
 *
 *	Input:
 *	pitch, relative pitch to give the magnet (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_rel_move_pitch(double pitch)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet the specified amount */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do a relative move of the magnet in pitch by %f rad\n", pitch);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_pitch > 0)
			esp7000_rel_stage_move(dev_ID, movemag_param.axis_pitch, movemag_param.acc, movemag_param.vel, pitch * movemag_param.dist_to_pitch_pivot);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.pitch += pitch;
	
	/* Log the move */
	movemag_log_rel_move_pitch(pitch);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_rel_move_yaw
 *  This function yaws the magnet by the specified
 *	amount.
 *
 *	Input:
 *	yaw, relative yaw to give the magnet (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_rel_move_yaw(double yaw)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet the specified amount */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do a relative move of the magnet in yaw by %f rad\n", yaw);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_yaw > 0)
			esp7000_rel_stage_move(dev_ID, movemag_param.axis_yaw, movemag_param.acc, movemag_param.vel, yaw * movemag_param.dist_to_yaw_pivot);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.yaw += yaw;
	
	/* Log the move */
	movemag_log_rel_move_yaw(yaw);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_rel_move_roll
 *  This function changes the roll of the magnet by the specified
 *	amount.
 *
 *	Input:
 *	roll, relative roll to give the magnet (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_rel_move_roll(double roll)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet the specified amount */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do a relative move of the magnet in roll by %f rad\n", roll);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_roll > 0)
			esp7000_rel_stage_move(dev_ID, movemag_param.axis_roll, movemag_param.acc, movemag_param.vel, roll * movemag_param.dist_to_roll_pivot);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.roll += roll;
	
	/* Log the move */
	movemag_log_rel_move_roll(roll);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_abs_move_x
 *  This function moves the magnet in the x direction to the specified
 *	position.
 *
 *	Input:
 *	x, position to move to in the x direction (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_abs_move_x(double x)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet to the specified position */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do an absolute move of the magnet in x to %f m\n", x);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_x > 0)
			esp7000_abs_stage_move(dev_ID, movemag_param.axis_x, movemag_param.acc, movemag_param.vel, x);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.x = x;
	
	/* Log the move */
	movemag_log_abs_move_x(x);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_abs_move_y
 *  This function moves the magnet in the y direction to the specified
 *	position.
 *
 *	Input:
 *	y, position to move to in the y direction (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_abs_move_y(double y)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet to the specified position */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do an absolute move of the magnet in y to %f m\n", y);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_y > 0)
			esp7000_abs_stage_move(dev_ID, movemag_param.axis_y, movemag_param.acc, movemag_param.vel, y);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.y = y;
	
	/* Log the move */
	movemag_log_abs_move_y(y);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_abs_move_z
 *  This function moves the magnet in the z direction to the specified
 *	position.
 *
 *	Input:
 *	z, position to move to in the z direction (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_abs_move_z(double z)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet to the specified position */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do an absolute move of the magnet in z to %f m\n", z);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_z > 0)
			esp7000_abs_stage_move(dev_ID, movemag_param.axis_z, movemag_param.acc, movemag_param.vel, z);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.z = z;
	
	/* Log the move */
	movemag_log_abs_move_z(z);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_abs_move_pitch
 *  This function adjusts the pitch of the magnet to the specified
 *	value.
 *
 *	Input:
 *	pitch, absolute pitch to give the magnet (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_abs_move_pitch(double pitch)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet to the specified position */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do an absolute move of the magnet in pitch to %f rad\n", pitch);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_pitch > 0)
			esp7000_abs_stage_move(dev_ID, movemag_param.axis_pitch, movemag_param.acc, movemag_param.vel, pitch * movemag_param.dist_to_pitch_pivot);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.pitch = pitch;
	
	/* Log the move */
	movemag_log_abs_move_pitch(pitch);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_abs_move_yaw
 *  This function moves the magnet in yaw to the specified
 *	position.
 *
 *	Input:
 *	yaw, absolute yaw value to give the magnet (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_abs_move_yaw(double yaw)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet to the specified position */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do an absolute move of the magnet in yaw to %f rad\n", yaw);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_yaw > 0)
			esp7000_abs_stage_move(dev_ID, movemag_param.axis_yaw, movemag_param.acc, movemag_param.vel, yaw * movemag_param.dist_to_yaw_pivot);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.yaw = yaw;
	
	/* Log the move */
	movemag_log_abs_move_yaw(yaw);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_abs_move_roll
 *  This function adjusts the roll of the magnet to the specified
 *	value.
 *
 *	Input:
 *	roll, absolute roll to give the magnet (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_abs_move_roll(double roll)
{
	/* Declare variables */
	char buf[80];
	
	/* Move the magnet to the specified position */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		printf("\nPlease do an absolute move of the magnet in roll to %f rad\n", roll);
		printf("Press ENTER when ready.");
		fgets(buf, 80, stdin);
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_roll > 0)
			esp7000_abs_stage_move(dev_ID, movemag_param.axis_roll, movemag_param.acc, movemag_param.vel, roll * movemag_param.dist_to_roll_pivot);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Save the position */
	cur_nom_pos.roll = roll;
	
	/* Log the move */
	movemag_log_abs_move_roll(roll);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_get_pos
 *  This function gets the position of the magnet.
 *
 *	Output:
 *	pos, magnet position, structure
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_get_pos(struct mag_pos_struct* pos)
{
	/* Declare variables */
	double x, y, z, pitch, yaw, roll;
	
	/* Get the magnet position */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		*pos = cur_nom_pos;
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_x > 0) esp7000_get_pos(dev_ID, movemag_param.axis_x, &x);
		else x = 0.;
		if (movemag_param.axis_y > 0) esp7000_get_pos(dev_ID, movemag_param.axis_y, &y);
		else y = 0.;
		if (movemag_param.axis_z > 0) esp7000_get_pos(dev_ID, movemag_param.axis_z, &z);
		else z = 0.;
		if (movemag_param.axis_pitch > 0) esp7000_get_pos(dev_ID, movemag_param.axis_pitch, &pitch);
		else pitch = 0.;
		if (movemag_param.axis_yaw > 0) esp7000_get_pos(dev_ID, movemag_param.axis_yaw, &yaw);
		else yaw = 0.;
		if (movemag_param.axis_roll > 0) esp7000_get_pos(dev_ID, movemag_param.axis_roll, &roll);
		else roll = 0.;
		pos->x = x;
		pos->y = y;
		pos->z = z;
		pos->pitch = pitch / movemag_param.dist_to_pitch_pivot;
		pos->yaw = yaw / movemag_param.dist_to_yaw_pivot;
		pos->roll = roll / movemag_param.dist_to_roll_pivot;
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE)
	{
		pos->x = 0.;
		pos->y = 0.;
		pos->z = 0.;
		pos->pitch = 0.;
		pos->yaw = 0.;
		pos->roll = 0.;
	}
	else movemag_error("Unknown device type.");
	
	/* Log the position */
	movemag_log_get_pos(*pos);
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_zero
 *  This function zeros the move device and the read device.
 *
 *	Zachary Wolf
 *	9/7/05
 */
void movemag_uzero(void)
{
	/* Declare variables */
	char buf[80];
    
    /* Have the user move to the zero position, if required */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE || movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
    	printf("\nMagnet Position Zero\n");
    	printf("Please move the magnet to the zero position.\n");
    	printf("The position scales will be zeroed.\n");
    	printf("Press ENTER when ready.");
    	fgets(buf, 80, stdin);
    }
    
    /* Done */
    return;
}

/* Zero the move device and the read device */ 
void movemag_zero(void)
{	
	
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE)
	{
		cur_nom_pos.x = 0.;
		cur_nom_pos.y = 0.;
		cur_nom_pos.z = 0.;
		cur_nom_pos.pitch = 0.;
		cur_nom_pos.yaw = 0.;
		cur_nom_pos.roll = 0.;
	}
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		if (movemag_param.axis_x > 0) esp7000_zero(dev_ID, movemag_param.axis_x);
		if (movemag_param.axis_y > 0) esp7000_zero(dev_ID, movemag_param.axis_y);
		if (movemag_param.axis_z > 0) esp7000_zero(dev_ID, movemag_param.axis_z);
		if (movemag_param.axis_pitch > 0) esp7000_zero(dev_ID, movemag_param.axis_pitch);
		if (movemag_param.axis_yaw > 0) esp7000_zero(dev_ID, movemag_param.axis_yaw);
		if (movemag_param.axis_roll > 0) esp7000_zero(dev_ID, movemag_param.axis_roll);
		cur_nom_pos.x = 0.;
		cur_nom_pos.y = 0.;
		cur_nom_pos.z = 0.;
		cur_nom_pos.pitch = 0.;
		cur_nom_pos.yaw = 0.;
		cur_nom_pos.roll = 0.;
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
	
	/* Log the zero */
	movemag_log_zero();
    
    /* Done */
    return;
}

/*	**************************************************************  */
/*
 *	movemag_exit
 *  This function configures the MOVEMAG for program exit and
 *	closes the devices.
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_exit(void)
{
	/* Exit all devices */
	if (movemag_param.device_type == MOVEMAG_MANUAL_NONE);
	else if (movemag_param.device_type == MOVEMAG_ESP7000_ESP7000)
	{
		esp7000_exit(dev_ID);
	}
	else if (movemag_param.device_type == MOVEMAG_NONE_NONE);
	else movemag_error("Unknown device type.");
    
    /* Done */
    return;
}

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

/*	PRIVATE FUNCTIONS  */

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

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

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

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

/*	**************************************************************	*/
/*
 *	movemag_log_device_type
 *	This function logs the types of devices being used.
 *
 *	Input:
 *	device_type, types of devices
 *
 *	Zachary Wolf
 *	9/7/05
 */

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

	/* Put the device type in a string */
	if      (device_type == MOVEMAG_MANUAL_NONE) strcpy(type, "Move Device = MANUAL, Read Device = NONE");
	else if (device_type == MOVEMAG_ESP7000_ESP7000) strcpy(type, "Move Device = ESP7000, Read Device = ESP7000");
	else if (device_type == MOVEMAG_NONE_NONE) strcpy(type, "Move Device = NONE, Read Device = NONE");
	else movemag_error("Unknown device type in log device function.");
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the types of devices to the log file */
	fprintf(file_ptr, "%s  Movemag Devices:  %s\n", TimeStr(), type);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_rel_move_x
 *	This function logs a move.
 *
 *	Input:
 *	x, relative x motion (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_rel_move_x(double x)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Relative move of magnet by x = %f m\n", TimeStr(), x);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_rel_move_y
 *	This function logs a move.
 *
 *	Input:
 *	y, relative y motion (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_rel_move_y(double y)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Relative move of magnet by y = %f m\n", TimeStr(), y);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_rel_move_z
 *	This function logs a move.
 *
 *	Input:
 *	z, relative z motion (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_rel_move_z(double z)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Relative move of magnet by z = %f m\n", TimeStr(), z);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_rel_move_pitch
 *	This function logs a move.
 *
 *	Input:
 *	pitch, relative pitch motion (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_rel_move_pitch(double pitch)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Relative move of magnet by pitch = %f rad\n", TimeStr(), pitch);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_rel_move_yaw
 *	This function logs a move.
 *
 *	Input:
 *	yaw, relative yaw motion (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_rel_move_yaw(double yaw)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Relative move of magnet by yaw = %f rad\n", TimeStr(), yaw);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_rel_move_roll
 *	This function logs a move.
 *
 *	Input:
 *	roll, relative roll motion (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_rel_move_roll(double roll)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Relative move of magnet by roll = %f rad\n", TimeStr(), roll);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_abs_move_x
 *	This function logs a move.
 *
 *	Input:
 *	x, absolute x position (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_abs_move_x(double x)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Absolute move of magnet to x = %f m\n", TimeStr(), x);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_abs_move_y
 *	This function logs a move.
 *
 *	Input:
 *	y, absolute y position (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_abs_move_y(double y)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Absolute move of magnet to y = %f m\n", TimeStr(), y);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_abs_move_z
 *	This function logs a move.
 *
 *	Input:
 *	z, absolute z position (m)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_abs_move_z(double z)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Absolute move of magnet to z = %f m\n", TimeStr(), z);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_abs_move_pitch
 *	This function logs a move.
 *
 *	Input:
 *	pitch, absolute pitch position (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_abs_move_pitch(double pitch)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Absolute move of magnet to pitch = %f rad\n", TimeStr(), pitch);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_abs_move_yaw
 *	This function logs a move.
 *
 *	Input:
 *	yaw, absolute yaw position (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_abs_move_yaw(double yaw)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Absolute move of magnet to yaw = %f rad\n", TimeStr(), yaw);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_abs_move_roll
 *	This function logs a move.
 *
 *	Input:
 *	roll, absolute roll position (rad)
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_abs_move_roll(double roll)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the move to the log file */
	fprintf(file_ptr, "%s  Absolute move of magnet to roll = %f rad\n", TimeStr(), roll);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_get_pos
 *	This function logs the result of a position measurement.
 *
 *	Input:
 *	pos, magnet position, structure
 *
 *	Zachary Wolf
 *	9/7/05
 */

void movemag_log_get_pos(struct mag_pos_struct pos)
{
	/* Declare variables */
	FILE* file_ptr;
	
	/* Open the log file */
	file_ptr = fopen(log_file, "a");
	if (file_ptr == NULL)
	{
		movemag_message("Unable to open log file");
		return;
	}
	
	/* Write the measured value to the log file */
	fprintf(file_ptr, "%s  Magnet Position Measurement\n", TimeStr());
	fprintf(file_ptr, "          x = %f m\n", pos.x);
	fprintf(file_ptr, "          y = %f m\n", pos.y);
	fprintf(file_ptr, "          z = %f m\n", pos.z);
	fprintf(file_ptr, "          pitch = %f rad\n", pos.pitch);
	fprintf(file_ptr, "          yaw = %f rad\n", pos.yaw);
	fprintf(file_ptr, "          roll = %f rad\n", pos.roll);
	
	/* Close the log file */
	fclose(file_ptr);
	
	/* Done */
	return;
}

/*	**************************************************************	*/
/*
 *	movemag_log_zero
 *	This function logs the zeroing of the move device and the read device.
 *
 *	Zachary Wolf
 *	9/7/05
 */

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