/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : setp.c
 *      Remarks : This monster is the interrupt driven trajectory generator.
 *                It maintains the time scheduling.
 *                Each path segment it looks into the queue, pops out requests
 *                and stores them in internal static variables.
 *                Computes the delta motions for the transitions.
 *                If moving coordinate frames, does something special point 2
 *                of the transition.
 *                Does also monitoring of forces and distances.
 *                Most things happen at the first point of the transition.
 *                Note that it is completely independant from the type of
 *                arm it controls. Headache generator.
 *      Usage   : part of the library
 */

/*
 * Et Dieu dit :
 *
 * T6 = COORD POS DRIVE COMPLY TOOL  (Cartesian)
 *
 * T6 = COORD POS COMPLY TOOL  (Joint int)
 *
 *      COORD, COMPLY, TOOL   : optional
 */


#include "../h/switch.h"
#include "../h/rccl.h"
#include "../h/manip.h"
#include "../h/umac.h"

#define STOP_SEGT       10
#define STOP_ACCT       5
#ifndef REAL
#define TR_to_JNS(j, t, c)      if(tr_to_jns_n(j, t, c)) {\
					giveup("joint(s) limit", NO);\
					return;}
#else
#define TR_to_JNS(j, t, c)      (void)tr_to_jns_n(j, t, c)
#endif


static TRFN monfn = (TRFN)NULL; /* pointer to the user's monitor function*/





setpoint_n() /*::*/
{
	/*
	 * static vars
	 */

	static JNTS jba,                        /* jb - ja              */
		    jbc,                        /* jb - jc              */
		    jabc,                       /* poly term            */
		    jba2,                       /* .........            */
		    ja,                         /* joints in A          */
		    jb,                         /* joints in B          */
		    jc = {"           "},       /* goal joints stores conf */
		    jo;                         /* last joints          */

	static DRVP drvbc,                      /* drive par b - c      */
		    drvba,                      /* ....  ... b - a      */
		    drabc,                      /* poly term            */
		    drba2,                      /* .... ....            */
		    drvu,                       /* differential drive par */
		    drvv;                       /* ............ ..... ... */

						/* canonical equation terms */
	static TRSF t6b  = {"T6B", const},      /* T6 in B              */
		    t6v  = {"T6V", const},      /* T6 when moving frames*/
		    t6o  = {"T6O", const},      /* last T6              */
		    posa = {"POSA", const},     /* POS in A             */
		    posb = {"POSB", const},     /* POS in B             */
		    posc = {"POSC", const},     /* POS in C             */
		    poso = {"POSO", const},     /* last POS             */
		    posv = {"POSV", const},     /* POS when moving frames */
		    coor = {"COOR", const},     /* COORD part           */
		    tool = {"TOOL", const},     /* TOOL part            */
		    coin = {"COIN", const},     /* COORD inverse        */
		    toin = {"TOIN", const},     /* TOOL inverse         */
		    cooo = {"COOO", const},     /* last COORD           */
		    tooo = {"TOOO", const},     /* last TOOL            */
		    drive= {"DRIVE", const},    /* current DRIVE        */
		    dist = {"DIST", const},     /* distance modifier    */
		    comply = {"COMPLY", const}; /* COMPLY accomodation  */

	static  int lastmode = '?',             /* mode last motion     */
		    currmode = '?',             /* mode of this motion  */
		    errset = OK,                /* tells in error state */
		    code = OK;                  /* current status       */

	static  int segmentime = 0,             /* current time segment */
		    tsg = 0,                    /* -ta1 -> segmentime - ta2 */
		    ta1 = 0,                    /* transition time      */
		    ta2 = 0;                    /* .......... .... of path */

	static bool armbefa = NO,               /* last point of path   */
		    armina = YES,               /* first point of path  */
		    armafta = NO;               /* second point of path */

	static real fmassobj;                   /* current mass of object */
	static JNTS tobj;                       /* corresponding torques  */

	static int  forsel;                     /* force selection word */
	static FORCE limfor;                    /* limit values         */

	static int cpysel;                      /* comply selection word */
	static FORCE cpyfor;                    /* forces values        */

	static int difsel;                      /* diff motion sel. word */
	static DIFF difmax;                     /* limit values         */

	static int exdsel;                      /* distance selection   */

	static bool lastfns = NO,               /* last motion had functions*/
		    nowfns = NO;                /* current motion has ...*/

	static real trat;                       /* term of poly         */

	static TERM_PTR updt = NULL;            /* term to update       */

	/*
	 * dynamic vars
	 */

	PST_PTR lsp = (PST_PTR)lastpos,         /* make casting now     */
		glp = (PST_PTR)goalpos;
	DRVP drvh;                              /* temp drive pars      */
	real transvel, rotvel;                  /* for computing seg time */
	TRSF temp;                              /* temp transform       */
	bool forcedtrans = NO;                  /* YES when motion int. */
	int newsmpl = 0;                        /* sample change        */
	JNTS jerr;                              /* joint pos err        */
	JNTS torques;                           /* to apply in comply m */
	JNTS obsj;                              /* observed joint pos   */
	DIFF tlerr,                             /* pos err in tool frame */
	     t6err;                             /* ... ... in t6 frame  */
	int cpyjs = 0;                          /* comply joint selection */

	char *strcpy();

#ifdef FAKE
	fprintf(stderr, "%d\n", rtime);         /* this can be removed  */
#endif

	if (force_ctl) {                /* force stuff active   */
		getobsj_n(&obsj);       /* get observed pos     */
	}
/*
 * CALL USER MONITOR FUNCTION
 */
	if (monfn != (TRFN)NULL && tsg > -ta1 + 1) {
		(* monfn)();                            /* call user's  */
		if (terminate) {                        /* if user set it*/
			return;
		}
	}

/*
 * UPDATE GRAVITY LOAD DUE TO OBJECT MASS
 *
 * do it anyway to get U5 updated
 */
	{

		FORCE fg6;      /* gravity in link 6    */

		static FORCE forgrav = {0., 0., 0., 0., 0., 0.};
		static TRSF y = {"Y", const,
			1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.};

		forgrav.f.z = -fmassobj;
		Takerot(&y, t6);
		if (glp->toolp) {
			Trmultinp(&y, &tool);
		}
		Forcetr(&fg6, &forgrav, &y);
		jacobT_n(&tobj, &fg6);
	}

/*
 * DO FORCE STOP
 */
	if (forsel) {
		FORCE ft6;                      /* force in link 6      */
		JNTS jtor;                      /* corresp. joint torques */
		JNTS jter;                      /* observed ..... ....... */
		real n1, n2, n3, n4, n5, n6, n, d;

		if (glp->toolp) {
			Forcetr(&ft6, &limfor, &tool);
		}
		else {
			Assignforce(&ft6, &limfor);
		}
		jacobT_n(&jtor, &ft6);
		getobst_n(&jter);

		/*               T        T
		 *      compute t  err / t  t
		 */

		d = jtor.th1 * jtor.th1 + jtor.th2 * jtor.th2 +
		    jtor.th3 * jtor.th3 + jtor.th4 * jtor.th4 +
		    jtor.th5 * jtor.th5 + jtor.th6 * jtor.th6;

		jter.th1 -= tobj.th1;
		jter.th2 -= tobj.th2;
		jter.th3 -= tobj.th3;
		jter.th4 -= tobj.th4;
		jter.th5 -= tobj.th5;
		jter.th6 -= tobj.th6;
		n1 = jtor.th1 * jter.th1;
		n2 = jtor.th2 * jter.th2;
		n3 = jtor.th3 * jter.th3;
		n4 = jtor.th4 * jter.th4;
		n5 = jtor.th5 * jter.th5;
		n6 = jtor.th6 * jter.th6;

		n = FABS(n1) + FABS(n2) + FABS(n3)
		  + FABS(n4) + FABS(n5) + FABS(n6);
		if (n > d) {
			nextmove = ONF;         /* force transition     */
		}
	}


/*
 * Compute Cart changes if neccessary   (comply or diff motion stop)
 */
	if (difsel || cpysel) {
		diffjnts_n(&jerr, &obsj, j6);
		jacobD_n(&t6err, &jerr);
		if (glp->toolp) {
			Difftr(&tlerr, &t6err, &tool);
		}
		else {
			Assigndiff(&tlerr, &t6err);
		}
	}

/*
 * DO STOP DIST
 */

	if ((difsel & SELTX) && FABS(tlerr.t.x) > difmax.t.x) {
		nextmove = OND;
	}
	if ((difsel & SELTY) && FABS(tlerr.t.y) > difmax.t.y) {
		nextmove = OND;
	}
	if ((difsel & SELTZ) && FABS(tlerr.t.z) > difmax.t.z) {
		nextmove = OND;
	}
	if ((difsel & SELRX) && FABS(tlerr.r.x) > difmax.r.x) {
		nextmove = OND;
	}
	if ((difsel & SELRY) && FABS(tlerr.r.y) > difmax.r.y) {
		nextmove = OND;
	}
	if ((difsel & SELRZ) && FABS(tlerr.r.z) > difmax.r.z) {
		nextmove = OND;
	}


/*
 * COMPUTE COMPLY
 */
	if (cpysel) {
		TRSF deltacpy;                  /* delta comply */

		/*
		 * set the errors in unwanted directions to zero
		 */

		if (!(cpysel & SELFX)) {
			tlerr.t.x = 0.;
		}
		if (!(cpysel & SELFY)) {
			tlerr.t.y = 0.;
		}
		if (!(cpysel & SELFZ)) {
			tlerr.t.z = 0.;
		}
		if (!(cpysel & SELMX)) {
			tlerr.r.x = 0.;
		}
		if (!(cpysel & SELMY)) {
			tlerr.r.y = 0.;
		}
		if (!(cpysel & SELMZ)) {
			tlerr.r.z = 0.;
		}
		Df_to_tr(&deltacpy, &tlerr);
		Trmultinp(&comply, &deltacpy);          /* cummulate    */
	}


/*
 * SEE IF NEXTMOVE SET, INCREMENT SEGMENT TIME
 *
 * tsg varies from (-ta1) to (segmentime - ta2) unless interrupted
 */

	armbefa = tsg == segmentime - ta2 - 1;  /* compute 'before A' pred  */

	if (nextmove) {                         /* interrupted  */
		armina = forcedtrans = YES;
		segmentime = tsg + ta2;
		code = nextmove;
		nextmove = NO;
	}
	if (cpysel) {
		forcedtrans = YES;              /* no func. trans. smooth */
	}
	++tsg;

/*
 * FIRST POINT OF TRANSITION, GET NEW INFO FROM QUEUE, STOP IF NOTHING
 */
	if (armina) {
		static int stopped = NO;
		MOT_PTR newm;                   /* new motion record    */

		Assigntr(here, t6);             /* update here, user trans */
		if (updt != NULL) {
			if (updt->rhs) {
				solvei_n(updt->altr, updt, updt);
			}
			else {
				solved_n(updt->altr, updt, updt);
			}
		}
		lastpos = goalpos;
		lsp = glp;
		Assigntr(&poso, &posc);
		goalpos->code = code;
		if (!stopped) {
			--(goalpos->end);       /* signal event         */
		}
		if (code == LIMIT) {            /* force move to there  */
			glp = (PST_PTR)(goalpos = there);
			Assigntr(&posc, here);
			newm = NULL;
		}
		else {
			/*
			 * get a new motion record
			 */

			newm = (MOT_PTR)dequeue_n(&mqueue_n);

			if (newm == NULL) {             /* queue empty  */
				if (completed > 0) {
					--completed;    /* signal event */
				}
				stopped = YES;          /* stop ==      */
			}
			else {
				--requestnb;
				stopped = NO;
			}
		}
		if (newm == NULL) {
			if (lastmode == '?') {  /* to get things started */
				ta2 = STOP_ACCT;
				tsg = 0;
			}
			ta1 = ta2;              /* make the shortest path */
			tsg -= segmentime;      /* as possible          */
			segmentime = ta2 + ta2;
			if (code != OK) {       /* do no transition     */
				ta1 = 0;
			}
		}
		else {
			if (newm->pst == NULL) {        /* user's stop  */
				ta1 = ta2;
				tsg -= segmentime;
				segmentime = ta2 + ta2 + newm->sgt;
			}
			else {                          /* normal case  */
				/*
				 * get all the new parameters
				 */

				TERM_PTR tp;
				TRSF_PTR trp;

				if (lastmode == '?') {
					ta2 = ta1 = newm->acct;
					tsg = -ta2;
				}
				else {
					ta1 = ta2;
					ta2 = newm->acct;
					tsg -= segmentime;
				}

				segmentime = newm->sgt;
				if (segmentime == 0) {          /* use vel. */
					transvel = newm->velt;
					rotvel = newm->velr;
				}
				lastmode = currmode;
				goalpos = (POS_PTR)(glp = newm->pst);
				currmode = newm->mod;
			 (void) strcpy(jc.conf, newm->cfg);
				monfn = newm->fnt;
				updt = newm->upd;

				if ((newsmpl = newm->smpl) != 0) {
					real fact, ftim;

					fact = (real)timeincrement / (real)newsmpl;
					ftim = tsg;
					ftim *= fact;
					tsg = ftim;
					ftim = ta1;
					ftim *= fact;
					ta1 = ftim;
					timeincrement = newsmpl;
					if (ta1 < 2) {
						ta1 = 2;
					}
				}
				fmassobj = newm->fmass;
				forsel = newm->fsp;
				Assignforce(&limfor, &(newm->frc));
				cpysel = newm->csp;
				Assignforce(&cpyfor, &(newm->cpy));
				if (!force_ctl) {
					cpysel = 0;
					forsel = 0;
				}
				if (!cpysel) {
					Assigntr(&comply, unitr);
				}
				difsel = newm->dsp;
				Assigndiff(&difmax, &(newm->dst));

				/*
				 * get the hold transforms
				 */
				for (tp = glp->t6ptr->prev;
				     tp != glp->t6ptr;
				     tp = tp->prev) {
					if (tp->trsf->fn == hold) {
						if ((trp = (TRSF_PTR)
							dequeue_n(&(tp->hd)))
						      == NULL) {
							giveup("jam", NO);
							return;
						}
						Assigntr(tp->altr, trp);
					}
				}
				exdsel = newm->exp;
				if (exdsel) {
					Df_to_tr(&dist, &(newm->exd));
					Trmult(&posc, glp->pos->altr, &dist);
				}
				else {
					Assigntr(&dist, unitr);
					Assigntr(&posc, glp->pos->altr);
				}
			}
		}
	}
	rtime += timeincrement;         /* time is passing              */

/*
 * COMPUTE DELTAS
 */

/*
 * perform some updates, compute scal
 */
	if (armina) {
		lastfns = nowfns;
		nowfns = glp->cfnsp || glp->tfnsp;
		if (lastmode == '?') {
			jns_to_tr_n(t6, j6, NO);
			Assigntr(&t6b, t6);
			assignjs_n(&jb, j6);
			assignjs_n(&jc, j6);
		 (void) strcpy(jc.conf, j6->conf);
			shifttr_n(glp);
		}
		goalpos->scal = 0.;
	}
	else {
		goalpos->scal = (real)(tsg + ta1 - 1) /
				(real)(segmentime - ta2 + ta1);
	}

/*
 * case with no transition (ta1 null)
 */
	if (armina && ta1 == 0) {
		switch (currmode) {
		case 'c' :
			/*
			 * just have to compute posb
			 */

			if (glp->coorp) {
				solvei_n(&coin, glp->t6ptr, glp->pos);
				Trmult(&posb, &coin, t6);
			}
			else {
				Assigntr(&posb, t6);
			}
			if (glp->toolp) {
				solvei_n(&toin, glp->pos, glp->t6ptr);
				Trmultinp(&posb, &toin);
			}
			if (cpysel) {
				Trmultinv(&posb, &comply);
			}
			if (terminate) {
				return;
			}
			setpar_n(&drvbc, &posb, &posc);
			break;

		case 'j' :
			/*
			 * get the canon. equation terms anyhow because
			 * tool part is needed
			 */

			assignjs_n(&jb, j6);
			if (glp->coorp) {
				solved_n(&coor, glp->t6ptr, glp->pos);
				Trmult(&t6b, &coor, glp->pos->altr);
			}
			else {
				Assigntr(&t6b, glp->pos->altr);
			}
			if (cpysel) {
				Trmultinp(&t6b, &comply);
			}
			if (glp->toolp) {
				solved_n(&tool, glp->pos, glp->t6ptr);
				Trmultinp(&t6b, &tool);
			}
			TR_to_JNS(&jc, &t6b, NO);
			diffjnts_n(&jbc, &jc, &jb);
			break;
		}
	}


/*
 * normal case
 */
	if (armina && ta1 != 0) {
		switch (currmode) {
		case 'c' :
			if (lastmode != 'c' || forcedtrans) {

				/*
				 * extrapolate t6b
				 */

				setpar_n(&drvh, &t6o, t6);
				fopar_n(&drvh, &drvh,
					(DRVP_PTR)NULL, (real)ta1);
				drivefn_n(&drive, &drvh);
				Trmult(&t6b, t6, &drive);
			}
			else {
				/*
				 * compute t6b with old equation
				 * redo the computations only if moving
				 * frames are involved
				 */

				POS_PTR savepos;

				savepos = goalpos;      /* cheat to get */
				goalpos = lastpos;      /* to get scal  */
				goalpos->scal = 1.;     /* 0 ... .99 1. */

				if (lsp->coorp && lsp->cfnsp) {
					solvedo_n(&cooo, lsp->t6ptr, lsp->pos);
				}
				if (lsp->toolp && lsp->tfnsp) {
					solvedo_n(&tooo, lsp->pos, lsp->t6ptr);
				}
				if (lsp->pos->rhs) {
					Assigntr(&temp, &poso);
				}
				else {
					Invert(&temp, &poso);
				}
				if (lsp->coorp) {
					Trmult(&t6b, &cooo, &temp);
				}
				else {
					Assigntr(&t6b, &temp);
				}
				if (lsp->toolp) {
					Trmultinp(&t6b, &tooo);
				}

				if (lastfns) {

					/*
					 * we need to compute t6 with old
					 * equation that has moved meanwhile
					 * coord and tool are ready
					 */

					if (lsp->coorp) {
						Trmult(t6, &cooo, &posb);
					}
					else {
						Assigntr(t6, &posb);
					}
					Trmultinp(t6, &drive);
					if (lsp->toolp) {
						Trmultinp(t6, &tooo);
					}
				}
				goalpos = savepos;
			}

			/*
			 * compute posa posb from canon. equation
			 */

			if (glp->coorp) {
				solvei_n(&coin, glp->t6ptr, glp->pos);
				Trmult(&posa, &coin, t6);
				Trmult(&posb, &coin, &t6b);
			}
			else {
				Assigntr(&posa, t6);
				Assigntr(&posb, &t6b);
			}
			if (glp->toolp) {
				solvei_n(&toin, glp->pos, glp->t6ptr);
				Trmultinp(&posa, &toin);
				Trmultinp(&posb, &toin);
			}
			if (cpysel) {
				Invert(&temp, &comply);
				Trmultinp(&posa, &temp);
				Trmultinp(&posb, &temp);
			}
			if (terminate) {
				return;
			}
			setpar_n(&drvba, &posb, &posa);
			setpar_n(&drvbc, &posb, &posc);

			/*
			 * compute corrective terms first point of transition
			 * only in the case of continuous cart mode and
			 * no forced trans and last motion had moving frames
			 */

			drvv.dx = drvv.dy = drvv.dz =
			drvv.dphi = drvv.dthe = 0.;
			if (lastmode == 'c' && lastfns && !forcedtrans) {
				if (glp->coorp) {
					Trmult(&posv, &coin, &t6v);
				}
				else {
					Assigntr(&posv, &t6v);
				}
				if (glp->toolp) {
					Trmultinp(&posv, &toin);
				}
				setpar_n(&drvu, &posv, &posb);
			}
			else {
				drvu.dx = drvu.dy = drvu.dz =
				drvu.dphi = drvu.dthe = 0.;
			}

			/*
			 * adjust psi
			 */

			if (drvbc.dpsi - drvba.dpsi > pib2_m) {
				drvba.dpsi += pi_m ;
				drvba.dthe = - drvba.dthe;
			}
			if (drvba.dpsi - drvbc.dpsi > pib2_m) {
				drvba.dpsi -= pi_m;
				drvba.dthe = - drvba.dthe;
			}
			break;

		case 'j' :

			/*
			 * if not continuous joint mode extrapolate pos jB
			 */

			if (lastmode != 'j' || forcedtrans) {
				fojnts_n(&jb, jd, j6, (real)ta1);
			}
			else {
				assignjs_n(&jb, &jc);
			}
			assignjs_n(&ja, j6);
			if (glp->coorp) {
				solved_n(&coor, glp->t6ptr, glp->pos);
				Trmult(&t6b, &coor, &posc);
			}
			else {
				Assigntr(&t6b, &posc);
			}
			if (cpysel) {
				Trmultinp(&t6b, &comply);
			}
			if (glp->toolp) {
				solved_n(&tool, glp->pos, glp->t6ptr);
				Trmultinp(&t6b, &tool);
			}
			TR_to_JNS(&jc, &t6b, NO);
			diffjnts_n(&jba, &ja, &jb);
			diffjnts_n(&jbc, &jc, &jb);
			break;
		}
	}

/*
 * COMPUTE SEGMENT TIME IF VELOCITY SPECIFIED AND SOME COEF VALID FOR ONE PATH
 */
	if (armina) {
		if (segmentime == 0) {
			real sg1, sg2, sgtm;

			if (currmode == 'j') {
				setpar_n(&drvbc, t6, &t6b);
			}
			sg1 = sqrt(drvbc.dx * drvbc.dx
				 + drvbc.dy * drvbc.dy
				 + drvbc.dz * drvbc.dz)
				 / transvel;
			sg2 = sqrt(drvbc.dphi * drvbc.dphi
				 + drvbc.dthe * drvbc.dthe) * rdtodg_m
				 / rotvel;
			sgtm = ((sg1 > sg2) ? sg1 : sg2) * 1000. / timeincrement;
			segmentime = sgtm + ta2 + ta2;
			if (segmentime < STOP_SEGT) {
				segmentime = STOP_SEGT;
			}
		}

		/*
		 * compute terms of the polynomials
		 */

		if (ta1 !=  0) {
			trat = (real)ta1 / (real)segmentime;
			switch (currmode) {
			case 'c' :
				fopar_n(&drabc, &drvbc, &drvba, trat);
				t2par_n(&drba2, &drvba);
				break;

			case 'j' :
				fojnts_n(&jabc, &jbc, &jba, trat);
				t2jnts_n(&jba2, &jba);
				break;
			}
		}
	}

/*
 * ALLOW SOME PRINTS FOR DEBUG VERSIONS
 */

#ifndef REAL
	if (prints_out && armina) {
		DIFF exdist;

		fprintf(fpi, "%s %d %d %c %d %d %d %d\n",
			goalpos->name,
			lastpos->code,
			rtime,
			currmode,
			ta1 * timeincrement,
			ta2 * timeincrement,
			segmentime * timeincrement,
			timeincrement);
		fprintf(fpi, "\tforce 0%o %g %g %g %g %g %g\n",
			forsel,
			limfor.f.x, limfor.f.y, limfor.f.z,
			limfor.m.x, limfor.m.y, limfor.m.z);
		fprintf(fpi, "\tcply  0%o %g %g %g %g %g %g\n",
			cpysel,
			cpyfor.f.x, cpyfor.f.y, cpyfor.f.z,
			cpyfor.m.x, cpyfor.m.y, cpyfor.m.z);
		fprintf(fpi, "\tdst   0%o %g %g %g %g %g %g\n",
			difsel,
			difmax.t.x, difmax.t.y, difmax.t.z,
			difmax.r.x, difmax.r.y, difmax.r.z);
		Tr_to_df(&exdist, &dist);
		fprintf(fpi, "\texd   0%o %g %g %g %g %g %g\n",
			exdsel,
			exdist.t.x, exdist.t.y, exdist.t.z,
			exdist.r.x, exdist.r.y, exdist.r.z);
	}
#endif


/*
 * COMPUTE CORRECTIONS SECOND POINT OF TRANSITION
 *
 * do that only if the path has moving frames and there is a transition
 */
	if (armafta && nowfns && ta1 != 0) {
		switch (currmode) {
		case 'c' :

			/*
			 * compute the differential POS change
			 */

			if (glp->coorp) {
				if (glp->cfnsp) {
					solvei_n(&coin, glp->t6ptr, glp->pos);
				}
				Trmult(&posv, &coin, &t6b);
			}
			else {
				Assigntr(&posv, &t6b);
			}
			if (glp->toolp) {
				if (glp->tfnsp) {
					solvei_n(&toin, glp->pos, glp->t6ptr);
				}
				Trmultinp(&posv, &toin);
			}
			if (cpysel) {
				Trmultinv(&posv, &comply);
			}
			setpar_n(&drvv, &posv, &posb);
			break;

		case 'j' :

			/*
			 * compute the position we expect to be at the
			 * end of path in cart space
			 */

			if (glp->coorp) {
				solved_n(&coor, glp->t6ptr, glp->pos);
				Trmult(&t6v, &coor, &posc);
			}
			else {
				Assigntr(&t6v, &posc);
			}
			if (cpysel) {
				Trmultinp(&t6v, &comply);
			}
			if (glp->toolp) {
				solved_n(&tool, glp->pos, glp->t6ptr);
				Trmultinp(&t6v, &tool);
			}
			setpar_n(&drvh, &t6b, &t6v);
			fopar_n(&drvh, &drvh,
			(DRVP_PTR)NULL, (real)segmentime);
			drivefn_n(&drive, &drvh);
			Trmultinp(&t6b, &drive);
			TR_to_JNS(&jc, &t6b, NO);
			diffjnts_n(&jbc, &jc, &jb);
			fojnts_n(&jabc, &jbc, &jba, trat);
			break;
		}
	}

/*
 * SELECT COMPLIANT JOINT(S)
 */
	if (cpysel) {
		if (glp->toolp) {
			cpyjs = select_n(cpysel, &tool);
		}
		else {
			cpyjs = select_n(cpysel, (TRSF_PTR)NULL);
		}
	}


/*
 * COMPUTE CURRENT POINT
 */
	Assigntr(&t6o, t6);
	assignjs_n(&jo, j6);

	switch(currmode) {
	real h, hm2, hv, hmv;

	case 'c' :
		if (tsg < ta1 && ta1 != 0) {    /* transition   */
			int tas;

			h = (real)(tsg + ta1) / (real)(ta1 + ta1);
			hm2 = (2. - h) * h * h;
			tas = ta1 - 1;
			hv = (real)(tsg + tas) / (real)(tas + tas);
			hmv = hv * (hv * (hv - 2.) + 1.) * (ta1 + ta1);
			drvh.dpsi = (drvbc.dpsi - drvba.dpsi) * h
				    + drvba.dpsi;

			polypar_n(&drvh, &drabc, hm2, &drba2, h, &drvba,
				&drvu, hmv, &drvv, -hmv);
		}
		else {                          /* straight      */
			h = (real)tsg / (real)segmentime;
			drvh.dpsi = drvbc.dpsi;
			fopar_n(&drvh, &drvbc, (DRVP_PTR)NULL, h);
		}
		drivefn_n(&drive, &drvh);

		/*
		 * avoid here to redo computations that have been done
		 * when computing the deltas or the corrections
		 */

		if (!(armina || (armafta && nowfns)) || ta1 == 0) {
			if (glp->coorp && glp->cfnsp) {
				solvei_n(&coin, glp->t6ptr, glp->pos);
			}
			if (glp->toolp && glp->tfnsp) {
				solvei_n(&toin, glp->pos, glp->t6ptr);
			}
		}

		/*
		 * compute t6 and j6
		 */

		if (glp->coorp) {
			Invert(&coor, &coin);
			Trmult(t6, &coor, &posb);
		}
		else {
			Assigntr(t6, &posb);
		}
		Trmultinp(t6, &drive);

		if (cpysel) {
			Trmultinp(t6, &comply);
		}
		if (glp->toolp) {
			Invert(&tool, &toin);
			Trmultinp(t6, &tool);
		}
		TR_to_JNS(j6, t6, YES);

		if (armbefa) {                  /* save the hold transforms */
			shifttr_n(glp);         /* if regular transition    */
		}                               /* will occur next time     */

		/*
		 * compute ahead coord and tool for next motion
		 * so it will not have to be done when computing deltas
		 */

		if (armbefa) {
			if (glp->coorp) {
				solvedo_n(&cooo, glp->t6ptr, glp->pos);
			}
			if (glp->toolp) {
				solvedo_n(&tooo, glp->pos, glp->t6ptr);
			}
			if (glp->pos->rhs) {
				Assigntr(&temp, &posc);
			}
			else {
				Invert(&temp, &posc);
			}
			if (glp->coorp) {
				Trmult(&t6v, &cooo, &temp);
			}
			else {
				Assigntr(&t6v, &temp);
			}
			if (glp->toolp) {
				Trmultinp(&t6v, &tooo);
			}
		}
		break;

	case 'j' :

		/*
		 * in comply mode the poly coeff need to be ajusted
		 * with the actual joint motions
		 */

		if (tsg < ta1 && ta1 != 0) {            /* transition   */
			h = (real)(tsg + ta1) / (real)(2 * ta1);
			hm2 = (2. - h) * h * h;
			polycpyc_n(&ja, &jabc, &jba2, hm2, h, &obsj, cpyjs);
			polyjnts_n(j6, &jabc, &jba2, &ja, hm2, h);
		}
		else {                                  /* straight     */
			h = (real)tsg / (real)segmentime;
			focpyc_n(&jb, &jbc, h, &obsj, cpyjs);
			fojnts_n(j6, &jbc, &jb, h);
		}

		jns_to_tr_n(t6, j6, YES);       /* t6 still need to be  */
						/* computed             */
		if (armbefa) {
			shifttr_n(glp);
		}
		break;

	case '?' :
		break;
	}

	diffjnts_n(jd, j6, &jo);                /* compute velocity     */

	if (cpyjs) {                            /* correct loads in comply */
		jacobT_n(&torques, &cpyfor);
		torques.th1 -= tobj.th1;
		torques.th2 -= tobj.th2;
		torques.th3 -= tobj.th3;
		torques.th4 -= tobj.th4;
		torques.th5 -= tobj.th5;
		torques.th6 -= tobj.th6;
	}

/*
 * SEND RESULTS
 */


#ifdef REAL
	code = jnsend_n(j6, newsmpl, cpyjs, &torques);
#else
	code = jnsend_n(glp, j6, &jo, t6, tsg * timeincrement, rtime, cpyjs,
	       (currmode == 'c')
		?       ((tsg < ta1)
			?       ((armina)
				?       'V'
				:       'H')
			:       'C')
		:       ((tsg < ta1) ?
				((armina)
				?       'E'
				:       'T')
			:       'J')
		);
#endif

/*
 * CHECK RESULTS
 *
 * with errset, code is forced to OK
 */
	if (errset != OK || goalpos == there) {
		code = OK;
	}
	if (armbefa && goalpos != there) {
		errset = code;
	}
	if (code != OK) {
		errset = nextmove = code;
	}
/*
 * SHIFT PREDICATES
 */
	armafta = armina;
	armina = armbefa;

	/* that's all   */
}



checkstate_n() /*::*/
{
/*
 * the code here has been moved at the beginning of setpoint
 * because of driver problem.
 */
}

