/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1991 by John E. Lloyd.				    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/* Program to test some of the basic trajectory generator stuff */

#include <stdio.h>

#ifdef VXWORKS_CFG
#define main testTgen_
#endif

#include "../h/rccl.h"

#define DIAM		100.0
#define VEL0		100.0
#define TOOLZ		 50.0

char nbuf[1024];

extern changeX();
extern changeZ();

MANIP *mnp;
int doit;
int doJustOne = UNDEF;
int startPoint = 0;
int automatic = NO;

extern FILE *setpClf;
FILE *fp;

main(argc, argv)
int argc;
char **argv;
{
	char **argvec = argv;

	TRSF_PTR start;
	TRSF_PTR tool;
	TRSF_PTR a, b, c, d, e;
	TRSF_PTR mx, mz; 
	POS_PTR ps;
	POS_PTR pa, pb, pc, pd, pe;
	POS_PTR pbMz, paMz, peMx;
	JNTS parkj;
	int mid;
	int i, k;
	float cvel;
	char *interModes = "cj";

	doit = 0;
	doJustOne = UNDEF;
	startPoint = 0;
	automatic = NO;

	argc--; argv++;
	while (argc)
	 { if (ARGUMENT ("-s"))
	    { argc--; argv++;
	      startPoint = atoi(*argv);
	    }
	   else if (ARGUMENT ("-p"))
	    { argc--; argv++;
	      doJustOne = atoi(*argv);
	    }
	   else if (ARGUMENT ("-a"))
	    { automatic = YES;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { fprintf (stderr, "USAGE: %s [-a] [-s <start>] [-p <point>]\n", 
			    argvec[0]);
	   exit (-1);
	 }

	rcclSetOptions (RCCL_ERROR_EXIT);

	start = allocTrans (NULL, UNDEF);
	tool = allocTrans (NULL, UNDEF);
	a  = allocTrans (NULL, UNDEF);
	b  = allocTrans (NULL, UNDEF);
	c  = allocTrans (NULL, UNDEF);
	d  = allocTrans (NULL, UNDEF);
	e  = allocTrans (NULL, UNDEF);
	mx = allocTrans (NULL, UNDEF);
	mz = allocTrans (NULL, UNDEF);

	ps = makePosition (NULL, T6, tool, EQ, start, TL, tool);
	pa = makePosition (NULL, T6, tool, EQ, start, a, TL, tool);
	pb = makePosition (NULL, T6, tool, EQ, start, b, TL, tool);
	pc = makePosition (NULL, T6, tool, EQ, start, c, TL, tool);
	pd = makePosition (NULL, T6, tool, EQ, start, d, TL, tool);

	pbMz = makePosition (NULL, T6, tool, EQ, start, mz, b, TL, tool);
	paMz = makePosition (NULL, T6, tool, EQ, start, mz, a, TL, tool);
	peMx = makePosition (NULL, T6, tool, EQ, start, mx, e, TL, tool);

	mnp = rcclCreate (getDefaultRobot(), 0);
	if (getRobotPosition (parkj.v, "rcclpark", getDefaultRobot()) <= 0)
	 { printf ("Can't get joint angle position %s\n", "rcclpark");
	   exit (-1);
	 }

	rcclStart();

	movej (mnp, &parkj);
	waitForCompleted (mnp);

	tool->p.z = TOOLZ;
	multTrsf (start, mnp->here, tool);

	xyzToTrsf (a, -DIAM, 0.0,  DIAM);
	xyzToTrsf (b,  DIAM, 0.0,  DIAM);
	xyzToTrsf (c,  DIAM, 0.0, -DIAM);
	xyzToTrsf (d, -DIAM, 0.0, -DIAM);
	xyzToTrsf (e,   0.0, 0.0,  DIAM);

	transMotionEval (mx, changeX, 50, mnp);
	transMotionEval (mz, changeZ, -50, mnp);

	move (mnp, pa);
	waitForCompleted (mnp);

	getCartVel (mnp, &cvel, NULL);
	setCartVel (mnp, VEL0, F_DEFAULT);
	setJvelScale (mnp, VEL0/cvel);

	for (i=0; interModes[i] != '\0'; i++)
	 { CHECKpnt(UNDEF, 
		     interModes[i] == 'c' ? "\nCARTESIAN mode testing:\n\n" :
					    "\nJOINT mode testing:\n\n" );

	   setMod (mnp, interModes[i]);
	    
	   CHECKpnt(UNDEF, "Stationary frames:\n\n");

	   CHECKpnt (1+i*100, "basic box\n");
	   if (doit)
	    { move (mnp, pb);
	      move (mnp, pc);
	      move (mnp, pd);
	      move (mnp, pa);
	    }
	   CHECKpnt (2+i*100, "basic box done using relative motions\n");
	   if (doit)
	    { distance (mnp, "dx dz", DIAM, DIAM);
	      move (mnp, ps);
	      distance (mnp, "dx dz", DIAM, -DIAM);
	      move (mnp, ps);
	      distance (mnp, "dx dz", -DIAM, -DIAM);
	      move (mnp, ps);
	      distance (mnp, "dx dz", -DIAM, DIAM);
	      move (mnp, ps);
	    }
	   CHECKpnt (3+i*100, "basic box with 1 second stops\n");
	   if (doit)
	    { move (mnp, pb);
	      stop (mnp, 1000.0);
	      move (mnp, pc);
	      stop (mnp, 1000.0);
	      move (mnp, pd);
	      stop (mnp, 1000.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (4+i*100, "basic box with 1 second idles\n");
	   if (doit)
	    { k = setMotionQueueSize (mnp, 0);
	      move (mnp, pb);
	      delay (1000.0);
	      move (mnp, pc);
	      delay (1000.0);
	      move (mnp, pd);
	      delay (1000.0);
	      move (mnp, pa);
	      setMotionQueueSize (mnp, k);
	    }
	   CHECKpnt (5+i*100, "interrupt half way to B\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      move (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (6+i*100, "stop half way to B\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      stop (mnp, 1000.0);
	      move (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (7+i*100, "idle half way to B\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (8+i*100, "pause half way to B\n");	
	   if (doit)
	    { mid = move (mnp, pb);
	      move (mnp, pa);
	      waitForMidpoint (mid);
	      rcclPause (F_DEFAULT, 2000.0);
	    }
	   CHECKpnt(UNDEF, "\nNow test moving frames\n\n");
	   CHECKpnt (9+i*100, "move to B(t) and stop for 1.5 seconds\n");
	   if (doit)
	    { identTrsf (mz);
	      move (mnp, pbMz);
	      stop (mnp, 1500.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (10+i*100, "move to B(t) and idle for 1.5 seconds\n");
	   if (doit)
	    { identTrsf (mz);
	      move (mnp, pbMz);
	      waitForCompleted (mnp);
	      delay (1500.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (11+i*100, "move to B(t) and track for 1.5 seconds\n");
	   if (doit)
	    { rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mz);
	      move (mnp, pbMz);
	      waitForCompleted (mnp);
	      delay (1500.0);
	      move (mnp, pa);
	      rcclClearModes (mnp, TRACKING_MODE);
	    }
	   CHECKpnt (12+i*100, "move to B(t), track, and kill the track\n");
	   if (doit)
	    { rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mz);
	      move (mnp, pbMz);
	      waitForCompleted (mnp);
	      delay (1500.0);
	      rcclClearModes (mnp, TRACKING_MODE);
	      delay (1500.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (13+i*100, "move to B(t) and then back to A(t)\n");
	   if (doit)
	    { identTrsf (mz);
	      move (mnp, pbMz);
	      move (mnp, paMz);
	      move (mnp, pa);
	    }
	   CHECKpnt (14+i*100, "interrupt half way to B(t)\n");
	   if (doit)
	    { identTrsf (mz);
	      mid = move (mnp, pbMz);
	      move (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (15+i*100, "stop half way to B(t)\n");
	   if (doit)
	    { identTrsf (mz);
	      mid = move (mnp, pbMz);
	      stop (mnp, 1000.0);
	      move (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (16+i*100, "idle half way to B(t)\n");
	   if (doit)
	    { identTrsf (mz);
	      mid = move (mnp, pbMz);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (17+i*100, "track half way to B(t)\n");
	   if (doit)
	    { rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mz);
	      mid = move (mnp, pbMz);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      move (mnp, pa);
	      rcclClearModes (mnp, TRACKING_MODE);
	    }
	   CHECKpnt (18+i*100,"pause while moving to and stopping at B(t)\n");
	   if (doit)
	    { int mid1, mid2;
	      identTrsf (mz);
	      mid1 = move (mnp, pbMz);
	      mid2 = stop (mnp, 2000.0);
	      move (mnp, pa);
	      waitForMidpoint (mid1);
	      rcclPause (F_DEFAULT, 2000.0);
	      waitForMidpoint (mid2);
	      rcclPause (F_DEFAULT, 2000.0);
	    }
	   CHECKpnt (19+i*100, "track half way to B(t), and pause\n");
	   if (doit)
	    { 
	      rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mz);
	      mid = move (mnp, pbMz);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      rcclPause (F_DEFAULT, 1000.0);
	      delay (3000.0);
	      move (mnp, pa);
	      rcclClearModes (mnp, TRACKING_MODE);
	      waitForCompleted (mnp);
	    }
	   CHECKpnt (20+i*100, "stop half way to B(t), and pause\n");
	   if (doit)
	    { int mid1, mid2;
	      identTrsf (mz);
	      mid1 = move (mnp, pbMz);
	      mid2 = stop (mnp, 1000.0);
	      move (mnp, pa);
	      waitForMidpoint (mid1);
	      stopMotion (mid1, 0);
	      waitForMidpoint (mid2);
	      rcclPause (F_DEFAULT, 2000.0);
	    }
	   CHECKpnt (21+i*100, "idle half way to B(t), and pause\n");
	   if (doit)
	    { identTrsf (mz);
	      mid = move (mnp, pbMz);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      rcclPause (F_DEFAULT, 1500.0);
	      delay (3000.0);
	      move (mnp, pa);
	    }
	   CHECKpnt(UNDEF, "now use a moving frame E(t) parallel to x\n");
   	   CHECKpnt (22+i*100, "move to E(t) and stop for 1 second\n");
	   if (doit)
	    { identTrsf (mx);
	      move (mnp, peMx);
	      stop (mnp, 1000.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (23+i*100, "move to E(t) and idle for 1 second\n");
	   if (doit)
	    { identTrsf (mx);
	      move (mnp, peMx);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (24+i*100, "move to E(t) and track for 1 second\n");
	   if (doit)
	    { rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mx);
	      move (mnp, peMx);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      move (mnp, pa);
	      rcclClearModes (mnp, TRACKING_MODE);
	    }
	   CHECKpnt (25+i*100, "interrupt half way to E(t)\n");
	   if (doit)
	    { identTrsf (mx);
	      mid = move (mnp, peMx);
	      move (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (26+i*100, "stop half way to E(t)\n");
	   if (doit)
	    { identTrsf (mx);
	      mid = move (mnp, peMx);
	      stop (mnp, 1000.0);
	      move (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (27+i*100, "idle half way to E(t)\n");
	   if (doit)
	    { identTrsf (mx);
	      mid = move (mnp, peMx);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      move (mnp, pa);
	    }
	   CHECKpnt (28+i*100, "track half way to E(t)\n");
	   if (doit)
	    { rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mx);
	      mid = move (mnp, peMx);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      move (mnp, pa);
	      rcclClearModes (mnp, TRACKING_MODE);
	    }
	   CHECKpnt(UNDEF, NULL);
	 }
	CHECKpnt(UNDEF, "\n now do some mixed mode control\n\n");

	CHECKpnt (200, "Cart (fixed target) to Joint (fixed target)\n");
	if (doit)
	 { setMod (mnp, 'c');
	   move (mnp, pb);
	   setMod (mnp, 'j');
	   move (mnp, pa);
	 }
	CHECKpnt (201, "Cart (moving target) to Joint (fixed target)\n");
	if (doit)
	 { identTrsf (mz);
	   setMod (mnp, 'c');
	   move (mnp, pbMz);
	   setMod (mnp, 'j');
	   move (mnp, pa);
	 }
	CHECKpnt (202, "Cart (fixed target) to Joint (moving target)\n");
	if (doit)
	 { identTrsf (mz);
	   setMod (mnp, 'c');
	   move (mnp, pb);
	   setMod (mnp, 'j');
	   move (mnp, paMz);
	   move (mnp, pa);
	 }
	CHECKpnt (203, "Cart (moving target) to Joint (moving target)\n");
	if (doit)
	 { identTrsf (mz);
	   setMod (mnp, 'c');
	   move (mnp, pbMz);
	   setMod (mnp, 'j');
	   move (mnp, paMz);
	   move (mnp, pa);
	 }
	CHECKpnt (204, "Joint (fixed target) to Cart (fixed target)\n");
	if (doit)
	 { setMod (mnp, 'j');
	   move (mnp, pb);
	   setMod (mnp, 'c');
	   move (mnp, pa);
	 }
	CHECKpnt (205, "Joint (moving target) to Cart (fixed target)\n");
	if (doit)
	 { identTrsf (mz);
	   setMod (mnp, 'j');
	   move (mnp, pbMz);
	   setMod (mnp, 'c');
	   move (mnp, pa);
	 }
	CHECKpnt (206, "Joint (fixed target) to Cart (moving target)\n");
	if (doit)
	 { identTrsf (mz);
	   setMod (mnp, 'j');
	   move (mnp, pb);
	   setMod (mnp, 'c');
	   move (mnp, paMz);
	   move (mnp, pa);
	 }
	CHECKpnt (207, "Joint (moving target) to Cart (moving target)\n");
	if (doit)
	 { identTrsf (mz);
	   setMod (mnp, 'j');
	   move (mnp, pbMz);
	   setMod (mnp, 'c');
	   move (mnp, paMz);
	   move (mnp, pa);
	 }
	CHECKpnt(UNDEF, NULL);

	/* --- zero acceleration section --- */

	xyzToTrsf (a, -DIAM/3.0, 0.0,  DIAM/3.0);
	xyzToTrsf (b,  DIAM/3.0, 0.0,  DIAM/3.0);
	xyzToTrsf (c,  DIAM/3.0, 0.0, -DIAM/3.0);
	xyzToTrsf (d, -DIAM/3.0, 0.0, -DIAM/3.0);

	transMotionEval (mx, changeX, 15, mnp);
	transMotionEval (mz, changeZ, -15, mnp);

	setSpeed (mnp, 0.3);

	move (mnp, pa);

	CHECKpnt(UNDEF, "\nNow test motions with zero transition times\n\n");

	for (i=0; interModes[i] != '\0'; i++)
	 { CHECKpnt(UNDEF,
		     interModes[i] == 'c' ? "\nCARTESIAN mode testing:\n\n" :
					    "\nJOINT mode testing:\n\n" );

	   setMod (mnp, interModes[i]);
	   CHECKpnt (300+i*100, "basic box, tau=0\n");
	   if (doit)
	    { moveZeroTau (mnp, pb);
	      moveZeroTau (mnp, pc);
	      moveZeroTau (mnp, pd);
	      moveZeroTau (mnp, pa);
	    }
	   CHECKpnt (301+i*100, "basic box with stops, tau=0\n");
	   if (doit)
	    { moveZeroTau (mnp, pb);
	      stopZeroTau (mnp, 1000.0);
	      moveZeroTau (mnp, pc);
	      stopZeroTau (mnp, 1000.0);
	      moveZeroTau (mnp, pd);
	      stopZeroTau (mnp, 1000.0);
	      moveZeroTau (mnp, pa);
	    }
	   CHECKpnt (302+i*100, "interrupt half way to B, tau=0\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (303+i*100, "stop half way to B, tau=0\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      stopZeroTau (mnp, 1000.0);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (304+i*100, "idle half way to B, tau=0\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      moveZeroTau (mnp, pa);
	    }
	   CHECKpnt (305+i*100, "normal pause half way to B, tau=0\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      rcclPause (F_DEFAULT, 2000.0);
	    }
	   CHECKpnt (306+i*100, "zero accel pause half way to B, tau=0\n");
	   if (doit)
	    { mid = move (mnp, pb);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      rcclPause (0.0, 2000.0);
	    }
	   CHECKpnt(UNDEF, "\nNow test moving frames\n\n");
	   
	   CHECKpnt (307+i*100, "move to B(t) and stop, tau=0\n");
	   if (doit)
	    { identTrsf (mz);
	      move (mnp, pbMz);
	      stopZeroTau (mnp, 2000.0);
	      moveZeroTau (mnp, pa);
	    }
	   CHECKpnt (308+i*100, "move to B(t) and idle, tau=0\n");
	   if (doit)
	    { identTrsf (mz);
	      move (mnp, pbMz);
	      waitForCompleted (mnp);
	      delay (2000.0);
	      moveZeroTau (mnp, pa);
	    }
	   CHECKpnt (309+i*100, "move to B(t) and track, tau=0\n");
	   if (doit)
	    { rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mz);
	      move (mnp, pbMz);
	      waitForCompleted (mnp);
	      delay (2000.0);
	      moveZeroTau (mnp, pa);
	      rcclClearModes (mnp, TRACKING_MODE);
	    }
	   CHECKpnt (310+i*100, "move to B(t) and then to A(t), tau=0\n");
	   if (doit)
	    { identTrsf (mz);
	      move (mnp, pbMz);
	      moveZeroTau (mnp, paMz);
	      moveZeroTau (mnp, pa);
	    }
	   CHECKpnt (311+i*100, "interrupt half way to B(t), tau=0\n");
	   if (doit)
	    { identTrsf (mz);
	      mid = move (mnp, pbMz);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (312+i*100, "stop half way to B(t), tau=0\n");
	   if (doit)
	    { identTrsf (mz);
	      mid = move (mnp, pbMz);
	      stopZeroTau (mnp, 1000.0);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	    }
	   CHECKpnt (313+i*100, "idle half way to B(t), tau=0\n");
	   if (doit)
	    { identTrsf (mz);
	      mid = move (mnp, pbMz);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      moveZeroTau (mnp, pa);
	    }
	   CHECKpnt (314+i*100, "track half way to B(t), tau=0\n");
	   if (doit)
	    { rcclSetModes (mnp, TRACKING_MODE);
	      identTrsf (mz);
	      mid = move (mnp, pbMz);
	      waitForMidpoint (mid);
	      stopMotion (mid, 0);
	      waitForCompleted (mnp);
	      delay (1000.0);
	      moveZeroTau (mnp, pa);
	      rcclClearModes (mnp, TRACKING_MODE);
	    }
	   CHECKpnt (315+i*100, "normal pause half way to B(t), tau=0\n");
	   if (doit)
	    { mid = moveZeroTau (mnp, pb);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      rcclPause (F_DEFAULT, 2000.0);
	    }
	   CHECKpnt (316+i*100, "zero accel pause half way to B(t), tau=0\n");
	   if (doit)
	    { mid = moveZeroTau (mnp, pb);
	      moveZeroTau (mnp, pa);
	      waitForMidpoint (mid);
	      rcclPause (0.0, 2000.0);
	    }
	   CHECKpnt(UNDEF, NULL);
	 }

	transMotionEval (mx, changeX, 50, mnp);
	transMotionEval (mz, changeZ, -50, mnp);

	xyzToTrsf (a, -DIAM, 0.0,  DIAM);
	xyzToTrsf (b,  DIAM, 0.0,  DIAM);
	xyzToTrsf (c,  DIAM, 0.0, -DIAM);
	xyzToTrsf (d, -DIAM, 0.0, -DIAM);

	setCartVel (mnp, VEL0, F_DEFAULT);
	setSpeed (mnp, 1.0);

	waitForCompleted (mnp);

	printf ("\n\nROUTINES OK\n\n");

	rcclRelease (1);
}

stopZeroTau (mnp, time)
MANIP *mnp;
float time;
{
	setTime (mnp, 0.0, F_DEFAULT);
	return (stop (mnp, time));
}

moveZeroTau (mnp, pos)
MANIP *mnp;
POS *pos;
{
	setTime (mnp, 0.0, F_DEFAULT);
	return (move (mnp, pos));
}

CHECKpnt (mark, str)
int mark;
char *str;
{
	if (doit)
	 { waitForCompleted (mnp);
	 }
	if (doJustOne != UNDEF && mark != doJustOne)
	 { doit = 0;
	   return;
	 }
	if (mark < startPoint && mark != UNDEF)
	 { doit = 0;
	   return;
	 }
	else
	 { waitForCompleted (mnp);
	   if (mark != UNDEF)
	    { printf ("%s(%d)", str ? str : NULL, mark);
	      if (!automatic)
	       { while (getchar() != '\n')
		    ;
	       }
	      else
	       { fflush(stdout);
	       }
	    }
	   else
	    { if (str)
	       { printf ("%s", str);
	       }
	    }
	   doit = 1;
	 }
}

waitForMidpoint (mid)
int mid;
{
	waitfor (motionScale (mid) > 0.5);
}
