/*-=-=-=-=-=-=-=-=-=-=-=- 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:	computeAccel
 *
 * DESCRIPTION:	Contains code which computes the acceleration times for
 *		the RCCL motions.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1988.
 *
 *-------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------

	RCCL tries to estimate the transition time that will be
	required to blend into the next motion. This time is
	selected so as to meet the specified acceleration limits.

	The basic algorithm is very simple:

	1. Estimate the peek velocity that will be reached by the
	   new motion segment (this is computed by routines in  
	   the module 'pendingMotion').

	2. Take the difference between this velocity and the current
	   velocity, divide by the acceleration limits, and out
	   comes the transition time.

	If no motion is pending, then until one appears, we can assume
	that the next motion will be a stop ... for which we can compute
	the transition time simply be dividing the current velocity by
	the acceleration limits.

	Finally, a motion may have a specific transition time assigned
	to it ... this is done using the primitive setTime().

	NOTE:

	The variable "motionEndCount" denotes the number of cycles
	ahead that we have to pre-empt the current motion so that the
	transition time will be symmetric about the path segment termination
	point. "motionEndCount" should always be equal to the
	"2*tau*tauSkew" plus 1, or 0. We keep it as a separate
	variable for computational efficiency reasons.
     	
=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#include <math.h>

#include "../h/fastmath.h"

#define EPSILON 1e-6

#include "../h/rcclDefs.h"

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

/* Makes sure transition time is either 0 or >= 2 */
#define TRANS_COUNT(dt,s)  ((dt) == 0 ? 0 : MAX (NINT((dt)/(s)), 2))

#define STIME m->sampleTime
#define UPDATE_INTERVAL	2	/* dynamic update freq. w/ nothing pending */

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

updateAcceleration (m)
register MOTION_PARAMETER_BLOCK *m;
{
	/* This function is called once every control cycle by 'setpoint',
	   to check and recompute the current transition time, if necessary.

	   Basically, the computation it performs is:

	   if (motion is pending)
	    { t = (pendingVelocity - currentVelocity)/acceleration;
	    }
	   else
	    { t = currentVelocity/acceleration;
	    }
	*/

	MOTION_REQUEST *mr;
	MOTION_CONTROL_BLOCK *mcb = m->mcb;
	int idx;

	idx = mcb->pathsegIndex&1;

	mr = m->nextMotionRequest;
	if (mr && mr->transitionTimeSet)
	 { /* there is a pending motion and the motion trans time 
	      has been pre-specified */
		
	   int count;

	   if (!m->updateAcceleration)
	    { return;
	    }
	   m->updateAcceleration = NO;

	   if ((count = TRANS_COUNT(mr->transitionTime, m->sampleTime)) &&
		mr->doDrive)
	    { m->endCount = (count+count)*mr->tauSkew+1;
	    }
	   else
	    { m->endCount = 0;
	    }
	 }
	else
	 {
	   /* Estimate transition time from current velocities. */

	   int interMode;
	   DIFF diffCvel;
	   JNTS diffJvel;
	 
	   if (mr) /* and no transition time is set */
	    {
	      /* If a motion is pending, then we only update accelerations
		 when the pending motion monitor tells us to ... */

	      if (!m->updateAcceleration)
	       { return;
	       }
	      m->updateAcceleration = NO;

	      if ((interMode = mr->interMode) == CARTESIAN)
	       { register float rate = 1000.0 / m->sampleTime;

		 trsfToVel (&diffCvel, &m->oldT6, T6);

		 diffCvel.t.x = m->nextCvel.t.x - rate * diffCvel.t.x;
		 diffCvel.t.y = m->nextCvel.t.y - rate * diffCvel.t.y;
		 diffCvel.t.z = m->nextCvel.t.z - rate * diffCvel.t.z;

		 diffCvel.r.x = m->nextCvel.r.x - rate * diffCvel.r.x;
		 diffCvel.r.y = m->nextCvel.r.y - rate * diffCvel.r.y;
		 diffCvel.r.z = m->nextCvel.r.z - rate * diffCvel.r.z;
	       }
	      else /* if (mr->interMode == JOINT) */
	       { HOW *how;

		 how = m->rbt->how;
		 diffJvel.v[0] = mr->jointVel.v[0] - how->angVel[0];
		 diffJvel.v[1] = mr->jointVel.v[1] - how->angVel[1];
		 diffJvel.v[2] = mr->jointVel.v[2] - how->angVel[2];

		 diffJvel.v[3] = mr->jointVel.v[3] - how->angVel[3];
		 diffJvel.v[4] = mr->jointVel.v[4] - how->angVel[4];
		 diffJvel.v[5] = mr->jointVel.v[5] - how->angVel[5];
	       }
	    }
	   else	/* there is no pending motion request */
	    { /* compute the transition time necessary to stop, at 
		 the rate specified by UPDATE_INTERVAL */
	      
	      if (((m->ci.c-1) % UPDATE_INTERVAL) != 0)
	       { return;
	       }

	      if ((interMode = m->interMode) == CARTESIAN)
	       { register float rate = 1000.0 / m->sampleTime;

		 trsfToVel (&diffCvel, &m->oldT6, T6);

		 diffCvel.t.x *= rate;
		 diffCvel.t.y *= rate;
		 diffCvel.t.z *= rate;

		 diffCvel.r.x *= rate;
		 diffCvel.r.y *= rate;
		 diffCvel.r.z *= rate;
	       }
	      else /* if (m->interMode == JOINT) */
	       { HOW *how;

		 how = m->rbt->how;
		 diffJvel.v[0] = how->angVel[0];
		 diffJvel.v[1] = how->angVel[1];
		 diffJvel.v[2] = how->angVel[2];

		 diffJvel.v[3] = how->angVel[3];
		 diffJvel.v[4] = how->angVel[4];
		 diffJvel.v[5] = how->angVel[5];
	       }
	    }
	   if (interMode == CARTESIAN)
	    { 
	      float dTransVel;
	      float dRotVel;
	      float transTime;

	      dTransVel = SQRT(diffCvel.t.x * diffCvel.t.x +
			       diffCvel.t.y * diffCvel.t.y +
			       diffCvel.t.z * diffCvel.t.z);

	      dRotVel   = SQRT(diffCvel.r.x * diffCvel.r.x +
			       diffCvel.r.y * diffCvel.r.y +
			       diffCvel.r.z * diffCvel.r.z);

	      if (mr)
	       { transTime = 500.0*MAX(dTransVel/mr->transAccel,
				       dRotVel/mr->rotAccel);
	       }
	      else
	       { transTime = 500.0*MAX(dTransVel/mcb->activeTransAccel[idx],
				       dRotVel/mcb->activeRotAccel[idx]);
	       }
	      if (m->currentTau = TRANS_COUNT(transTime, m->sampleTime))
	       { if (mr)
		  { m->endCount = 2.0*m->currentTau*mr->tauSkew + 1;
		  }
		 else
		  { /* tauSkew = 0.5 by default */
		    m->endCount = m->currentTau + 1;
		  }
	       }
	      else
	       { m->endCount = 0;
	       }
	    }
	   else /* if (interMode == JOINT) */
	    { 
	      float dtime;
	      float transTime;
	      float *jAccel;
	     
	      jAccel = (mr) ? mr->jointAccel.v : mcb->activeJointAccel[idx].v;

	      transTime = 0;

	      dtime = diffJvel.v[0] / jAccel[0];
	      if (FABS(dtime) > transTime)
	       { transTime = FABS(dtime);
	       }
	      dtime = diffJvel.v[1] / jAccel[1];
	      if (FABS(dtime) > transTime)
	       { transTime = FABS(dtime);
	       }
	      dtime = diffJvel.v[2] / jAccel[2];
	      if (FABS(dtime) > transTime)
	       { transTime = FABS(dtime);
	       }

	      dtime = diffJvel.v[3] / jAccel[3];
	      if (FABS(dtime) > transTime)
	       { transTime = FABS(dtime);
	       }
	      dtime = diffJvel.v[4] / jAccel[4];
	      if (FABS(dtime) > transTime)
	       { transTime = FABS(dtime);
	       }
	      dtime = diffJvel.v[5] / jAccel[5];
	      if (FABS(dtime) > transTime)
	       { transTime = FABS(dtime);
	       }

	      transTime *= 500.0;

	      if (m->currentTau = TRANS_COUNT(transTime, m->sampleTime))
	       { if (mr)
		  { m->endCount = 2.0*m->currentTau*mr->tauSkew + 1;
		  }
		 else
		  { /* tauSkew = 0.5 by default */
		    m->endCount = m->currentTau + 1;
		  }
	       }
	      else
	       { m->endCount = 0;
	       }
	    }
	   m->updateAcceleration = NO;	/* just to be safe */
	 }
}

computeStopAccel(m)
MOTION_PARAMETER_BLOCK *m;
{
	MOTION_CONTROL_BLOCK *mcb = m->mcb;
	float transTime;
	int tau;
	int new;

	/* Return, in milliseconds, the time required to bring the
	   manipulator to a halt.

	   Used by the motion instantiation routines to determine the 
	   time required to bring the manipulator to a halt. We do this 
	   when the we are stopping and the current motion was interrupted: 
	   in such cases the default computation of the transition time
	   might not be valid, since it uses the current target as a basis
	   for computation.	   

	   This routine is also called when we change the speed or
	   arival time for a motion, to determine how long we need to
	   effect the change.

	   As with the above routine, the basic idea is to compute 

			t = currentVelocity / acceleration
	 */

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

	if (m->interMode == CARTESIAN)
	 { 
	   DIFF  diffCvel;
	   float dTransVel;
	   float dRotVel;
	   float rate = 1000.0 / m->sampleTime;

	   trsfToVel (&diffCvel, &m->oldT6, T6);

	   dTransVel = rate * SQRT(diffCvel.t.x * diffCvel.t.x +
			 	   diffCvel.t.y * diffCvel.t.y +
				   diffCvel.t.z * diffCvel.t.z);

	   dRotVel   = rate * SQRT(diffCvel.r.x * diffCvel.r.x +
			 	   diffCvel.r.y * diffCvel.r.y +
				   diffCvel.r.z * diffCvel.r.z);

	   transTime = MAX(dTransVel/mcb->activeTransAccel[new],
			   dRotVel/mcb->activeRotAccel[new]);
	 }
	else /* if (interMode == JOINT) */
	 { 
	   float *jvel = m->rbt->how->angVel;
	   float *jacc = mcb->activeJointAccel[new].v;
	   float dtime;

	   transTime = 0;

	   dtime = jvel[0] / jacc[0];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }
	   dtime = jvel[1] / jacc[1];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }
	   dtime = jvel[2] / jacc[2];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }

	   dtime = jvel[3] / jacc[3];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }
	   dtime = jvel[4] / jacc[4];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }
	   dtime = jvel[5] / jacc[5];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }
	 }
	return (transTime*1000.0);
}

computeTargetAccel(m)
MOTION_PARAMETER_BLOCK *m;
{
	/* called by the instantiateMotionRequest routine to determine 
	   the time required to accelerate the manipulator to the target
	   position from the current position T6. We use this when
	   the current motion was interrupted, and therefore our
	   background computation of the transition time, which is
	   based on the current target position, might not be valid.

	   This routine assumes that the m->nextMotionRequest field is
	   valid (which it must be if called only from 
	   instantiateMotionRequest()).

	   As with the above routine, the basic idea is just to compute 

		t = (pendingVelocity - currentVelocity) / acceleration
	 */

	MOTION_CONTROL_BLOCK *mcb = m->mcb;
	float transTime;
	int tau;
	int new;

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

	if (m->interMode == CARTESIAN)
	 { DIFF diffCvel;
	   DIFF targetCvel;
	   float dTransVel;
	   float dRotVel;
	   float rate = 1000.0 / m->sampleTime;

	   if (m->relativePosition)
	    { TRSF diff;
	      multTrsf (&diff, T6, &m->nextMotionRequest->shiftTransform);
	      multRiTrsf (&diff, &diff, T6);
	      computeCvel (&targetCvel, &diff, m->nextMotionRequest);
	    }
	   else
	    { TRSF diff;
	      multRiTrsf (&diff, &m->nextTC0, T6);
	      computeCvel (&targetCvel, &diff, m->nextMotionRequest);
	    }

	   trsfToVel (&diffCvel, &m->oldT6, T6);

	   diffCvel.t.x = m->nextCvel.t.x - rate * diffCvel.t.x;
	   diffCvel.t.y = m->nextCvel.t.y - rate * diffCvel.t.y;
	   diffCvel.t.z = m->nextCvel.t.z - rate * diffCvel.t.z;

	   diffCvel.r.x = m->nextCvel.r.x - rate * diffCvel.r.x;
	   diffCvel.r.y = m->nextCvel.r.y - rate * diffCvel.r.y;
	   diffCvel.r.z = m->nextCvel.r.z - rate * diffCvel.r.z;

	   dTransVel = SQRT(diffCvel.t.x * diffCvel.t.x +
			    diffCvel.t.y * diffCvel.t.y +
			    diffCvel.t.z * diffCvel.t.z);

	   dRotVel   = SQRT(diffCvel.r.x * diffCvel.r.x +
			    diffCvel.r.y * diffCvel.r.y +
			    diffCvel.r.z * diffCvel.r.z);

	   transTime = MAX(dTransVel/mcb->activeTransAccel[new],
			   dRotVel/mcb->activeRotAccel[new]);
	 }
	else /* if (interMode == JOINT) */
	 { HOW *how;
	   JNTS targetJvel;
	   float *jacc;
	   float dtime;

	   jacc = mcb->activeJointAccel[new].v;

	   computeJvel (&targetJvel,J6,&m->nextJC0,m->nextMotionRequest);

	   how = m->rbt->how;
	   transTime = 0.0;

	   dtime = (targetJvel.v[0] - how->angVel[0]) / jacc[0];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = (targetJvel.v[1] - how->angVel[1]) / jacc[1];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = (targetJvel.v[2] - how->angVel[2]) / jacc[2];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    

	   dtime = (targetJvel.v[3] - how->angVel[3]) / jacc[3];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = (targetJvel.v[4] - how->angVel[4]) / jacc[4];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = (targetJvel.v[5] - how->angVel[5]) / jacc[5];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	 }
	return (transTime*1000.0);
}

computeScaleAccel(m, ds, newDs)
MOTION_PARAMETER_BLOCK *m;
float ds, newDs;
{
	/* Return, in milliseconds, the time required to smoothly
	   change the scale increment from 'ds' to 'newDs'.

	   This is called whenever we are changing the speed of
	   a motion (using 'adjustScale()') and the transition time
	   calculation is left up to the system.

	   The calculation is done using the motion drive
	   parameters to see how much the velocity will change for
	   a given change in velocity scale.
	 */
		
	MOTION_CONTROL_BLOCK *mcb = m->mcb;
	float transTime;
	int new;

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

	if (m->cycle < 2)
	 { /* don't know drive parameters yet. Use the current transition
	      time (m->lastScaleTrans is initialized to be equivalent 
	      to m->currentTau). */
	   return (m->lastScaleTrans);
	 }

	if (m->interMode == CARTESIAN)
	 { 
	   DRIVEP da;		/* change in drive parameter velocity */
	   float maxAt;		/* max. translational acceleration */
	   float maxAr;		/* max. rotational acceleration */
	   float dvt2;		/* est. change in trans vel., squared */
	   float dvr2;		/* est. change in rot vel., squared */

	   maxAt = mcb->activeTransAccel[new];
	   maxAr = mcb->activeRotAccel[new];

	   scaleDrive (&da, 1000.0*(newDs-ds)/m->sampleTime, &m->drivePar);
	   dvt2 = da.dx*da.dx + da.dy*da.dy + da.dz*da.dz;
	   dvr2 = da.dthe*da.dthe + da.dphi*da.dphi;

	   transTime = sqrt(MAX(dvt2/(maxAt*maxAt),dvr2/(maxAr*maxAr)));
	 }
	else /* m->interMode == JOINT */
	 { JNTS daj;		/* change in drive parameter velocity */
	   float *maxAj;	/* max. accelerations */
	   float dtime;		/* transition time, per joint */
	   float transTime;	/* max transition time over all joints */
	   
	   scaleJnts (&daj, 1000.0*(newDs-ds)/m->sampleTime, &m->driveJnts);

	   maxAj = mcb->activeJointAccel[new].v;

	   transTime = -1;
	   dtime = daj.v[0] / maxAj[0];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = daj.v[1] / maxAj[1];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = daj.v[2] / maxAj[2];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    

	   dtime = daj.v[3] / maxAj[3];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = daj.v[4] / maxAj[4];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	   dtime = daj.v[5] / maxAj[5];
	   if (FABS(dtime) > transTime)
	    { transTime = FABS(dtime);
	    }		    
	 }
	return (1000.0*transTime);
}
