/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	motionInit
 *
 * DESCRIPTION:	The functions in this file are used to initialize the
 *		different types of motion states. RCCL currently has 4
 *		different motion states:
 *
 *		motion request -- a path segment is computed to handle a 
 *			specific move() or stop() request from the RCCL 
 *			program.
 *
 *		idle state -- nothing else to do; just move to the current
 *			position.
 *
 *		tracking state -- similar to the idle state; there is nothing
 *			else to do at the moment, but instead of just 
 *			stopping at whereever we happen to be, we continue
 *			to track the target specified by the most recent
 *			motion request.
 *
 *		pause state -- the system has been instructed to "freeze",
 *			so we save the motion parameters for the current
 *			motion segment and stop at whereever we happend to
 *			be. Later, we will resume the original path segment
 *			at the point where we left off.
 *
 *      	For convenience, each of these states is treated as 
 *		motion path segment -- in reality, you cannot change from one
 *      	state to the other without going through some sort of
 *      	transition period during which the robot is accelerated to
 *      	its new state. 
 *
 *      	Since computing these transitions is tedious (and accounts
 *      	for much of what the trajectory generator does), a good way to
 *      	handle things is to consider EVERYTHING the robot does to be
 *      	some sort of motion.
 *
 *      	Each state described above has a function that sets the
 *      	necessary field in the motion parameter block. The "pause"
 *      	state has two  functions: one to set the pause state up, and
 *      	another to clear it and resume the pre-empted motion.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/
	
#include "../h/cdefs.h"
#include "../h/robotMath.h"
#include "../h/rciTask.h"
#include "../h/rcclDefs.h"
#include "../h/errorCodes.h"
#include "../h/assignMem.h"

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

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

/* miscelaneous definitions */

#define TARGET_KNOWN 	1	/* target is known from motion request */
#define TARGET_STOP	4	/* target is known from stop request */
#define TARGET_B     	2	/* target is where ever we are heading */

/* Use macros to assign motion and transition counts */
#define TRANS_COUNT(dt,s)	((dt) == 0 ? 0 : MAX ((int)((dt)/(s)), 2))
#define SET_MOTION_COUNT(tc,skew) ((tc) == 0 ? 1 : -(skew)*(tc+tc))

extern int transDebug;		/* for debugging */
extern int rciCpu;		/* the CPU we are executing on */

/* ----- Error Messages. Defined externally to save space ----- */

static char *confChangeMsg	= 
"trajectory generator -- robot config change (0x%x to 0x%x) in Cart. mode\n";
static char *noUpdateObjMsg	= 
"trajectory generator -- can't find object(s) for updateTrans()\n";
static char *cantAllocUpdateMsg	= 
"trajectory generator -- can't allocate position updateTrans()\n";

instantiateMotionRequest(mq, m)
MOTION_QUEUE *mq;
register MOTION_PARAMETER_BLOCK *m;
{
	/* set up the motion parameter block to handle the next
	   motion. To get the required information for the motion, we 
	   basically unpack the motion request packet.
	 */
	
	MOTION_CONTROL_BLOCK *mcb;
	MOTION_REQUEST *mr;
	MANIP *mnp;
	char *mem;		/* pointer to info on motion queue */
	register int new, k;
    
	mcb = m->mcb;

	/* Get the motion request packet off the queue */
	
	mem = (char*)m->nextMotionRequest;
	ASSIGN_MEM (mr, MOTION_REQUEST*, sizeof(MOTION_REQUEST), mem);
	mnp = m->mnps[rciCpu];

	/* set basic path control parameters */

	new = (mcb->pathsegIndex&1)^1;

	m->mode = mr->mode;
	mcb->activeMode = mr->mode;

	mcb->activeTransVel[new] = mr->transVel;
	mcb->activeRotVel[new] = mr->rotVel;
	mcb->activeJointVel[new] = mr->jointVel;
	mcb->activeTransAccel[new] = mr->transAccel;
	mcb->activeRotAccel[new] = mr->rotAccel;
	mcb->activeJointAccel[new] = mr->jointAccel;

	if (mcb->activeComplyMask[new] = mr->complyMask)
	 { mcb->activeComplyVals[new] = mr->complyValues;
	 }
	if (mcb->activeForceLimMask[new] = mr->forceLimitMask)
	 { mcb->activeForceLimVals[new] = mr->forceLimitValues;
	 }
	if (mcb->activeDiffLimMask[new] = mr->diffLimitMask)
	 { mcb->activeDiffLimVals[new] = mr->diffLimitValues;
	 }

	m->motionNumber = mr->number;
	m->stopOnNextRequest = mr->stopOnNextRequest;

	/* used by estimateTargetAndTime() to see how to estimate B */
	m->lastTargetFixed = m->targetFixed;
	m->lastGoalPosFixed = m->goalPosFixed;
	m->lastJbiasFixed = m->jbiasFixed;

	m->targetFixed = mr->targetFixed;
	m->goalPosFixed = mr->goalPosFixed;
	m->jbiasFixed = mr->jbiasFixed;

	m->tracking = NO;	/* not entering tracking idle state */
	m->stopMotion = mr->stopMotion;
	m->doDrive = mr->doDrive;
	m->objectLoad = mr->objectLoad;	/* not used */

	for (k=0; k<mr->numMotionFxns; k++)
	 { m->motionFxns[k] = mr->motionFxns[k];
	 }
	m->numMotionFxns = mr->numMotionFxns;

	if (mr->jbiasEnable)
	 { m->jbiasFxn = mr->jbiasFxn;
	   if (m->jbiasFxn.fxn != constTrsf)
	    { readMem (mnp->jbias, &m->jbias);
	    }
	   else
	    { m->jbias = mr->jbias;
	    }
	   m->jbiasEnable = YES;
	 }
	else
	 { m->jbiasEnable = NO;
	 }

	/* The 'relativePosition' field is only used in case we
	   have to compute the motion segment time later,
	   in estimateTargetAndTime().
	 */
	if (m->relativePosition = mr->relativePosition)
	 { m->targetFixed = YES;	/* target is effectively fixed */
	   m->goalPosFixed = YES;
	   if (mr->shiftSet)		/* we will use shift xform later */
	    { m->shiftTransform = mr->shiftTransform;
	    }
	   else
	    { identTrsf (&m->shiftTransform);
	    }
	 }

	if (m->goalPos = m->nextGoalPos)
	 { POS *p;

	   p = m->goalPos;

	   mem += ALIGN_SIZE(mr->positionSize);

	   /* Motion target is a position equation. */

	   m->interMode = mr->interMode;
	   mcb->activeInterMode[new] = m->interMode;

	   /* See if position equation has COORD or TOOL components */
	   if (p->t6Term->leftSide)
	    { m->coordTerm = (p->t6Term != p->toolTerm->next);
	      m->toolTerm = (p->t6Term != p->toolTerm);
	    }
	   else
	    { m->coordTerm = (p->t6Term != p->toolTerm->prev);
	      m->toolTerm = (p->t6Term != p->toolTerm);
	    }
	   
	    { POS_TERM *t;
	      TRSF_DESC *tr;
	      TRSF_DESC **headp = &m->evalListHead;
	      TRSF_DESC **tailp = &m->evalListTail;

	      /* Take all transforms in that goal position which 
		 have been set 'transMotionEval()' and put them onto
		 the eval list for that motion.
	       */
   
	      if (transDebug)
	       { for (tr = *headp; tr; tr = tr->next)
		  { rciPrintf ("manip unlinking %s (new move)\n", 
			    getMemName(tr->parent));
		  }
	       }
	      m->evalListHead = NULL;		/* clear list */
	      m->evalListTail = NULL; 
   
	      for (t = p->t6Term->prev; t != p->t6Term; t = t->prev)
	       { tr = TRSF_GETDESC(t->trsf);
		 if (tr->fxn != constTrsf)
		  { if (tr->parent->manipOwner == (int)m->hostMan)
		     { TRSF_DESC *tt;
   
		       /* put transform on eval list, if the transform
			  is not already on the eval list */
		       
		       for (tt = *headp; tt; tt = tt->next)
			{ if (tt->parent == tr->parent)
			   { break;
			   }
			}
		       if (!tt)
			{ LINK_HT (*headp, *tailp, tr);
			  if (transDebug)
			   { rciPrintf ("manip linking\n");
			   }
			  readTrans ((TRSF*)tr->parent, (TRSF*)tr);
			}
		     }
		    tr->timestamp = 0;
		  }
	       }
	    }
	 }
	else
	 { /* No goal position ... only a set of joint angles. */

	   m->goalPos = NULL;
	   m->interMode = JOINT;
	   mcb->activeInterMode[new] = JOINT;
	   m->jointPosition = mr->jointPosition;

	   /* Clear the transMotionEval() list for the motion. */
	
  
	   if (transDebug)
	    { TRSF_DESC *tr;
	      for (tr = m->evalListHead; tr; tr = tr->next)
	       { rciPrintf ("manip unlinking %s (new move)\n", 
			 getMemName(tr->parent));
	       }
	    }
	   m->evalListHead = NULL;
	   m->evalListTail = NULL;
	 }

	/* Process a configuration change request. Do not allow
	   configuration change in joint mode.
	 */

	 { int newConfig;

	   newConfig = (m->configTarget & ~mr->configSet) | mr->config;
	   if (newConfig != m->configTarget && 
	       m->interMode != JOINT)
	    { rciAbort (EBadMove, confChangeMsg, m->configTarget,newConfig);
	      return;
	    }
	   m->configTarget = newConfig;
	 }

	/* Set internal control fields ... */

	m->newMotion = YES;
	m->targetType = (m->stopMotion ? TARGET_STOP : TARGET_KNOWN);

	/* set up the completion block -- this contains the "paperwork"
	   that we have to fill out when the motion completes.
	 */

	if (mr->mflagNum != UNDEF)
	 { m->cbk.mflag = &m->mflagBuf[mr->mflagNum];
	   setManipMid (m, mr->mflagNum);
	 }
	else
	 { m->cbk.mflag = NULL;
	   setManipMid (m, UNDEF);
	 }

/* KILROY */
#if 0

	/* updateT6() or updateTrans() requests are attached to the
	   completion block */

	if (mr->updateT6)
	 { if ((m->cbk.t6 = getTransByAddr(mr->updateT6)) == NULL)
	    { rciAbort (EInternal, "can't find update T6 0x%x\n", 
			mr->updateT6);
	      return;
	    }
	 }
	else
	 { m->cbk.t6 = NULL;
	 }
	if (mr->updateSize)
	 { 
	   /* A list of transforms has been supplied for an updateTrans()
	      request. Make a copy of this list, getting the valid pointers
	      to each of the transforms in question.
	    */
	     
	   int k;
	   POS_TRSF *list;
	   POS_TRSF *localList;

	   if (m->expiringCbk.transList && 
	       (m->expiringCbk.transList == m->updatePosList1) )
	    { localList = m->updatePosList2;
	    }
	   else
	    { localList = m->updatePosList1;
	    }

	   ASSIGN_MEM (list, POS_TRSF*, sizeof(POS_TRSF)*mr->updateSize, mem);

	   for (k=0; k<mr->updateSize; k++)
	    { localList[k].leftSide = list[k].leftSide;
	      if ((localList[k].trsf = getTransByAddr(list[k].trsf)) == NULL)
	       { rciAbort (EInternal, "can't find update trans 0x%x\n", 
			   list[k].trsf);
		 return;
	       }
	    }
	   m->cbk.transListSize = mr->updateSize;
	   m->cbk.transList = localList;
	 }
	else
	 { m->cbk.transListSize = 0;
	 }
#endif
/* WAS HERE */
	if (mr->updatePos)
	 { 
	   POS *pos;

	   /* A position equation has been specified to be solved for when
	      the motion ends. Set up the variables necessary to do
	      this solution when the time comes.
	    */

	   if (mr->updateT1 == T6)
	    { if ((m->cbk.updateTr = getTransByAddr(mr->updateTr)) == NULL||
		  (pos = getPositionByAddr(mr->updatePos)) == NULL)
	       { rciAbort (EInternal, noUpdateObjMsg);
		 return;
	       }
	      m->cbk.updateT1 = T6;
	      m->cbk.updateTx = NULL;
	    }
	   else
	    { if ((m->cbk.updateTr = getTransByAddr(mr->updateTr)) == NULL||
	    	  (m->cbk.updateT1 = getTransByAddr(mr->updateT1)) == NULL||
	       	  (m->cbk.updateTx = getTransByAddr(mr->updateTx)) == NULL||
		  (pos = getPositionByAddr(mr->updatePos)) == NULL)
	       { rciAbort (EInternal, noUpdateObjMsg);
		 return;
	       }
	    }
	   if ((m->cbk.updatePos = copyPosition (pos)) == NULL)
	    { rciAbort (EInternal, cantAllocUpdateMsg);
	      return;
	    }
	 }
	else
	 { m->cbk.updatePos = NULL;
	 }

	/* Determine transition times */

	m->tauSkew = mr->tauSkew;
	if (mr->transitionTimeSet)
	 { m->motionTau = TRANS_COUNT (mr->transitionTime,
					 m->sampleTime);
	   m->currentTau = m->motionTau; 
	   /* used to be m->currentTau = doDrive ? m->motionTau : 0; */
	   m->updateAcceleration = YES;
	 }
	else
	 { m->motionTau = UNDEF;
	   if (!m->completedOnSchedule)
	    { float time;
	      time = computeTargetAccel(m) / 2.0;
	      m->currentTau = MAX(2, time/m->sampleTime);
	    }
	 }

	m->ci.c = SET_MOTION_COUNT(m->currentTau,m->tauSkew);

	/* Determine motion time ... if not specified already, it
	    will be computed later by estimateTargetAndTime().
	 */

	if (mr->segmentTimeSet)
	 { if (mr->segmentTime == -1)
	    { m->motionHasTimeLimit = NO;
	      m->ci.sigma = UNDEF;
	    }
	   else
	    { m->motionHasTimeLimit = YES;
	      m->ci.sigma = mr->segmentTime / m->sampleTime;
	      if (m->ci.sigma < (2*m->currentTau))
	       { m->ci.sigma = MAX(2*m->currentTau,2);
	       }
	      else if (m->ci.sigma == 0)
	       { m->ci.sigma = 1;
	       }
	    }
	 }
	else if (mr->stopTimeSet)
	 { m->motionHasTimeLimit = YES;
	   m->ci.sigma = (mr->stopTime - m->sysTime) / m->sampleTime;
	   if (m->ci.sigma < (2*m->currentTau))
	    { m->ci.sigma = MAX(2*m->currentTau,2);
	    }
	   else if (m->ci.sigma <= 0)
	    { m->ci.sigma = 1;
	    }
	 }
	else
	 { m->motionHasTimeLimit = UNDEF;
	 }
	/* set lastScaleTau to 'currentTau', just in case we get a
	   change of scale request right at the beginning of the motion */

	m->lastScaleTrans = m->currentTau*m->sampleTime/1000.0;

	/* set the initial tC0 or jC0 values. These will only be used
	   by estimateTargetAndTime() if it has to compute the motion
	   segment time. Otherwise, these fields are updated again
	   anyway by computeInitialSetpoints() during the same cycle.
	 */

	m->tC0 = m->nextTC0;
	if (m->interMode == JOINT)
	 { m->jC0 = m->nextJC0;
	 }

	freeMotionRequest(mq);
}

instantiateIdleState(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Called to set up an "idle state" motion. All this really
	   does is set the robot to move to a stop (of indefinite
	   duration) at the point where it currently is at (allowing
	   for the transition time).
	*/

	MOTION_CONTROL_BLOCK *mcb;
	MANIP *mnp;
	register int new, old;

	mcb = m->mcb;
	mnp = m->mnps[rciCpu];

	old = mcb->pathsegIndex&1;
	new = old^1;

	mcb->activeInterMode[new]   = mcb->activeInterMode[old];
	mcb->activeTransVel[new]    = mcb->activeTransVel[old];
	mcb->activeRotVel[new]	    = mcb->activeRotVel[old];
	mcb->activeJointVel[new]    = mcb->activeJointVel[old];
	mcb->activeTransAccel[new]  = mcb->activeTransAccel[old];
	mcb->activeRotAccel[new]    = mcb->activeRotAccel[old];
	mcb->activeJointAccel[new]  = mcb->activeJointAccel[old];

	mcb->activeComplyMask[new] = 0;
	mcb->activeForceLimMask[new] = 0;
	mcb->activeDiffLimMask[new] = 0;

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

	m->stopOnNextRequest = YES;
	m->targetFixed = YES;
	m->goalPosFixed = YES;
	m->jbiasFixed = YES;
	m->tracking = NO;	/* not entering tracking idle state */
	m->stopMotion = NO; /* this is not an explicit stop request */
	m->doDrive = NO;
	m->numMotionFxns = 0;
	m->jbiasEnable = NO;
	m->relativePosition = NO;	/* won't be used anyway */

	m->goalPos = NULL;
	m->coordTerm = NO;
	m->toolTerm = NO;

	/* Clear the transMotionEval() evaluation list */

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

	/* Set internal control fields */

	m->newMotion = YES;
	m->targetType = TARGET_B;
	setManipMid (m, UNDEF);
	m->cbk.mflag = NULL;
	m->cbk.t6 = NULL;
	m->cbk.transListSize = 0;
	m->cbk.transList = NULL;
	m->cbk.updatePos = NULL;

	/* Determine transition times */

	m->tauSkew = 0.5;
	m->motionTau = UNDEF;
	if (!m->completedOnSchedule)
	 { float time;
	   time = computeStopAccel(m)/2.0;
	   m->currentTau = MAX (time/m->sampleTime, 2);
	 }

	m->ci.c = SET_MOTION_COUNT(m->currentTau, m->tauSkew);
	m->motionHasTimeLimit = NO;
	m->ci.sigma = UNDEF;
}

instantiateTrackingIdleState(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* Similar to instantiateIdleState(), except that we set things
	   up so that we will continue to track the last specified target
	   until some other motion request happens to come along.

	   This is like have an explicit stop(mnp, F_UNDEF) request serviced
	   when the motions run out.
	 */

	MOTION_CONTROL_BLOCK *mcb;
	MANIP *mnp;
	register int new, old;
	
	mcb = m->mcb;
	mnp = m->mnps[rciCpu];

	old = mcb->pathsegIndex&1;
	new = old^1;

	/* --- set the basic path parameters --- */

	mcb->activeInterMode[new]   = mcb->activeInterMode[old];
	mcb->activeTransVel[new]    = mcb->activeTransVel[old];
	mcb->activeRotVel[new]	    = mcb->activeRotVel[old];
	mcb->activeJointVel[new]    = mcb->activeJointVel[old];
	mcb->activeTransAccel[new]  = mcb->activeTransAccel[old];
	mcb->activeRotAccel[new]    = mcb->activeRotAccel[old];
	mcb->activeJointAccel[new]  = mcb->activeJointAccel[old];

	if (mcb->activeComplyMask[new] = mcb->activeComplyMask[old])
	 { mcb->activeComplyVals[new] = mcb->activeComplyVals[old];
	 }
	if (mcb->activeForceLimMask[new] = mcb->activeForceLimMask[old])
	 { mcb->activeForceLimVals[new] = mcb->activeForceLimVals[old];
	 }
	if (mcb->activeDiffLimMask[new] = mcb->activeDiffLimMask[old])
	 { mcb->activeDiffLimVals[new] = mcb->activeDiffLimVals[old];
	 }

	/* --- set the control variables --- */

	m->stopOnNextRequest = YES;
	m->tracking = YES;	/* this IS entering tracking idle state */
	m->stopMotion = NO; /* this is not an explicit stop request */
	m->doDrive = NO;
	m->relativePosition = NO; 	/* won't be used anyway */

	m->newMotion = YES;
	m->targetType = TARGET_STOP;
	setManipMid (m, UNDEF);
	m->cbk.mflag = NULL;
	m->cbk.t6 = NULL;
	m->cbk.transListSize = 0;
	m->cbk.transList = NULL;
	m->cbk.updatePos = NULL;

	/* Compute transition times */

	m->tauSkew = 0.5;
	m->motionTau = UNDEF;
	if (!m->completedOnSchedule)
	 { float time;
	   time = computeStopAccel(m)/2.0;
	   m->currentTau = MAX (time/m->sampleTime, 2);
	 }
	m->ci.c = SET_MOTION_COUNT (m->currentTau, m->tauSkew);
	m->motionHasTimeLimit = NO;
	m->ci.sigma = UNDEF;
}

instantiatePauseState(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* This is called to setup a pause state, which is basically like
	   putting the robot into the idle state (move to whereever we 
	   happen to be, allowing for transitions), EXCEPT that we
	   will have to resume the interrupted motion later, and so
	   we have to save some things.
	 */
	   
	MOTION_CONTROL_BLOCK *mcb;
	MANIP *mnp;
	register new, old;

	mcb = m->mcb;
	mnp = m->mnps[rciCpu];

	old = mcb->pathsegIndex&1;
	new = old^1;

	if (m->cbk.mflag)
	 { m->cbk.mflag->status = M_PAUSED;
	 }

	/* save the parameters we will need when we restart the motion */

	m->SAVEstopOnNextRequest = m->stopOnNextRequest;
	m->SAVEtargetFixed = m->targetFixed;
	m->SAVEgoalPosFixed = m->goalPosFixed;
	m->SAVEjbiasFixed = m->jbiasFixed;
	m->SAVEtracking = m->tracking;
	m->SAVEstopMotion = m->stopMotion;
	m->SAVEdoDrive = m->doDrive;
	m->SAVEnumMotionFxns = m->numMotionFxns;
	m->SAVEjbiasEnable = m->jbiasEnable;

	m->SAVEgoalPosition = m->goalPos;
	m->SAVEevalListHead = m->evalListHead;
	m->SAVEevalListTail = m->evalListTail;

	m->SAVEcbk = m->cbk;

	m->SAVEmotionTau = m->motionTau;
	m->SAVEmotionHasTimeLimit = m->motionHasTimeLimit;

	m->SAVEcomplyMask = mcb->activeComplyMask[old];
	m->SAVEforceLimitMask = mcb->activeForceLimMask[old];
	m->SAVEdiffLimitMask = mcb->activeDiffLimMask[old];

	m->SAVEci = m->ci;
	m->SAVEtau = m->tau;
	m->SAVEtauSkew = m->tauSkew;
	m->SAVEDRIVE = m->DRIVE;	
	m->SAVEjdrive = m->jdrive;
	m->SAVEdrivePar = m->drivePar;
	m->SAVEdriveJnts = m->driveJnts;

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

	/* Now act sort of like we are entering an idle state, with
	   some modifications */

	mcb->activeInterMode[new]   = mcb->activeInterMode[old];
	mcb->activeTransVel[new]    = mcb->activeTransVel[old];
	mcb->activeRotVel[new]	    = mcb->activeRotVel[old];
	mcb->activeJointVel[new]    = mcb->activeJointVel[old];
	mcb->activeTransAccel[new]  = mcb->activeTransAccel[old];
	mcb->activeRotAccel[new]    = mcb->activeRotAccel[old];
	mcb->activeJointAccel[new]  = mcb->activeJointAccel[old];

	mcb->activeComplyMask[new] = 0;
	mcb->activeForceLimMask[new] = 0;
	mcb->activeDiffLimMask[new] = 0;

	m->stopOnNextRequest = NO;
	m->targetFixed = YES;
	m->goalPosFixed = YES;
	m->jbiasFixed = YES;
	m->tracking = NO;	/* not entering tracking idle state */
	m->stopMotion = NO; /* this is not an explicit stop request */
	m->doDrive = NO;
	m->numMotionFxns = 0;
	m->jbiasEnable = NO;
	m->relativePosition = NO;

	m->goalPos = NULL;
	m->coordTerm = NO;
	m->toolTerm = NO;

	/* Clear the transMotionEval() evaluation list */

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

	/* Set internal control fields */

	m->newMotion = YES;
	m->targetType = TARGET_B;
	m->cbk.mflag = NULL;
	m->cbk.t6 = NULL;
	m->cbk.transListSize = 0;
	m->cbk.transList = NULL;
	m->cbk.updatePos = NULL;

	m->tauSkew = 0.5;
	/* Determine transition times a little differently than 
	   with when instantiating an idle state ... make sure any specified
	   pause accel time is not greater than the default motion trans
	   time, to prevent overshoot */

	 { float time;
	   int defaultCount;

	   time = computeStopAccel(m)/2.0;
	   defaultCount = MAX (time/m->sampleTime, 2);
	   if (m->mcb->pauseTransTime != UNDEF)
	    { int count = TRANS_COUNT (m->mcb->pauseTransTime,
				       m->sampleTime);
	      m->motionTau = MIN(count, defaultCount);
	    }
	   else
	    { m->motionTau = defaultCount;
	    }
	   m->currentTau = m->motionTau;
	 }

	m->ci.c = SET_MOTION_COUNT (m->currentTau, m->tauSkew);

	/* The motion segment count is either undefined (infinite)
	   or has a specified time limit.
	 */
	if (m->mcb->pauseTime == UNDEF)
	 { m->motionHasTimeLimit = NO;
	   m->ci.sigma = UNDEF;
	 }
	else
	 { m->motionHasTimeLimit = YES;
	   m->ci.sigma = m->mcb->pauseTime / m->sampleTime;
	   if (m->ci.sigma < (2*m->currentTau))
	    { m->ci.sigma = MAX(2*m->currentTau,2);
	    }
	 }
}

clearPauseState(m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* reset the motion state that was previously paused */

	MOTION_CONTROL_BLOCK *mcb;
	MANIP *mnp;
	int unfinishedTrans;
	register int old;

	mcb = m->mcb;
	mnp = m->mnps[rciCpu];

	old = mcb->pathsegIndex&1;

	/* we don't need to restore the velocity and acceleration data
	   because that is garanteed to be the same in both buffers */

	mcb->activeComplyMask[old] = m->SAVEcomplyMask;
	mcb->activeForceLimMask[old] = m->SAVEforceLimitMask;
	mcb->activeDiffLimMask[old] = m->SAVEdiffLimitMask;

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

	m->stopOnNextRequest = m->SAVEstopOnNextRequest;
	m->targetFixed = m->SAVEtargetFixed;
	m->goalPosFixed = m->SAVEgoalPosFixed;
	m->jbiasFixed = m->SAVEjbiasFixed;
	m->tracking = m->SAVEtracking;
	m->stopMotion = m->SAVEstopMotion;
	m->doDrive = m->SAVEdoDrive;

	m->numMotionFxns = m->SAVEnumMotionFxns;
	m->jbiasEnable = m->SAVEjbiasEnable;
	if (m->goalPos = m->SAVEgoalPosition)
	 { POS *p = m->goalPos;
	   
	   if (p->t6Term->leftSide)
	    { m->coordTerm = (p->t6Term != p->toolTerm->next);
	      m->toolTerm = (p->t6Term != p->toolTerm);
	    }
	   else
	    { m->coordTerm = (p->t6Term != p->toolTerm->prev);
	      m->toolTerm = (p->t6Term != p->toolTerm);
	    }
	 }
	
	if (transDebug)
	 { TRSF_DESC *tr;
	   for (tr = m->evalListHead; tr; tr = tr->next)
	    { rciPrintf ("manip linking %s (pause clear)\n", 
		      getMemName(tr->parent));
	    }
	 }
	m->evalListHead = m->SAVEevalListHead;
	m->evalListTail = m->SAVEevalListTail;

	m->newMotion = NO;
	m->targetType = TARGET_KNOWN;
	m->cbk = m->SAVEcbk;

	m->ci = m->SAVEci;

	/* if the paused motion had not yet finished transitioning, make
	   sure the start-up transition time is as least as long */
/***count*/
	if ((unfinishedTrans = (2.0*m->SAVEtau)*(1.0-m->SAVEtauSkew)
			     - m->ci.c) > 0)
	 { m->currentTau = m->motionTau + unfinishedTrans/2;
	 }
	else
	 { m->currentTau = m->motionTau;
	 }
	m->tauSkew = m->SAVEtauSkew;
	m->motionTau = m->SAVEmotionTau;
	m->motionHasTimeLimit = m->SAVEmotionHasTimeLimit;

	m->DRIVE = m->SAVEDRIVE;	
	m->jdrive = m->SAVEjdrive;
	m->drivePar = m->SAVEdrivePar;
	m->driveJnts = m->SAVEdriveJnts;

	if (m->cbk.mflag)
	 { m->cbk.mflag->status = M_RUNNING;
	 }
	m->SAVEgoalPosition = NULL; /* so we know buffer is free */
}
