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(float x, float y);
int Laser_GetPosition(float &x, float &y);
int Laser_MoveRelative(const char *sdx, const char *sdy);
int Laser_MoveVertical(void);
int Laser_MoveHorizontal(void);

};

#include "vs4.h"

// Conversion factors
#define steps_per_inchX (1000/12)
#define steps_per_inchY 6395 // inconvenient number due to thread count?

// 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(char *sx, char *sy)
{
	float x = atof(sx);
	float y = atof(sy);
	
	// 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.
	
	// 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);

	// Allowing time for limit switches to decompress
	float originalX, originalY;
	Laser_GetPosition(originalX, originalY);
	char statusX[256];
	char statusY[256];
	VS4cmd("AX QA", statusX, 256, 1);
	VS4cmd("AY QA", statusY, 256, 1);
	if(originalX>24 && statusX[0]=='M') sleep(1);
	if(originalX<24 && statusX[0]=='P') sleep(1);
	if(originalY>24 && statusY[0]=='M') sleep(1);
	if(originalY<24 && statusY[0]=='P') sleep(1);

	// 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
		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);

	// Allowing time for limit switches to decompress
	float originalX, originalY;
	Laser_GetPosition(originalX, originalY);
	char statusX[256];
	char statusY[256];
	VS4cmd("AX RV", statusX, 256, 1);
	VS4cmd("AY RV", statusY, 256, 1);
	if(originalX>24 && statusX[0]=='M') sleep(1);
	if(originalX<24 && statusX[0]=='P') sleep(1);
	if(originalY>24 && statusY[0]=='M') sleep(1);
	if(originalY<24 && statusY[0]=='P') sleep(1);

	// 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
		VS4cmd("AX RV", statusX, 256, 1);
		VS4cmd("AY RV", statusY, 256, 1);
		
		// Check if both have their "Done" flag set
		if(statusX[0]=='0' && statusX[2]&& statusY[0]=='0')break;

		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
		float x,y;
		Laser_GetPosition(x, y);
		printf("Now at position (%f , %f)\n", x, y);
	}
	
	return 0;
}

//----------------------------
//Laser_MoveRelative()
//----------------------------
/*int Laser_MoveRelative(float sdx, float sdy)
{
	char dx[256], dy[256];
	sprintf(dx, "%f", sdx);
	sprintf(dy, "%f", sdy);
	printf("((((( sdx = %f sdy = %f dx = \"%s\" dy = \"%s\" )))))\n", sdx, sdy, dx, dy);
	Laser_MoveRelative_Method(dx, dy);

	return 0;
}
*/

//---------------------------
// Laser_MoveVertical
// Moves laser vertically away from currently active limit switch to opposite limit switch.
//---------------------------
int Laser_MoveVertical(void) 
{
	// determining current status
	char statusY[256];
	VS4cmd("AY RA", statusY, 256, 1);
	int NstepsY = (int)(100.0*steps_per_inchY);
	
	// sending command to move in negative direction if positive limit switch active
	if (statusY[0]=='P' && statusY[2]=='L') {
		char cmd[256];
		sprintf(cmd, "AY MR%d; GD", NstepsY*(-1));
		VS4Command(cmd);
		sleep(1);	// allowing limit switch to decompress
	}
	// sending command to move in positive direction if negative limit switch active
	else if (statusY[0]=='M' && statusY[2]=='L') {
		char cmd[256];
		sprintf(cmd, "AY MR%d; GD", NstepsY);
		VS4Command(cmd);
		sleep(1);	// allowing limit switch to decompress
	}	
	// notifying if not at limit
	else { printf("Not at vertical limit.  Vertical move failed."); }

	// Loop until vertical limit switch is 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 switch
		char statusY[256];
		VS4cmd("AY QA", statusY, 256, 1);
		
		// Check if in the "overtravel" position
		if(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{
		// 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;
}

//---------------------------
// Laser_MoveHorizontal
// Moves laser horizontally away from currently active limit switch to opposite limit switch.
//---------------------------
int Laser_MoveHorizontal(void) 
{
	// determining current status
	char statusX[256];
	VS4cmd("AX RA", statusX, 256, 1);
	int NstepsX = (int)(100.0*steps_per_inchX);
	
	// sending command to move in negative direction if positive limit switch active
	if (statusX[0]=='P' && statusX[2]=='L') {
		char cmd[256];
		sprintf(cmd, "AX MR%d; GD", NstepsX*(-1));
		VS4Command(cmd);
		sleep(1);	// allowing limit switch to decompress
	}
	// sending command to move in positive direction if negative limit switch active
	else if (statusX[0]=='M' && statusX[2]=='L') {
		char cmd[256];
		sprintf(cmd, "AX MR%d; GD", NstepsX);
		VS4Command(cmd);
		sleep(1);	// allowing limit switch to decompress
	}	
	// notifying if not at limit
	else { printf("Not at horizontal limit.  Horizontal move failed."); }

	// Loop until horizontal limit switch is hit. We do the loop
	// as a precaution against looping infinitely. The maximum
	// time it should take is:
	// 48 inches * Nsteps/inch / Nsteps/sec = S seconds
	// Ergo, we set the limit to S seconds.
	int i;
	int S = 48*steps_per_inchX*100; // 100 steps/sec for X-motion (see vs4.c)
	for(i=0; i<S; i++){
		
		// Get status of switch
		char statusX[256];
		VS4cmd("AX QA", statusX, 256, 1);
		
		// Check if in the "overtravel" position
		if(statusX[2]=='L')break;
		
		// Sleep for 1 second and then check again
		sleep(1);
	}

	// Warn user if we went S seconds without ever hitting the limit.
	if(i>=S){
		fprintf(stderr,"ERROR: Waited at least %d seconds for move to complete\n", S);
		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;
}


int Laser_Snake()
{
	Laser_Home();
	
	int i;
	for (i=0; i<50; i++)	{
		// Move one inch in negative X direction
		Laser_MoveRelative("-1", "0");

		// Get status of switch
		char statusX[256];
		VS4cmd("AX QA", statusX, 256, 1);
		
		// Check if in the "overtravel" position
		if(statusX[2]=='L')break;
		
		// Move entire vertical length
		Laser_MoveVertical();
	}

	if(i>=20000) {
		fprintf(stderr, "ERROR: Waited at least 20000 seconds for full snake scan to complete \n");
		fprintf(stderr, "and it never did.");
	}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);
	}
	
	Laser_Home();

	return 0;
}