extern "C" {
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <taskLib.h>
#include <timers.h>
#include <private/timerLibP.h>
#include <sysLib.h>

#include <sigLib.h>
#include <vme.h>

// These must be declared extern "C" so that names
// aren't mangled!
int Laser_Home(void);
int Laser_MoveTo(const char *sdx, const char *sdy);
int Laser_GetPosition(float &x, float &y);
int Laser_MoveRelative(const char *sdx, const char *sdy);

};

#include "vs4.h"

// Conversion factors
#define steps_per_inchX 8000
#define steps_per_inchY 8000

// Backflip to get a sleep function in VxWorks
#define sleep(SltX) taskDelay(SltX*sysClkRateGet())





//----------------
// Laser_Home
//----------------
int Laser_Home(void)
{
	// Move the laser to the upper right corner until the
	// limit switches are hit. Then, reset the position to
	// 0,0.
	
	// The maximum distance should be less than 48 inches.
	// Since we're using the limit switches, we can give any
	// value greater than this (e.g. 100 inches).
	int NstepsX = (int)(100.0*steps_per_inchX);
	int NstepsY = (int)(100.0*steps_per_inchY);
	char cmd[256];
	sprintf(cmd, "AA MR%d,%d; GD", NstepsX, NstepsY);
	
	// Send the command
	VS4cmd(cmd, NULL, 0, 0);

	// Loop until both limit switches are hit. We do the loop
	// as a precaution against looping infinitely. The maximum
	// time it should take is:
	//  48 inches * 8000 steps/inch / 1000 steps/sec = 384 seconds
	// Ergo, we set the limit to 400 seconds.
	int i;
	for(i=0; i<400; i++){
		
		// Get status of switches
		char statusX[256];
		char statusY[256];
		VS4cmd("AX QA", statusX, 256, 1);
		VS4cmd("AY QA", statusY, 256, 1);
		
		// Check if both are in the "overtravel" position
		if(statusX[2]=='L' && statusY[2]=='L')break;
		
		// Sleep for 1 second and then check again
		sleep(1);
	}
	
	// Warn user if we went 400 seconds without ever hitting the limit.
	if(i>=400){
		fprintf(stderr,"ERROR: Waited at least 400 seconds for move to complete\n");
		fprintf(stderr,"ERROR: and it never did!\n");
		return -1;
	}else{
		// Set the current position as the origin
		VS4cmd("AA LP0,0,0,0 ", NULL, 0, 0);
		printf("Now in the home position\n");
	}

	return 0;
}

//----------------
// Laser_MoveTo
//----------------
int Laser_MoveTo(const char *sdx, const char *sdy)
{
	// Move the laser to the specified x, y coordinates. The 
	// units of x and y are inches and are measured relative
	// to the home position.
	float dx = atof(sdx);
	float dy = atof(sdy);

	// The maximum distance should be less than 48 inches.
	// Since we're using the limit switches, we can give any
	// value greater than this (e.g. 100 inches).
	int NstepsX = (int)(x*steps_per_inchX);
	int NstepsY = (int)(y*steps_per_inchY);
	char cmd[256];
	sprintf(cmd, "AA MA%d,%d; GD", NstepsX, NstepsY);
	
	// Send the command
	VS4cmd(cmd, NULL, 0, 0);

	// Loop until both axes are done. We do the loop
	// as a precaution against looping infinitely. The maximum
	// time it should take is:
	//  48 inches * 8000 steps/inch / 1000 steps/sec = 384 seconds
	// Ergo, we set the limit to 400 seconds.
	int i;
	for(i=0; i<400; i++){
		
		// Get status of switches
		char statusX[256];
		char statusY[256];
		VS4cmd("AX QA", statusX, 256, 1);
		VS4cmd("AY QA", statusY, 256, 1);
		
		// Check if both have their "Done" flag set
		if(statusX[1]=='D' && statusY[1]=='D')break;
		
		// Sleep for 1 second and then check again
		sleep(1);
	}
	
	// Warn user if we went 400 seconds without ever hitting the limit.
	if(i>=400){
		fprintf(stderr,"ERROR: Waited at least 400 seconds for move to complete\n");
		fprintf(stderr,"ERROR: and it never did!\n");
		return -1;
	}else{
		// Request the axis status for each axis, resetting the done and limit flags
		VS4cmd("AX RA AY RA ", NULL, 0, 0);
		
		// Request the position of each axis and report it
		Laser_GetPosition(x, y);
		printf("Now at position (%f , %f)\n", x, y);
	}
	
	return 0;
}

//----------------
// Laser_GetPosition
//----------------
int Laser_GetPosition(float &x, float &y)
{
	// Get the current position of the laser as reported by the VS4.
	// Note that it is possible that this comes from the VS4 counting
	// pulses it sends to the motor so it may not be entirely accurate
	// if any slippage has occured.

	// Request the position of each axis
	char statusX[256];
	char statusY[256];
	VS4cmd("AX RP", statusX, 256, 1);
	VS4cmd("AY RP", statusY, 256, 1);
	int NstepsX = atoi(statusX);
	int NstepsY = atoi(statusY);
	
	// Convert from steps to inches
	x = (float)NstepsX/steps_per_inchX;
	y = (float)NstepsY/steps_per_inchY;

	return 0;
}

//----------------
// Laser_MoveRelative
//----------------
int Laser_MoveRelative(const char *sdx, const char *sdy)
{
	// Move the laser by the specified amounts  dx and dy. The 
	// units of dx and dy are inches and are measured relative
	// to the current position.
	float dx = atof(sdx);
	float dy = atof(sdy);
	
printf("dx=%f  dy=%f  sdx=\"%s\"  sdy=\"%s\"\n",dx,dy,sdx,sdy);
	
	// The maximum distance should be less than 48 inches.
	// Since we're using the limit switches, we can give any
	// value greater than this (e.g. 100 inches).
	int NstepsX = (int)(dx*steps_per_inchX);
	int NstepsY = (int)(dy*steps_per_inchY);
	char cmd[256];
	sprintf(cmd, "AA MR%d,%d; GD", NstepsX, NstepsY);
	
	// Send the command
	VS4Command(cmd);
	//VS4cmd(cmd, NULL, 0, 0);

	// Loop until both axes are done. We do the loop
	// as a precaution against looping infinitely. The maximum
	// time it should take is:
	//  48 inches * 8000 steps/inch / 1000 steps/sec = 384 seconds
	// Ergo, we set the limit to 400 seconds.
	int i;
	for(i=0; i<400; i++){
			
		// Sleep for 1 second between checks. Since we're checking the
		// velocity, we also want to give it time to ramp up before
		// checking the first time.
		sleep(1);
	
		// Get status of switches
		char statusX[256];
		char statusY[256];
		VS4cmd("AX RV", statusX, 256, 1);
		VS4cmd("AY RV", statusY, 256, 1);
		
		// Check if both have their "Done" flag set
		if(statusX[0]=='0' && statusY[0]=='0')break;
	}
	
	// Warn user if we went 400 seconds without ever hitting the limit.
	if(i>=400){
		fprintf(stderr,"ERROR: Waited at least 400 seconds for move to complete\n");
		fprintf(stderr,"ERROR: and it never did!\n");
		return -1;
	}else{
		// Request the axis status for each axis, resetting the done and limit flags
		VS4cmd("AX RA AY RA ", NULL, 0, 0);
		
		// Request the position of each axis and report it
		float x,y;
		Laser_GetPosition(x, y);
		printf("Now at position (%f , %f)\n", x, y);
	}
	
	return 0;
}