/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/* rccl program to check the joint biasing code */

#include <stdio.h>

#ifdef VXWORKS_CFG
#define main testJb_
#endif

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

#define DIAM		100.0
#define JVEL		 10.0
#define JB_VEL		  5.0
#define TOOLZ		 50.0

char nbuf[1024];

extern changeJ0();
extern changeJ1();
extern changeX();

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

extern int setpDebug1;
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;
	POS_PTR ps, pa, pb, pc, pd, pe;
	POS_PTR psMx, paMx;
	JNTS parkj;
	JNTS jtarget;
	JNTS jbias;
	JNTS jvel;
	int mid;
	int i, k;

	float delta0;
	float delta1;

	char *interModes = "cj";

	doit = 0;
	doJustOne = UNDEF;
	startPoint = 0;

	argc--; argv++;
	while (argc)
	 { if (ARGUMENT ("-s"))
	    { argc--; argv++;
	      startPoint = atoi(*argv);
	    }
	   else if (ARGUMENT ("-p"))
	    { argc--; argv++;
	      doJustOne = atoi(*argv);
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { fprintf (stderr, "USAGE: %s [-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);

	ps = makePosition (NULL, T6, tool, EQ, start, TL, tool);
	pa = makePosition (NULL, T6, tool, EQ, start, a, TL, tool);
	paMx = makePosition (NULL, T6, tool, EQ, start, mx, a, TL, tool);
	psMx = makePosition (NULL, T6, tool, EQ, start, mx, 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);

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

	if (mnp->jls->jtype[0] == 'R')
	 { delta0 = DEGTORAD*10.0;
	 }
	else
	 { delta0 = 0.075*mnp->jls->jrng[0];
	 }

	if (mnp->jls->jtype[1] == 'R')
	 { delta1 = DEGTORAD*10.0;
	 }
	else
	 { delta1 = 0.075*mnp->jls->jrng[1];
	 }

	transMotionEval (mx, changeX, 25, mnp);

	rcclStart();

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

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

	xyzToTrsf (a, -DIAM, 0.0,   0.0);
	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);

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

	for (k=0; k<6; k++)
	 { jvel.v[k] = JVEL;
	 }
	setJointVel (mnp, &jvel);

	setMod (mnp, 'j');

	CHECKpnt (1, "moving by a fixed bias\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   mnp->jbias->v[0] += delta0;
	   jbiasSelect (mnp, 1);
	   movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   movej (mnp, &jtarget);
	 }
	CHECKpnt (2, "move by a fixed bias, stop, then move other joint\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   mnp->jbias->v[0] += delta0;
	   jbiasSelect (mnp, 1);
	   movej (mnp, &jtarget);
	   stop (mnp, 2000.0);
	   jtarget.v[1] += delta1;
	   movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   jtarget.v[1] -= delta1;
	   movej (mnp, &jtarget);
	 }
	CHECKpnt (3, "repeat, but idle instead of stop\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   mnp->jbias->v[0] += delta0;
	   jbiasSelect (mnp, 1);
	   movej (mnp, &jtarget);
	   waitForCompleted (mnp);
	   delay (2000.0);
	   jtarget.v[1] += delta1;
	   movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   jtarget.v[1] -= delta1;
	   movej (mnp, &jtarget);
	 }
	CHECKpnt (4, "repeat, but track instead of idle\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   mnp->jbias->v[0] += delta0;
	   jbiasSelect (mnp, 1);
	   rcclSetModes (mnp, TRACKING_MODE);
	   movej (mnp, &jtarget);
	   waitForCompleted (mnp);
	   delay (2000.0);
	   jtarget.v[1] += delta1;
	   movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   jtarget.v[1] -= delta1;
	   movej (mnp, &jtarget);
	   rcclClearModes (mnp, TRACKING_MODE);
	 }
	CHECKpnt (5, "move to fixed bias point and pause\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   mnp->jbias->v[0] += 2*delta0;
	   jbiasSelect (mnp, 1);
	   mid = movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   movej (mnp, &jtarget);
	   waitForMidpoint (mid);
	   rcclPause (F_DEFAULT, 2000.0);
	 }
	CHECKpnt (6, "move to fixed bias point and interrupt the motion\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   mnp->jbias->v[0] += 2*delta0;
	   jbiasSelect (mnp, 1);
	   mid = movej (mnp, &jtarget);
	   stop (mnp, 2000.0);
	   jbiasSelect (mnp, 0);
	   movej (mnp, &jtarget);
	   waitForMidpoint (mid);
	   stopMotion (mid, 0);
	 }
	CHECKpnt (7, "move, stop, and move using a jbias fxn\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   jbiasMotionEval (changeJ0, (int)(1000*delta0/2), mnp);
	   jbiasSelect (mnp, 1);
	   jtarget.v[0] += delta0;
	   movej (mnp, &jtarget);
	   stop (mnp, 1000.0);
	   jtarget.v[1] += delta1;
	   movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   jtarget.v[1] -= delta1;
	   jtarget.v[0] -= delta0;
	   movej (mnp, &jtarget);
	 }
	CHECKpnt (8, "repeat, only idle instead of stopping\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   jbiasMotionEval (changeJ0, (int)(1000*delta0/2), mnp);
	   jbiasSelect (mnp, 1);
	   jtarget.v[0] += delta0;
	   movej (mnp, &jtarget);
	   waitForCompleted (mnp);
	   delay (2000.0);
	   jtarget.v[1] += delta1;
	   movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   jtarget.v[1] -= delta1;
	   jtarget.v[0] -= delta0;
	   movej (mnp, &jtarget);
	 }
	CHECKpnt (9, "repeat, but track instead of idle, and pause track\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   jbiasMotionEval (changeJ0, (int)(1000*delta0/2), mnp);
	   jbiasSelect (mnp, 1);
	   rcclSetModes (mnp, TRACKING_MODE);
	   jtarget.v[0] += delta0;
	   movej (mnp, &jtarget);
	   waitForCompleted (mnp);
	   delay (1000.0);
	   rcclPause (F_DEFAULT, 2000.0);
	   delay (3000.0);
	   jtarget.v[1] += delta1;
	   movej (mnp, &jtarget);
	   jbiasSelect (mnp, 0);
	   jtarget.v[1] -= delta1;
	   jtarget.v[0] -= delta0;
	   movej (mnp, &jtarget);
	   rcclClearModes (mnp, TRACKING_MODE);
	 }
	CHECKpnt (10, "now add jbias fxn to a Cartesian target\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   jbiasMotionEval (changeJ1, (int)(1000*delta1/2), mnp);
	   jbiasSelect (mnp, 1);
	   move (mnp, pa);
	   stop (mnp, 1000.0);
	   setJbiasVarb (mnp);
	   move (mnp, ps);
	   jbiasSelect (mnp, 0);
	   move (mnp, ps);
	 }
	CHECKpnt (11, "repeat, but pause during initial motions and stops\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   jbiasMotionEval (changeJ1, (int)(1000*delta1/2), mnp);
	   jbiasSelect (mnp, 1);
	   mid = move (mnp, pa);
	   waitForMidpoint (mid);
	   rcclPause (F_DEFAULT, 2000.0);
	   mid = stop (mnp, 1000.0);
	   waitForMidpoint (mid);
	   rcclPause (F_DEFAULT, 2000.0);
	   setJbiasVarb (mnp);
	   move (mnp, ps);
	   jbiasSelect (mnp, 0);
	   move (mnp, ps);
	 }
	CHECKpnt (12, "repeat, only idle instead of stopping\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   jbiasMotionEval (changeJ1, (int)(1000*delta1/2), mnp);
	   jbiasSelect (mnp, 1);
	   move (mnp, pa);
	   waitForCompleted (mnp);
	   delay (2000.0);
	   setJbiasConst (mnp);
	   move (mnp, ps);
	   jbiasSelect (mnp, 0);
	   move (mnp, ps);
	 }
	CHECKpnt (13, "now add a jbias fxn and a moving Cartesian target\n");
	if (doit)
	 { zeroJnts (mnp->jbias);
	   identTrsf (mx);
	   jbiasMotionEval (changeJ1, (int)(1000*delta1/2), mnp);
	   move (mnp, pa);
	   jbiasSelect (mnp, 1);
	   setTime (mnp, F_DEFAULT, 2000.0);
	   move (mnp, psMx);
	   stop (mnp, 1000.0);
	   setJbiasVarb (mnp);
	   move (mnp, ps);
	   jbiasSelect (mnp, 0);
	   move (mnp, ps);
	 }
	waitForCompleted (mnp);

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

	rcclRelease (1);

}

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);
	      while (getchar() != '\n')
		 ;
	    }
	   else
	    { if (str)
	       { printf ("%s", str);
	       }
	    }
	   doit = 1;
	 }
}

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