/*-=-=-=-=-=-=-=-=-=-=-=- 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:	pendingMotion
 *
 * DESCRIPTION:	Routines for monitoring the next (pending) motion request
 *		to be serviced by the RCCL trajectory generator. The main
 *		reason we monitor pending motions is to decide when to
 *		start the transition to the motion.
 *
 * AUTHOR/DATE:	John Lloyd, Spring 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <math.h>

#include "../h/fastmath.h"
#include "../h/rcclDefs.h"
#include "../h/errorCodes.h"
#include "../h/assignMem.h"
#include "../h/robot_kynvar.h"

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

#define T6	(&m->t6)
#define J6	(&m->j6)

/* Limit codes from the inverse kinematic routines */
#define KYN_CHECK	(KYN_JOINT_LIMIT | KYN_CANT_REACH)

extern int rciCpu;	/* CPU running this task */

setupPendingMotion (m, mr)
register MOTION_PARAMETER_BLOCK *m;
register MOTION_REQUEST *mr;
{
	/* Called the first time a new motion request is noticed
	   on the queue. It obtains sets up those things necessary
	   in order for the system to monitor the next motion request
	   (which it does mainly to determine what transition time
	    the next request will require).
	*/

	char *fxnName = "setupPendingMotion()";

	char *mem = (char*)mr;
	mem += ALIGN_SIZE(sizeof(MOTION_REQUEST));

	/* If there is a goal position, copy the position equation into
	   a local buffer and reset the pointers so we can read it.
	*/

	if (mr->positionSize)
	 { char *posmem;
	   POS_TERM *pTerm;

	   if ((m->goalPos == (POS*)m->goalPosBuf1) ||
	       (m->SAVEgoalPosition == (POS*)m->goalPosBuf1))
	    { m->nextGoalPos = (POS*)m->goalPosBuf2;
	    }
	   else
	    { m->nextGoalPos = (POS*)m->goalPosBuf1;
	    }

	   ASSIGN_MEM (posmem, char*, mr->positionSize, mem);
	   bcopy (posmem, (void*)m->nextGoalPos, mr->positionSize);
	   delocatePosition (m->nextGoalPos, (int)mr->positionAddr - 
						  (int)m->nextGoalPos);
	    
	   /* go through the position structure and get the local addresses
	      of all the variable transforms. */

	    { POS_TERM *t;
	      POS *p = m->nextGoalPos;
	      TRSF_DESC *tr;

	      for (t = p->t6Term->prev; t!= p->t6Term; t = t->prev)
	       { tr = TRSF_GETDESC(t->trsf);
		 if (tr->fxn != constTrsf)
		  { if ((tr->parent = TRSF_GETDESC(
			     getTransByAddr((TRSF*)tr->source))) == NULL)
		     { rciAbort (EInternal, 
		   "%s -- can't find transform for source 0x%x\n", 
				 fxnName, tr->source);
		       return (-1);
		     }
		  }
	       }
	    }
	 }
	else
	 { m->nextGoalPos = NULL;
	   m->nextJC0 = mr->jointPosition; /* this will be overwritten */
	 }

	/* set up motion control flag, if necessary */

	if (mr->mflagNum != UNDEF)
	 { m->nextMflag = &m->mflagBuf[mr->mflagNum];
	 }
	else
	 { m->nextMflag = NULL;
	 }
	m->nextCount = 0;
	m->nextMotionInRange = NO;	/* for joint mode only */

	/* request acceleration update if the acceleration time is 
	   already specified */

	if (mr->transitionTimeSet)
	 { m->updateAcceleration = YES;
	 }
	m->nextMotionRequest = mr;
	m->nextMotionReady = NO;

	/* Set these to make sure we compute the target jC at least once,
	   if necessary. These should really but done in
	   computeInitialSetpoints(), but I don't want to bother
	   that code ... */
	m->targetInRange = NO;
	m->configUsed = UNDEF;
}

clearPendingMotion (m)
MOTION_PARAMETER_BLOCK *m;
{
	/* Called when a pending motion is either taken for servicing,
	   or cancelled.
	 */

	m->updateAcceleration = YES;
	m->nextMotionRequest = NULL;
	m->nextMotionReady = NO;
}

#define EPSILON	1.0e-7

/* Determine the configuration to be used for the next motion target:
   take the current target configuration, plus any explicit setting */
#define NEXT_CONFIG ((m->configTarget & ~mr->configSet) | mr->config)

static char *rngMsg1 = "monitorPendingMotion(): next target out of range";
static char *rngMsg2 = "monitorPendingMotion(): current target out of range";

monitorPendingMotion (m, mr)
MOTION_PARAMETER_BLOCK *m;
MOTION_REQUEST *mr;
{
	/* Lets see now ... this routine is called every cycle during
	   which a new motion request is pending. Its purpose, as the name
	   inplies, is to monitor the motion request and (1) see if it is
	   cancelled before being serviced, and (2) try to develop an
	   intelligent estimate of what sort of velocity it is going to
	   develop. We need to estimate this velocity for the routine 
	   updateAcceleration(), which will try and decide,
	   on the fly, what sort of transition time will be needed.

	   Estimating the upcoming velocity is a bit tricky, especially
	   when the target is (possibly) in motion. One could simply say that
	   the upcoming velocity has a magnitude equal the desired
	   velocities, but this has the following problem: If the length
	   of the motion path is very small, then the acceleration limits
	   will not allow the robot to ramp up to the desired velocity.
	   Instead, we take the length of the motion path into account
	   and try ans decide on a likely velocity.

	   To compute the length of the motion path requires keeping
	   track of the next target position, in coordinates appropriate 
	   to the interpolation mode chosen for the next motion. 
	   This is a bit of a problem if the target is moving in time, 
	   because we have to recompute it all the time, and we don't have
	   too much CPU time available (at least on the MicroVAX II). So one
	   strategy we have adopted is to course-sample the target position,
	   and only recompute it every few cycles. 

	   What sampling we use depends on the interpolation mode and
	   whether the target type is fixed. We implement the sampling by
	   switching on the low bits of the 'nextCount' field in the 'm'.
	   This routine is sort of under development, and changes as
	   different things are made more efficient, but you have the basic
	   idea ...
	*/
	char *fxnName = "monitorPendingMotion()";

	MFLAG *mflag;

	/* check to see if the motion has been cancelled */

	if (m->nextMflag->stopRequest >= 0)
	 { deleteMotion (m, mr, m->nextMflag);
	   free_queue (m->mq);
	   clearPendingMotion (m);
	   return;
	 }

	/* Estimate the target position and the likely velocity
	   for the motion segment. The like velocity is stored in
	   the 'nextCvel' or 'nextJvel' fields of the 'm'.
	 */

#define COURSE_COUNT	4
#define CYCLE0		0	/* Implement course sampling by dividing */
#define CYCLE1		1	/* some computations up over several (up */
#define CYCLE2		2	/* to 4) control cycles. Get current cycle */
#define CYCLE3		4	/* num from low bits of 'nextCount'. */

	if (mr->interMode == CARTESIAN)
	 { switch (m->nextCount % COURSE_COUNT)
	    { case CYCLE0:
	      case CYCLE2:
	       { TRSF disp;
		 if (mr->targetFixed)
		  { if (m->nextCount == 0)
		     { m->nextTC0 = mr->tC0;
		     }
		  }
		 else
		  { if (!mr->relativePosition)
		     { solveT6U (&m->nextTC0, m->nextGoalPos);
		     }
		    /* else we will use the shift transform instead */
		  }

		 /* Note that we calculate displacement in world
		    coordinates so it is compatitble with current
		    velocity information ... */

		 if (mr->relativePosition)
		  { multTrsf (&disp, &m->tC, &mr->shiftTransform);
		    multRiTrsf (&disp, &disp, &m->tC);
		    computeCvel (&m->nextCvel, &mr->shiftTransform, mr);
		  }
		 else
		  { multRiTrsf (&disp, &m->nextTC0, &m->tC);
		    computeCvel (&m->nextCvel, &disp, mr);
		  }
		 m->updateAcceleration = YES; /* request accel update */
		 if ((m->nextMflag->startRequest < 0) ||
		     (mr->startTimeSet && mr->startTime > m->sysTime))
		  { m->mcb->waitingForStart = YES;
		  }
		 else
		  { m->mcb->waitingForStart = NO;
		    if (m->nextMotionReady == NO)
		     { if (mr->startTimeSet ||
			   (m->sysTime-mr->queueTime > rcclGetMotionDelay()))
			{ m->nextMotionReady = YES;
			}
		     }		    
		  }
		 break;
	       }
	      case CYCLE1:
	      case CYCLE3:
	       { /* take a break */
		 break;
	       }
	    }
	 }
	else if (mr->interMode == JOINT)
	 { int config;
	   int status;
	   RCI_RBT *rbt;

	   rbt = m->rbt;
	   
	   switch (m->nextCount % COURSE_COUNT)
	    { case CYCLE0:
	      case CYCLE2:
	       { if (m->nextGoalPos)
		  { if (mr->goalPosFixed)
		     { if (m->nextCount == 0)
			{ m->nextTC0 = mr->tC0;
			  m->nextJCp0 = mr->jC0;
			}
		       if (((config = NEXT_CONFIG) != mr->configUsed) ||
			     !mr->targetInRange)
			{ status = INV_KINEMATICS (m->nextJCp0.v,
						   &m->nextTC0,
						   config, J6->v, NULL);
			  mr->configUsed = config;
			}
		       else
			{ status = RANGE_ANGLES (m->nextJCp0.v, 
						 m->nextJCp0.v, J6->v);
			}
		     }
		    else
		     { if (!mr->relativePosition)
			{ solveT6U (&m->nextTC0, m->nextGoalPos);
			}
		       else
			{ multTrsf (&m->nextTC0, &m->tC,
				    &mr->shiftTransform);
			}
		       config = NEXT_CONFIG;
		       status = INV_KINEMATICS (m->nextJCp0.v,
						&m->nextTC0,
						config, J6->v, NULL);
		     }
		    if (!(mr->targetInRange = !(status & KYN_CHECK)))
		     { if (m->state == TGEN_IDLE)
			{ solutionAbort (m, rngMsg1, status, &m->nextJCp0);
			}
		       return;
		     }
		  }
		 else
		  { m->nextJCp0 = mr->jointPosition;
		    mr->targetInRange = YES;
		  }
		 if (mr->jbiasEnable)
		  { if (mr->jbiasFxn.fxn != constTrsf)
		     { readMem (m->mnps[rciCpu]->jbias, &mr->jbias);
		     }
		    addJnts(&m->nextJC0, &mr->jbias, &m->nextJCp0);
		  }
		 else
		  { m->nextJC0 = m->nextJCp0;
		  }
		 break;
	       }
	      case CYCLE1:
	      case CYCLE3:
	       { if (mr->targetInRange)
		  { JNTS jC;

		    /* compute our current target joints */

		    if (m->interMode == CARTESIAN)
		     { if (m->goalPosFixed)
			{ if ((m->configTarget != m->configUsed) ||
			      !m->targetInRange)
			   { status = INV_KINEMATICS (m->jC.v, &m->tC,
						      m->configTarget,
						      J6->v, NULL);
			     m->configUsed = m->configTarget;
			   }
			  else
			   { status = RANGE_ANGLES (m->jC.v, m->jC.v, J6->v);
			   }
			}
		       else
			{ status = INV_KINEMATICS (m->jC.v, &m->tC,
						   m->configTarget, J6->v,
						   NULL);
			}
		       if (!(m->targetInRange = !(status & KYN_CHECK)))
			{ if (m->state == TGEN_IDLE)
			   { solutionAbort (m, rngMsg2, status, &m->jC);
			   }
			  return;
			}
		     }
		    else
		     { /* traject.c will maintain m->jC in JOINT mode */
		     }			

		    computeJvel (&m->nextJvel, &m->jC, &m->nextJC0, mr);

		    m->updateAcceleration = YES;
		    if ((m->nextMflag->startRequest < 0) ||
			(mr->startTimeSet && mr->startTime > m->sysTime))
		     { m->mcb->waitingForStart = YES;
		     }
		    else
		     { m->mcb->waitingForStart = NO;		       
		       if (m->nextMotionReady == NO)
			{ if (mr->startTimeSet ||
			      (m->sysTime-mr->queueTime > 
			       rcclGetMotionDelay()))
			   { m->nextMotionReady = YES;
			   }
			}		    
		     }
		  }
		 break;
	       }
	    }
	 }
	m->nextCount++;
}

VECT zerovect = { 0.0, 0.0, 0.0 };

computeCvel (vel, disp, mr)
DIFF *vel;
TRSF *disp;
register MOTION_REQUEST *mr;
{
	/* computes the anticipated velocity implied by the indicated
	   displacement. 

	   The method used is very simple:

	   Assume we have several orthogonal degrees of freedom, each
	   with a desired maximum velocity (v_max) and acceleration (a). 
	   We compute the distance d that each of these DOFs is from the 
	   target position. We also assume that we will accelerate to
	   speed, cruise, and decelerate to the target (basically
	   true, ignoring non-zero initial velocites for the moment).

	   The distance covered during both acceleration phases is
			   2
		d_a = v_max  / a

	   We can precompute this. Now, if (d_a > d), then we don't 
	   have time to ramp up to the cruising velocity. Instead, the 
	   total path segment time t and midpoint velocity v are given by

		t = 2 sqrt( d / a )

		v = t a / 2

	   Otherwise, we do have time to ramp up to crusing speed, and
	   the total path segment t and midpoint velocity v are given by

		t = [ 2 v_max / a   +  (d - d_a) / v_max ]

		v = v_max

	   The (2 v_max / a) term can also be precomputed.
	   
	   Next, since the motion of all the DOFs is syncronized, we
	   examine the time terms for each and take the maximum.
	   We then scale back the velocity for each DOF by

		v = v t / t_max
	 */

	DSPL dist;	/* displacment in DSPL parameters */

	float d_t;	/* translational displacement */
	float d_r;	/* rotational displacment */
	float v_t;	/* attained translational velocity */
	float v_r;	/* attained rotational velocity */
	float timeTrans;/* translational time */
	float timeRot;	/* rotational time */
	float time_max;	/* net time for motion */
	float v_scale;	/* velocity scaling factor */

	trsfToDspl (&dist, disp);

	/* Compute translational time first */

	d_t = SQRT(dist.p.x*dist.p.x + dist.p.y*dist.p.y + dist.p.z*dist.p.z);
	if (mr->transAccelDist > d_t)
	 { timeTrans = SQRT(d_t/mr->transAccel);
	   v_t = timeTrans*mr->transAccel;
	   timeTrans *= 2.0;
	 }
	else
	 { timeTrans =   mr->transAccelTime 
		    + (d_t - mr->transAccelDist)/mr->transVel;
	   v_t = mr->transVel;
	 }

	/* Compute rotational time */

	d_r = FABS(dist.a);
	if (mr->rotAccelDist > d_r)
	 { timeRot = SQRT(d_r/mr->rotAccel);
	   v_r = timeRot*mr->rotAccel;
	   timeRot *= 2.0;	      
	 }
	else
	 { timeRot =   mr->rotAccelTime 
		    + (d_r - mr->rotAccelDist)/mr->rotVel;
	   v_r = mr->rotVel;
	 }

	time_max = MAX (timeRot, timeTrans);

	/* scale velocities ... */

	if (d_t > EPSILON && time_max > EPSILON)
	 { v_scale = v_t * timeTrans / (time_max * d_t);
	   vel->t.x = dist.p.x * v_scale;
	   vel->t.y = dist.p.y * v_scale;
	   vel->t.z = dist.p.z * v_scale;
	 }
	else
	 { vel->t = zerovect;
	 }

	if (time_max > EPSILON)
	 { v_scale = v_r * timeRot / time_max;
	  /* dist.u is already a unit vector */
	   vel->r.x = dist.u.x * v_scale;
	   vel->r.y = dist.u.y * v_scale;
	   vel->r.z = dist.u.z * v_scale;
	 }
	else
	 { vel->r = zerovect;
	 }
}

computeJvel (jvel, j1, j2, mr)
JNTS *jvel;
JNTS *j1, *j2;
MOTION_REQUEST *mr;
{
	/* computes the anticipated velocity implied by the indicated
	   distance between j1 and j2, in joint coordiantes.

	   The method of computation is analagous to that used
	   by computeCvel().
	 */

	float d_j[NUM_JNTS];	/* net joint displacement */
	float v_j[NUM_JNTS];	/* attained joint velocity */
	float time_j[NUM_JNTS];	/* per-joint motion time */
	float time_max;		/* net time for motion */
	register int k;		

	time_max = 0.0;

	for (k=0; k<NUM_JNTS; k++)
	 { d_j[k] = FABS(j2->v[k] - j1->v[k]);

	   if (mr->jointAccelDist.v[k] > d_j[k])
	    { time_j[k] = SQRT(d_j[k] / mr->jointAccel.v[k]);
	      v_j[k] = mr->jointAccel.v[k] * time_j[k];
	      time_j[k] *= 2.0;
	    }
	   else
	    { time_j[k] = mr->jointAccelTime.v[k]
		 + (d_j[k] - mr->jointAccelDist.v[k])/mr->jointVel.v[k];
	      v_j[k] = mr->jointVel.v[k];
	    }
	   if (time_j[k] > time_max)
	    { time_max = time_j[k];
	    }
	 }

	/* and scale the output velocities: */

	if (time_max < EPSILON)
	 { zeroJnts (jvel);
	 }
	else
	 { for (k=0; k<NUM_JNTS; k++)
	    { jvel->v[k] = v_j[k] * time_j[k] / time_max;
	    }
	 }
}
