/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	setpointCtrl
 *
 * DESCRIPTION:	The central computation routines for the RCCL
 *		trajectory generator.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1986
 *
 *-------------------------------------------------------------------------*/

#include <stdio.h>
#include <math.h>

#define EPSILON	1e-6

#include "../h/cdefs.h"
#include "../h/robotMath.h"
#include "../h/rciTask.h"
#include "../h/rcclDefs.h"
#include "../h/errorCodes.h"
#include "../h/timeval.h"
#include "../h/robot_kynvar.h"

#include "../rccl/motionQueue.h"
#include "../rccl/setpoint.h"
#include "../rccl/solveEqn.h"
#include "../rccl/kynMacros.h"
#include "../rccl/rcclErrorMsgs.h"

#include "../rci/link.h"

/* Routine to get motion request off of queue */
MOTION_REQUEST *motionRequestReady();

/* Setpoint transition actions */
#define SERVICE_NEXT_REQUEST 	1
#define ENTER_IDLE_STATE	2
#define ENTER_PAUSE_STATE	3
#define LEAVE_PAUSE_STATE	4

/* The 3 main classes of motion target */
#define TARGET_KNOWN 	1
#define TARGET_B     	2
#define TARGET_STOP	4

/* Useful abbreviations ... */
#define T6	(&m->t6)
#define J6	(&m->j6)

/* tell if we are running in simulation mode */
/* MS In linux ON_SIGNAL is not synonomous with simulation
#define SIMULATE	(m->td->tcb->scheduleDisp == ON_SIGNAL)
*/
#define SIMULATE	(m->td->tcb->scheduleDisp == ON_CLOCK)

int verbose = 0; /* MS Change printing from SIMULATE to verbose */

extern int transDebug;	/* external debugging flag */
extern int rciCpu;	/* CPU this task is running on */

/* Some internal debugging stuff ... for JL only */
int setpDebug1 = 0;
int setpDebug2 = 0;
char *setpStateStr();
FILE *setpJlf;	/* for debugging ... Joint log file */
FILE *setpClf;	/* for debugging ... Cartesian log file */

static TRSF unitr = {1.0, 0.0, 0.0, 0.0,
		     1.0, 0.0, 0.0, 0.0,
		     1.0, 0.0, 0.0, 0.0 };

setpointInit(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Called once during the first control cycle of any control
	   session. Sets up and initializes the various
	   parameter blocks. This is not the only initialization
	   routine, but it is the LAST one called. We do initialization
	   here instead of during the control session startup routine
	   (setpStartup()) to initialize many of the fields here we
	   have to information in from the robot.
	 */

	MANIP *mnp = m->mnps[rciCpu];
	int i;

	RCI_RBT *rbt = m->rbt;

	/* Initialize motion parameter block */

	m->mcb->flushReq = NO;
	m->mcb->terminateReq = NO;
	m->mcb->waitingForStart = NO;

	m->state = TGEN_IDLE;
	m->noMotionsYet = 1;
	m->ci.s = 0.0;
	setManipCompleted (m, 1);
	setManipMid (m, UNDEF);

	m->goalPos = NULL;

	m->monitorFxnHead = NULL;
	m->monitorFxnTail = NULL;
	for (i=0; i<MAX_MONITOR_FXNS-1; i++)
	 { m->fxnBlockPool[i].next = &m->fxnBlockPool[i+1];
	   m->fxnBlockPool[i].id = i+1;
	 }
	m->fxnBlockPool[MAX_MONITOR_FXNS-1].next = NULL;
	m->fxnBlockPool[MAX_MONITOR_FXNS-1].id = MAX_MONITOR_FXNS;
	m->mcb->monitorFxnsActive = 0;

	m->freeFxnBlocks = &m->fxnBlockPool[0];

	m->numMotionFxns = 0;

	m->ci.c = 1;
	m->ci.sigma = 1;
	initScale (&m->ci);
	m->motionHasTimeLimit = YES;
	m->finishCode = UNDEF;
	m->lastFinishCode = ON_NORMAL;
	m->objectLoad = 0.0;

	zeroJnts (&m->j6vel);

	m->motionNumber = 0;
	m->flushedMotionNumber = 0;

	if (transDebug && m->evalListHead)
	 { TRSF_DESC *tr;
	   for (tr = m->evalListHead; tr; tr = tr->next)
	    { rciPrintf ("manip unlinking %s (initialize)\n", 
		      getMemName(tr->parent));
	    }
	 }
	m->evalListHead = NULL;
	m->evalListTail = NULL;
	m->stopOnNextRequest = NO;
	m->doDrive = NO;

	m->lastTargetFixed = YES;
	m->lastGoalPosFixed = YES;
	m->lastJbiasFixed = YES;
	m->targetFixed = YES;
	m->goalPosFixed = YES;
	m->jbiasFixed = YES;

	m->endCount = 0;
	m->tau = 0;
	m->motionTau = 0;
	m->currentTau = 0;
	m->nextMotionRequest = NULL;
	m->nextGoalPos = NULL;
	m->nextMotionReady = 0;
	m->updateAcceleration = NO;

	if (rbt)
	 { 
	   /* If there is a robot attached to this manipulator (there
	      won't be if it is an object frame), compute the initial 
	      values of the joint angles and such */

	   J6->v[0] = rbt->how->ang[0];
	   J6->v[1] = rbt->how->ang[1];
	   J6->v[2] = rbt->how->ang[2];

	   J6->v[3] = rbt->how->ang[3];
	   J6->v[4] = rbt->how->ang[4];
	   J6->v[5] = rbt->how->ang[5];

	   FWD_KINEMATICS (T6, &m->config, J6->v, NULL);
#if 0
	   pumaSingTolEnable (J6, rbt->var);
#endif
	   m->configTarget = m->lastConfigTarget = m->config;
	   m->oldJ6 = *J6;
	   if (m->mcb->mode & T6O_EVAL)
	    { writeTrans (mnp->t6o, T6);
	      writeMem (mnp->j6o, rbt->how->ang);
	    }
	   copyJnts (&m->jC0, J6);
	   copyJnts (&m->jC, J6);
	   bzero (&m->jbias, sizeof(JNTS));
	   writeMem (mnp->jbias, &m->jbias);
	   writeMem (mnp->j6, J6);
	   bzero (mnp->jvel, sizeof(float)*6);
	   setManipHandPos (m, rbt->how->hand);
	 }
	else
	 { readTrans (mnp->t6, T6); 
	 }

	copyTrsf (&m->tC0, T6);
	copyTrsf (&m->tC, T6);
	copyTrsf (&m->lastTC0, T6);
	m->oldT6 = *T6;

	writeTrans (mnp->here, T6);
	writeTrans (mnp->t6, T6);
	writeTrans (mnp->lastTC, T6);
}

setpointControl (m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* The first of two routines called during every control cycle. 
	   This is routine is mainly responsible for flow control:
	   deciding if and when a path segment has completed, getting
	   the next path segment off the queue, and setting up the
	   next path segment. It also evaluates any transforms which are
	   bound to functions on a per-motion basis for this manipulator.
	 */

	MOTION_CONTROL_BLOCK *mcb = m->mcb;
	MANIP *mnp;
	MFLAG *mf;
	int motion_change;
	RCI_RBT *rbt;
	int k;

	mnp = m->mnps[rciCpu];
	rbt = m->rbt;
	mf = m->cbk.mflag;

	if (rbt && (mcb->mode & T6O_EVAL))
	 { /* Evaluate OBSERVED joint angles and T6 value for
	      the manipulator */
	   TRSF t6o;
	   FWD_KINEMATICS (&t6o, NULL, rbt->how->ang, NULL);
	   writeTrans (mnp->t6o, &t6o);
	   writeMem (mnp->j6o, rbt->how->ang);
	 }
	if (rbt && rbt->how->hand != mnp->handPos)
	 { setManipHandPos (m, rbt->how->hand);
	 }

	/* Code to handle permanent monitor functions */

	 { FXN_BLOCK *fbk;
	   FXN_BLOCK **headp;
	   FXN_BLOCK **tailp;

	   headp = &m->monitorFxnHead;
	   tailp = &m->monitorFxnTail;

	   /* Check to see if there is a command to change the
	      monitor functions list (either to add a function
	      or delete one). Handle this command appropriately.
	    */
	   if (mcb->monitorFxnCmd)
	    { if (mcb->monitorFxnCmd == RUN_MONITOR_FXN)
	       { /* Add a new monitor function the evaluation list */

		 if ((fbk = m->freeFxnBlocks) == NULL)
		  { rciAbort (EInternal, "out of function blocks\n");
		  }
		 m->freeFxnBlocks = fbk->next;
		 fbk->fxn = mcb->monitorFxn;
		 fbk->arg = mcb->monitorArg;
		 LINK_HT (*headp, *tailp, fbk);
		 mcb->monitorRunId = fbk->id;
		 mcb->monitorFxnsActive++;
	       }
	      else if (mcb->monitorFxnCmd == DELETE_MONITOR_FXN)
	       { /* Remove a function from the evaluation list */

		 for (fbk = *headp; fbk; fbk = fbk->next)
		  { if (fbk->id == mcb->monitorDeleteId)
		     { UNLINK_HT(*headp, *tailp, fbk);
		       fbk->next = m->freeFxnBlocks;
		       m->freeFxnBlocks = fbk;
		       mcb->monitorFxnsActive--;
		       break;
		     }
		  }
	       }
	      mcb->monitorFxnCmd = 0;
	    }

	   /* Execute all functions on the evaluation list */

	   for (fbk = *tailp; fbk; fbk = fbk->prev)
	    { (*fbk->fxn)(fbk->arg, mnp);
	    }
	 }

	/* Call motion monitor functions */

	for (k=0; k<m->numMotionFxns; k++)
	 { (*m->motionFxns[k].fxn)(m->motionFxns[k].arg,
				     mnp);
	 }

	/* Check for requests to adjust the motion arrival time */

	if (mf && mf->timeCmd)
	 { adjustArrivalTime (mf, m);
	   mf->timeCmd = 0;
	 }

	/* Bump the count for the current motion, plus any 
	   previous motion which has not yet expired */

	m->ci.c++;
	if (m->expiringCycles)
	 { m->expiringCi.c++;
	 }

	/* Check the motion queue. If things have changed there, set or
	   clear the pending motion request as required */	   
	if (!m->nextMotionRequest)
	 { MOTION_REQUEST *mr;
	   if (mr = motionRequestReady(m->mq, m))
	    { setupPendingMotion (m, mr);
	      monitorPendingMotion(m, m->nextMotionRequest);
	    }
	 }
	else
	 { monitorPendingMotion(m, m->nextMotionRequest);
	 }

	/* Check, and update if necessary, the transition time for
	   the next motion request */

	updateAcceleration (m);

	/* Check if the current path segment has finished, and if so,
	   determine what to do next. If it has finished, and it belonged
	   to a real move() or stop() request (as opposed to the idle
	   state or a pause in the motion), then the field 'finishCode'
	   will be set to some non-negative value; otherwise, it will
	   be set to UNDEF.

	   The computation for a motion ends at the beginning of the
	   transition to the next motion. However, if the RCCL mode
	   COMPLETE_AT_TB is set (which it is be default), then bookkeeping
	   associated wuth the motion's completion will be delayed until
	   the transition midpoint (when the manipulator will be the closest
	   in both time and position to the actual motion target). The
	   bookkeeping involves things such as setting the completion flag,
	   evaluating any update transform requested, etc. Information
	   needed by an expired motion which is waiting for completion
	   is stored in fields whose names begin with the prefix 'expiring'.
	   These fields include a copy of the completion block 
	   ('expiringCbk'), a count-to-zero expiration counter 
	   ('expiringCycles'), and information to allow continued computation
	   of the motion scale ('expiringSigma' and 
	   'expiringCount').
	 */

	m->finishCode = UNDEF;
	motion_change = motion_request_control(m->mq, m);
	if (motion_change)
	 { if (m->finishCode != UNDEF)	
	    { 
	      /* Close out any presently expiring path segment
		 information; we don't queue it */

	      if (m->expiringCycles)
	       { doCompletion (m, &m->expiringCbk);
		 if (mf)
		  { mf->startValue = mf->startRequest;
		    mf->status = M_RUNNING;
		  }
	       }

	      /* store new expiring motion info */

	      m->expiringCbk = m->cbk;
	      m->expiringCbk.code = m->finishCode;
	      m->expiringCi = m->ci;

	      m->lastStopMotion = m->stopMotion;
	      mcb->tailMotionNumber = m->motionNumber;
	    }

	   /* store away info about last motion segment */
	   m->lastConfigTarget = m->configTarget;
	   m->configTarget = m->config;
	   if ((m->lastInterMode = m->interMode) == CARTESIAN)
	    { m->lastTC0 = m->tC0;
	    }
	   else
	    { m->lastJC0 = m->jC0;
	    }

	   /* Call the appropriate routine to set up the next path 
	      segment.
	      --- Unstable zone for pathsegIndex begins here ---
	    */

	   switch (motion_change) 
	    {
	      case SERVICE_NEXT_REQUEST:
	       { instantiateMotionRequest (m->mq, m);
		 clearPendingMotion (m);
		 m->noMotionsYet = 0;
		 break;
	       }
	      case ENTER_IDLE_STATE:
	       { if ((mcb->mode & TRACKING_MODE) && !m->noMotionsYet)
		  { instantiateTrackingIdleState (m);
		  }
		 else
		  { instantiateIdleState (m);
		  }
	         break;
	       }
	      case ENTER_PAUSE_STATE:
	       { instantiatePauseState (m);
		 break;
	       }
	      case LEAVE_PAUSE_STATE:
	       { clearPauseState (m);
		 break;
	       }
	    }
	   mf = m->cbk.mflag;	/* this may have changed */
	   
	   /* initialize motion counters */
	   m->cycle = 0;
	   m->tau = m->currentTau;

	   if (m->finishCode != UNDEF)
	    { /* If last motion was real motion request, decide whether 
		 to record completion of the last motion segment now or 
		 later (at the transition midpoint) */
	     
	      if (!(m->mode & COMPLETE_AT_TB) || m->lastStopMotion ||
		  (m->tau == 0))
	       { doCompletion (m, &m->expiringCbk);
		 if (mf)
		  { mf->startValue = mf->startRequest;
		    mf->status = M_RUNNING;
		  }
		 m->expiringCycles = 0;
	       }
	      else
	       { m->expiringCycles = (2.0*m->tau*m->tauSkew + 2);
		 if (m->expiringCi.sigma != UNDEF &&
		     m->expiringCycles>m->expiringCi.sigma-m->expiringCi.c+2)
		  { m->expiringCycles = m->expiringCi.sigma-m->expiringCi.c+2;
		  }
	       }
	    }

	   /* clear these fields so they will be recomputed */
	   if (m->goalPos)
	    { m->tool = m->coord = NULL;
	    }

	   /* Compute the B point we were heading to, and determine
	      the path segment time if not already done so. */

	   estimateTargetAndTime(m);

	   mcb->pathsegIndex++;

	   /* --- Unstable zone for pathsegIndex ends here --- */

	   /* Update the path segment count parameters */

	    { int new;
	      new = (mcb->countIndex&1)^1;

	      mcb->activeSigma[new] = m->ci.sigma;
	      mcb->activeTau[new] = m->tau;
	      mcb->activeCount[new] = 0;

	      mcb->countIndex++;
	      if (mf)
	       { mf->startTime = m->sysTime - (m->ci.c-1)*m->sampleTime;
	       }
	    }


	   if (setpDebug1)	/* for debugging */
	    { rciPrintf ("%d %s sigma=%d endc=%d tau=%d mTau=%d\n", 
		getCurrentTask()->tcb->cycleCount,
		setpStateStr(m),
		m->ci.sigma, 
		m->endCount, m->tau, m->motionTau);
	    }

	   /* if real request just finished, update last target field */
	   if (m->finishCode != UNDEF)
	    { writeTrans (mnp->lastTC, &m->tB_est);
	    }

	   /* Compute the current motion scale number */	      

	   if (m->newMotion)
	    { initScale (&m->ci);
	    }
	   else
	    { advanceScale (&m->ci);
	    }
	 }
	else
	 { /* bump the cycle count, clear the 'coord' and 'tool' fields
	      so they will be recomputed for this cycle, and compute the
	      motion scale factor.
	    */
	   
	   m->cycle++;

	   /* Update the path segment count parameters */

	    { int new;
	      new = (mcb->countIndex&1)^1;

	      mcb->activeSigma[new] = m->ci.sigma;
	      mcb->activeTau[new] = m->tau;
	      mcb->activeCount[new] = m->cycle;

	      mcb->countIndex++;
	    }	   

	   if (m->goalPos)
	    { m->tool = m->coord = NULL;
	    }
	   advanceScale (&m->ci);
	 }
	/* set motion scale value (nominally 0.0 to 1.0) */
	if (mf)
	 { mf->scale = m->ci.s;
	 }

	/* Evaluate functional transforms tied to motions on this
	   manipulator. */

	evalManipTransforms (m->evalListTail, m->sysTime, 
			     m->ci.s, mnp);

	/* Read in, or compute, the joint bias */

	if (m->jbiasEnable)
	 { if (m->jbiasFxn.fxn == varbTrsf)
	    { readMem (mnp->jbias, &m->jbias);
	    }
	   else if (m->jbiasFxn.fxn != constTrsf)
	    { 
	      extern int oldTfxnArgs;	/* defined in transControl.c */
	      TRANS_ARGS targs;		/* for old function interface */

	      if (oldTfxnArgs)
	       { targs.scale = m->ci.s;
		 targs.time = m->sysTime;
		 targs.manip = m->mnps[rciCpu];
		 targs.arg = m->jbiasFxn.arg;
		 (*m->jbiasFxn.fxn)(&m->jbias, &targs);
	       }
	      else
	       { (*m->jbiasFxn.fxn)(&m->jbias, m->jbiasFxn.arg, mnp);
	       }
	      writeMem (mnp->jbias, &m->jbias);
	    }
	 }

	/* Save the old T6 and J6 values. */

	m->oldT6 = *T6;
	if (rbt)
	 { m->oldJ6 = *J6;
	 }
	
	/* Instantiate 'here' position after we have moved to the
	   idle position. If we are in the idle state, and we have just 
	   finished our transition, we update the 'here' transform again */

	if (m->state == TGEN_IDLE && m->cycle == 2*m->tau)
	 { writeTrans (mnp->here, T6);
	 }

	/* If there is an expiring motion, decrease the expire count,
	   and keep updateing the scale value in the MFLAG (if there
	   is an MFLAG for the motion). When the motion actually
	   does expire, call the doRompletion() routine, and 
	   officially set the current motion's status to RUNNING
	   and set its start code.
	 */

	if (m->expiringCycles)
	 { MFLAG *emf;

	   if (--m->expiringCycles == 0)
	    { doCompletion (m, &m->expiringCbk);
	      if (mf)
	       { mf->startValue = mf->startRequest;
		 mf->status = M_RUNNING;
	       }
	    }
	   else
	    { if (emf = m->expiringCbk.mflag)
	       { advanceScale (&m->expiringCi);
		 emf->scale = m->expiringCi.s;
	       }
	    }
	 }
}

setpointSolve(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* The second of two routines called every control cycle
	   for every manipulator. This routine calls the necessary
	   routines to compute the output setpoint values.
	 */
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)m->mcb;
	MANIP *mnp = m->mnps[rciCpu];
	int limit;
	RCI_RBT *rbt;

	rbt = m->rbt;

	if (m->cycle == 0)
	 { computeInitialSetpoints(m);

	   if (verbose)
	    { char infostr[256];
	      char conf[16];
	      
	      /* write the new motion info to the screen */

	      sprintf (infostr, 
		       "#%s(%g): mod=%c tseg=%d tau=%d conf=%s",
		       setpStateStr(m),
		       m->sysTime/m->sampleTime,
		       m->interMode == CARTESIAN ? 'c' : 'j',
		       m->ci.sigma, 
		       m->tau,
		       rbt ? configToStr(conf, m->configTarget, rbt->kyn)
			   : "N/A");

	      rciPrintf ("%s\n", infostr);

	      /* write the new motion info to the joint or Cartesian 
		 log files also */

	      if (setpClf)
	       { fprintf (setpClf, "%s\n", infostr);
	       }
	      if (setpJlf)
	       { fprintf (setpJlf, "%s\n", infostr);
	       }
	    }
         }
        else if (m->cycle == 1) 
	 { computeSecondarySetpoints(m);
	 }
	else
	 { computeNominalSetpoints(m);
	 }

	/* write setpoints in the Cartesain log file */
	if (SIMULATE && setpClf)
	 { DSPL dspl;
	   fprintf (setpClf, "%4d%c", m->ci.c,
			     (m->cycle < 2*m->tau) ? 'T' : ' ');
	   trsfToDspl (&dspl, T6);
	   fprintf(setpClf, "%10.4f %10.4f %10.4f  %9.3f %9.6f %9.6f %9.6f\n",
		    dspl.p.x, dspl.p.y, dspl.p.z, dspl.a*RADTODEG, 
		    dspl.u.x, dspl.u.y, dspl.u.z);
	 }

	/* OUTPUT CARTESIAN/JOINT SETPOINTS HERE, using T6/J6 */

	if (rbt)
	 { /* Update terms of the Jacobian. */
	   
	   updateVarXsincos (rbt->var, rbt->how->ang, rbt->kyn);

	   /* send joint setpoints out to the robot */
	
	   if (limit = output_setpoints (m))
	    { char buf[128];
	      jointDescStr (buf, limit);
	      rciAbort (ELimit, "setpoint() -- range error on %s\n", buf);
	      return (-1);
	    }

	   /* write the joint log file, if one is open */

	   if (SIMULATE && setpJlf)
	    { JNTS deg;
	      fprintf (setpJlf, "%4d%c", m->ci.c,
			     (m->cycle < 2*m->tau) ? 'T' : ' ');
	      radianToDegree (deg.v, J6->v, mnp->jls);
	      fprintVf (setpJlf, "%9.4f\n", deg.v, mnp->jls->numJ);
	    }

	   /* update fields in the MANIP structure */

	   subtractJnts (&m->j6vel, J6, &m->oldJ6);
	   writeMem (mnp->j6, J6);
	   writeMem (mnp->jvel, &m->j6vel);
	   if (mnp->conf != m->config)
	    { setManipConf (m, m->config);
	    }
	 }

	 { TRSF *tool;

	   writeTrans (mnp->t6, T6);
	   if (m->goalPos == NULL || (tool = GET_TOOL(m)) == NULL)
	    { writeTrans (mnp->tool, &unitr);
	    }
	   else
	    { writeTrans (mnp->tool, tool);
	    }
	 }

	/* Assert that yes, control is active */
	mcb->active = 1;

	/* The setting of the 'completed' flag is delayed until after the
	   TGEN_IDLE transition phase has been completed */

	if (!m->nextMotionRequest && m->state == TGEN_IDLE)
	 { if (mnp->completed == 0 &&
	       (m->cycle >= 2*m->tau))
	    { setManipCompleted (m, 1);
	      if (mcb->completedSig)
	       { rciSignal (mcb->completedSig);
		 mcb->completedSig = 0;
	       }
	    }
	   if (m->flushedMotionNumber > m->motionNumber)
	    { m->motionNumber = m->flushedMotionNumber;
	      mcb->tailMotionNumber = m->motionNumber;
	    }	
	 }
	else
	 { if (mnp->completed)
	    { setManipCompleted (m, 0);
	    }
	 }
}

setpointRelease(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Called by setpRelease() for each manipulator when RCCL
	   control is released.
	 */
	m->mcb->active = 0;
}

motion_request_control(mq, m)
register MOTION_QUEUE *mq;
register MOTION_PARAMETER_BLOCK *m;
{
	/* Routine called to determine if the current motion has finished,
	   and if so, what the next motion segment will be.
	 */

	int action;

	/* Flush motion queue if requested */

	if (m->mcb->flushReq)
	 { doFlushMotionQueue(mq, m);
	   m->mcb->flushReq = 0;
	   clearPendingMotion (m);
 	 }

	/* Check flag used by stopCurrentMotion() */

	if (m->mcb->terminateReq && m->state == TGEN_SERVICE)
	 { m->finishCode = m->mcb->terminateCode;
	   m->completedOnSchedule = NO;
	   m->mcb->terminateReq = 0;
	 }

	/* If motion is associated with an MFLAG, check the stop flag
	   set by stopMotion() */

	else if (m->cbk.mflag && m->cbk.mflag->stopRequest >= 0 && 
		 m->state == TGEN_SERVICE)
	 { m->finishCode = m->cbk.mflag->stopRequest;
	   m->completedOnSchedule = NO;
	 }
	
	/* Check if the motion segment time has expired. */

/*****count*/
/*****sigma*/
	else if (m->motionHasTimeLimit && m->ci.c > 1 &&
	     m->ci.c > (m->ci.sigma-m->endCount))
	 { m->completedOnSchedule = YES;
	   m->finishCode = ON_NORMAL;
	 }
	
	if (m->finishCode != UNDEF)
	 { 
	   /* Motion request has completed. In that case ... */

	   m->lastFinishCode = m->finishCode;
	   if (m->systemPause)
	    { m->SAVEstate = UNDEF;
	      action = ENTER_PAUSE_STATE;
	    }
	   else if (nextMotionReady(m))
	    { action = SERVICE_NEXT_REQUEST;
	    }
	   else
	    { action = ENTER_IDLE_STATE;
	    }
	 }

	/* Check for pause state */

	else if (m->systemPause && m->state != TGEN_PAUSE &&
		 m->cycle != 0)
	 { m->completedOnSchedule = NO;
	   m->SAVEstate = m->state;
	   action = ENTER_PAUSE_STATE;
	 }

	/* Check for return from pause state */

	else if (!m->systemPause && m->state == TGEN_PAUSE)
	 { m->completedOnSchedule = YES;
	   if (m->SAVEstate == UNDEF)
	    { if (nextMotionReady(m))
	       { action = SERVICE_NEXT_REQUEST;
	       }
	      else
	       { action = ENTER_IDLE_STATE;
	       }
	    }
	   else if (m->SAVEstate == TGEN_IDLE)
	    { if (nextMotionReady(m))
	       { action = SERVICE_NEXT_REQUEST;
		 m->finishCode = ON_NORMAL;
	       }
	      else if (!m->SAVEtracking)
	       { action = ENTER_IDLE_STATE;
	       }
	      else
	       { action = LEAVE_PAUSE_STATE;
	       }
	    }
	   else
	    { action = LEAVE_PAUSE_STATE;
	    }
	 }

	/* Check for a new motion request if we are in the IDLE state */

	else if (m->state == TGEN_IDLE && !m->systemPause &&
		 nextMotionReady(m))
	 { m->completedOnSchedule = YES;
	   action = SERVICE_NEXT_REQUEST;
	   m->finishCode = ON_NORMAL;
	 }

	/* If we are tracking, check if tracking mode has been cleared */
	
	else if (m->state == TGEN_IDLE && m->tracking &&
		 !(m->mcb->mode & TRACKING_MODE))
	 { m->completedOnSchedule = YES;
	   action = ENTER_IDLE_STATE;
	   m->finishCode = ON_NORMAL;
	 }

	/* or check for a new request if 'stopOnNextRequest' is set */

	else if (m->stopOnNextRequest && !m->systemPause &&
		 nextMotionReady(m))
	 { m->completedOnSchedule = NO;
	   action = SERVICE_NEXT_REQUEST;
	   m->finishCode = ON_NORMAL;
	 }
	else
	 { action = 0;	/* continue with current motion */
	 }

	/* Reset the global terminate flag */
	if (m->mcb->terminateReq)
	 { m->mcb->terminateReq = 0;
	 }

	/* Reset the trajectory generator state */

	switch (action)
	 { case SERVICE_NEXT_REQUEST:
	        m->state = TGEN_SERVICE;
	        break;
	   case ENTER_IDLE_STATE:
		m->state = TGEN_IDLE;
		break;
	   case ENTER_PAUSE_STATE:
	 	m->state = TGEN_PAUSE;
		break;
	   case LEAVE_PAUSE_STATE:
		m->state = m->SAVEstate;
		break;
	 }

	return (action);
}

nextMotionReady(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Check the motion queue to see if we have a motion ready to
	   go. If the motion is conditional upon the current completion
	   status, check that there is a match; if not, pop the queue
	   until we find a match */

	MOTION_REQUEST *mr;

	mr = NULL;
	if (m->nextMotionReady)
	 { mr = m->nextMotionRequest;
	   while (mr && mr->condition != UNDEF && 
			mr->condition != m->lastFinishCode)
	    { freeMotionRequest (m->mq);
	      mr = motionRequestReady(m->mq, m);
	    }
	 }
	return (mr ? 1 : 0);
}

initializeMpb (m, gmem)
register MOTION_PARAMETER_BLOCK *m;
RCCL_GMEM *gmem;
{
	/* Called by setpStartup() at the beginning of every control
	   cycle to set up initial motion parameters */

	int k;

	m->interMode = m->rbt ? JOINT : CARTESIAN;

	m->mode = m->mcb->mode;

	m->blendInT6 = YES;

	m->td = getCurrentTask();
	m->sampleTime = m->td->tcb->interval;

	m->controlMode = SETPOINT_CTRL;

	bzero (&m->cbk, sizeof(COMPLETION_BLOCK));
	bzero (&m->expiringCbk, sizeof(COMPLETION_BLOCK));
	m->expiringCycles = 0;

	m->completedOnSchedule = 0;
	m->relativePosition = NO;

	m->nextGoalPos = NULL;
	m->jbiasFxn.fxn = constTrsf;
}

doCompletion (m, cbk)
MOTION_PARAMETER_BLOCK *m;
register COMPLETION_BLOCK *cbk;
{
	/* called when a motion officially completes. Sends signals, 
	   sets flags, updates transforms, and other things that have 
	   been requested to be done when the motion completes */

	register MFLAG *mf = cbk->mflag;

	if (cbk->updatePos)
	 { TRSF *status;
	   status = solveTrans (cbk->updateTr, cbk->updatePos,
			     	cbk->updateT1, cbk->updateTx);
	   freePosition (cbk->updatePos);
	   if (status == NULL)
	    { errorMsg (EInternal, rcclTgenErrorMsg, "calling solveTrans()");
	      rciAbortErrors();
	    }
	 }
	writeTrans (m->mnps[rciCpu]->here, T6);
	if (mf)
	 { int signum = mf->stopSig;
	   mf->stopSig = 0;
	   mf->stopValue = cbk->code;
	   mf->stopTime = m->sysTime;
	   mf->status = M_FINISHED;
	   if (signum)
	    { rciSignal (signum);
	    }
	 }
}

char *setpStateStr (m)
MOTION_PARAMETER_BLOCK *m;
{
	/* Debugging function: Return a string describing the current
	   state.
	 */

	if (m->state == TGEN_IDLE)
	 { if (m->tracking)
	    { return ("TRACK");
	    }
	   else
	    { return ("IDLE");
	    }
	 }
	else if (m->state == TGEN_SERVICE)
	 { if (m->stopMotion)
	    { return ("STOP");
	    }
	   else
	    { return ("MREQ");
	    }
	 }
	else if (m->state == TGEN_PAUSE)
	 { return ("PAUSE");
	 }
	else
	 { return ("???");
	 }
}

#define FUZZ	1e-4	/* epsilon for floor/ceiling functions */

adjustArrivalTime (mf, m)
MFLAG *mf;
MOTION_PARAMETER_BLOCK *m;
{
	/* called to adjust the motion arrival time in response to
	   a request to either explicitly set the arrival time, or 
	   change the speed on the motion */
	
	MOTION_COUNT_INFO *ci;

	int cmd;
	float value;
	float trans;
	int newSigma;

	cmd = mf->timeCmd;
	value = mf->timeCmdVal;
	trans = mf->timeCmdTrans;
	mf->timeCmd = 0;

	ci = &m->ci;

	if (ci->c >= ci->sigma)
	 { /* no time left; motion is over */
	   mf->timeCmd = 0;
	   return;
	 }

	switch (cmd)
	 { case MF_ADJUST_SPEED:
	    { float ds_t;	/* new desired scale increment */
	      float dds;	/* desired change in scale increment */
	      float t_a;	/* acceleration time */
	      float t_b;	/* new post-acceleration time */

	      ds_t = value/(float)ci->sigma_0;
	      dds = ds_t - ci->ds;
	      if (ABS(dds) < EPSILON)
	       { ci->k_a = 0;
		 return;
	       }
	      if (ci->sigma_0 == UNDEF)
	       { ci->k_a = 0;
		 return;
	       }
	      else if (trans == 0.0)
	       { t_b = (dds>0) ? ceil((ci->sigma_0/value)*(1.0-ci->s)-FUZZ) :
				 floor((ci->sigma_0/value)*(1.0-ci->s)+FUZZ);
		 ci->sigma = t_b + ci->c;
		 ci->k_a = 0;
	       }
	      else
	       { float a;	/* acceleration */
		 float s_a;	/* s value immediately after acceleration */

		 if (trans == F_DEFAULT)
		  { /* compute transition time ourselves. Right now
		       we do this in a bit of a hacky way: we use the
		       drive parameters to give us one estimate, and use
		       the current time required to stop to give us
		       another estimate. We then MAX the two values, plus
		       any scale transitioning currently in progress (the
		       varibale ci->k_a). */

		    float t1, t2;

		    t1 = computeScaleAccel(m, ci->ds, ds_t);
		    t2 = computeStopAccel(m);
		    t_a = ceil(MAX(t1, t2)/m->sampleTime-FUZZ);
		    t_a = MAX(t_a, ci->k_a);
		  }
		 else
		  { t_a = ceil(trans/m->sampleTime-FUZZ);
		  }
		 a = (ds_t-ci->ds)/t_a;
		 s_a = ci->s + (ci->ds + 0.5*a*t_a)*t_a;
		 if (s_a > 1.0)
		  { t_a = (-ci->ds + sqrt(ci->ds*ci->ds - 2*a*(ci->s-1)))/a;
		    t_a = (t_a < ci->sigma - ci->c) ? 
			  ceil(t_a-FUZZ) : 
			  floor(t_a+FUZZ);
		    ci->a = 2*(1 - ci->s - ci->ds*t_a) / (t_a*t_a);
		    ci->k_a = t_a;
		    ci->sigma = ci->c + t_a;
		  }
		 else
		  { t_b = (t_a + t_b < ci->sigma - ci->c) ? 
			  ceil((1-s_a)/ds_t-FUZZ) :
			  floor((1-s_a)/ds_t+FUZZ);
		    ci->a = (1 - ci->s - ci->ds*(t_a+t_b)) / 
			    ((t_b + 0.5*t_a)*t_a);
		    ci->k_a = t_a;
		    ci->sigma = ci->c + t_a + t_b;
		  }
	       }
	      break;
	    }
	   case MF_ADJUST_STOPTIME:
	    { 
#if 0
	      if (mf->startTime + ci->sigma >= m->sysTime)
	       { return;
	       }
	      else
	       { ci->sigma = (MAX(m->sysTime,value) - mf->startTime) /
			     m->sampleTime;
	       }
#endif
	      break;
	    }
	 }
}

initScale (ci)
register MOTION_COUNT_INFO *ci;
{
	/* Called during the 1st cycle of the motion to initialize the
	   path scale parameters.
	 */

	if (ci->sigma != UNDEF)
	 { ci->s = ci->c/(float)ci->sigma;
	 }
	else
	 { ci->s = 0.0;
	 }
	ci->sigma_0 = ci->sigma;
	ci->k_a = 0;
}

advanceScale (ci)  
register MOTION_COUNT_INFO *ci;
{
	/* Called during the second and subsequent cycles of the motion
	   to advance the path scale paramter.
	 */

	float ticksLeft;

	if (ci->sigma != UNDEF)
	 { ticksLeft = ci->sigma - ci->c + 1;
	   if (ticksLeft <= 1)
	    { ci->ds = 1.0 - ci->s;
	      ci->s = 1.0;
	    }
	   else if (ci->k_a)
	    { ci->ds = (1.0 - ci->s + 0.5*ci->a*ci->k_a*ci->k_a)/ticksLeft
		       - ci->a*(ci->k_a - 0.5);
	      ci->s += ci->ds;
	      ci->k_a--;
	    }
	   else
	    { ci->ds = (1.0 - ci->s) / ticksLeft;
	      ci->s += ci->ds;
	    }
	 }
}
