/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 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:	motionRequest
 *
 * DESCRIPTION:	Contains code which queues the motion requests from the planning
 * 		level to the trajectory generator. 
 *
 * AUTHOR/DATE:	John Lloyd, McGill University, Fall 1988, after
 *		Vincent Hayward, Purdue University 1983.
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/robotMath.h"
#include "../h/rcclDefs.h"
#include "../h/errorCodes.h"
#include "../h/vector.h"
#include "../h/assignMem.h"

#include "../rccl/motionRequest.h"
#include "../rccl/setpoint.h"
#include "../rccl/transform.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"
#include "../rccl/kynMacros.h"
#include "../rccl/motionQueue.h"

#include "../h/robot_kynvar.h"

MOTION_REQUEST *reserve_queue_insert();

int rcclMotionNumber;	        	/* unique motion number */
extern RCCL_GMEM *rcclGmem;		/* global memory */
int printQueuedEqn = 0;			/* a debugging option */

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	initDone = 1;
	rcclMotionNumber = 0;
	printQueuedEqn = 0;
	requestFlagInit (&initDone);
}

motionRequest(mnp)
MANIP *mnp;
{
	/* Queue a motion request to the trajectory generator */

	char *fxnName = "motionRequest()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	MOTION_QUEUE *mq = (MOTION_QUEUE*)mnp->mq;
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	MOTION_REQUEST *req;		/* pointer to allocated motion req. */
	int termCount;			/* no. terms in target pos. eqn. */
	int motionId;			/* identification no. of motion */
	int mflagNum;			/* pointer to motion control flag */
	char *mem;			/* pointer for building motion req. */

	TRSF tC0;			/* current T6 value of target */
	JNTS jC0;			/* current joint value of target */
	int goalPosFixed;		/* the goal position is not moving */
	RCI_RBT *rbt;			/* RCI robot structure */

	INIT_CHECK();

	rbt = MANIP_ROBOT(mnp);

	if (mvb->goalPosition && mvb->goalPosition->t6Term == NULL)
	 { errorMsg (ENoT6, rcclNoT6TransMsg, fxnName);
	   return (-1);
	 }

	/* check for shift transform (if any) now, so we can build it 
	   into the motion target position. Also, check for any change
	   in the joint biasing.
	 */

	if (!mvb->stopMotion)
	 { if (mvb->shiftSet = mvb->newShiftSet)
	    { mvb->shiftTransform = mvb->newShiftTransform;
	      mvb->newShiftSet = NO;
	    }
	   if (mvb->newJbiasSetting)
	    { mvb->jbiasFxn = mvb->newJbiasFxn;
	      mvb->jbiasEnable = mvb->newJbiasEnable;
	      mvb->newJbiasSetting = NO;
	    }
	 }

	/* Compute the target position values tC0 or jC0 as best
	   we can. If the target is fixed (not moving in time), then
	   we can compute these values exactly. We will determine 
	   if the target is fixed on this pass through the position
	   equation. The target is not fixed if the position
	   equation contains functional transforms. At the time of 
	   this writing, the trajectory generator will only use
	   tC0 and jC0 if the target is fixed. For completeness, we
	   compute them regardless.
	*/

	goalPosFixed = 1;	     
	if (mvb->goalPosition)
	 { POS *p = mvb->goalPosition;
	   POS_TERM *term;

	   /* Be more efficient to compute this after the optimized
	      position was built, but I want tC0 before I allocate the
	      queue space so I can do my range limit checking */

	   identTrsf (&tC0);
	   if (p->t6Term->leftSide)
	    { for (term=p->t6Term->prev; term!=p->t6Term; term=term->prev)
	       { if (TRSF_GETFXN(term->trsf) != constTrsf)
		  { goalPosFixed = 0;
		  }
		 if (term->t6Side)
		  { multRiTrsf (&tC0, &tC0, term->trsf);
		  }
		 else
		  { multTrsf (&tC0, &tC0, term->trsf);
		  }
		 if (mvb->shiftSet && term == p->toolTerm->next)
		  { multTrsf (&tC0, &tC0, &mvb->shiftTransform);
		  }
	       }
	    }
	   else
	    { for (term=p->t6Term->next; term!=p->t6Term; term=term->next)
	       { if (TRSF_GETFXN(term->trsf) != constTrsf)
		  { goalPosFixed = 0;
		  }
		 if (term->t6Side)
		  { multRiTrsf (&tC0, &tC0, term->trsf);
		  }
		 else
		  { multTrsf (&tC0, &tC0, term->trsf);
		  }
		 if (mvb->shiftSet && term == p->toolTerm->prev)
		  { multTrsf (&tC0, &tC0, &mvb->shiftTransform);
		  }
	       }
	    }
	   mvb->goalPosFixed = goalPosFixed;
	   mvb->relativePosition = (mvb->goalPosition == mnp->last);
	   mvb->tC0 = tC0;

	   /* Compute the inverse kinematics. This won't be used by the 
	      trajectory generator if the target is not fixed, but I 
	      compute it anyway for completeness.
	    */
	   if (rbt)
	    { int config;
	      int code;

	      config = mnp->conf;
	      if (mvb->configSet && !mvb->stopMotion)
	       { config = (config & ~mvb->configSet) | mvb->config;
	       }
	      code = INV_KINEMATICS (jC0.v, &tC0, config, mnp->j6->v, NULL);
	      mvb->targetInRange = !(code & (KYN_JOINT_LIMIT|KYN_CANT_REACH));
	      mvb->configUsed = config;
	    }	      
	 }
	else if (mvb->jointPositionSet)
	 { 
	   mvb->goalPosFixed = YES;
	   mvb->relativePosition = NO;
	   jC0 = mvb->jointPosition;
	   FWD_KINEMATICS (&tC0, NULL, jC0.v, NULL);	      
	   mvb->targetInRange = YES;
	 }
	else
	 { fatalMsg (-1, "%s NULL position specified\n", fxnName);
	 }
	mvb->tC0 = tC0;
	mvb->jC0 = jC0;

	if (mvb->goalPosition)
	 { 
	   /* determine no. of position terms, and the position size */

	   termCount = numOptimizedTerms(mvb->goalPosition, mvb->shiftSet);
	   mvb->positionSize = sizeof(POS) + termCount*(sizeof(POS_TERM) 
			    + sizeof(TRSF_DESC));

	   if (mvb->positionSize > MAX_GOAL_POSITION)
	    { fatalMsg (-1, "%s goal position %s is too large\n",
			 fxnName, getMemName(mvb->goalPosition));
	    }
	 }
	else /* mvb->jointPositionSet */
	 { mvb->positionSize = 0;
	 }

	/* Grab memory for the request off the motion queue */

	mem = (char*)reserve_queue_insert (mq,
			  ALIGN_SIZE(sizeof(MOTION_REQUEST))
			+ ALIGN_SIZE(mvb->positionSize) );

	if (mem == NULL)
	 { errorMsg (ENoSpace, 
	"%s can't allocate space for motion\n", fxnName);
	   return(-1);
	 }
	ASSIGN_MEM (req, MOTION_REQUEST*, sizeof(MOTION_REQUEST), mem);

	/* build the optimized position request, with the shift
	   transform (if any) built in, and store it into the
	   the motion request packet.
	 */

	if (mvb->goalPosition)
	 { POS *p;
	   POS_TERM *t;
	   TRSF trans;

	   ASSIGN_MEM (p, POS*, mvb->positionSize, mem);
	
	   bindPosition (mvb->goalPosition, mnp->t6);
	   makeOptimizedPosition (p,
				  mvb->goalPosition, 
				  mvb->shiftSet ? &mvb->shiftTransform : NULL,
				  termCount);
	   bindPosition (mvb->goalPosition, T6);

	   if (printQueuedEqn)
	    { printPosition (p, printQueuedEqn == 2 ? YES : NO);
	    }
	   mvb->positionAddr = p; /* for trajectory task to rebuild links */
	 }

#if KILROY
	/* If there is an update transform to be evaluated at the
	   end of the motion, build the equation list for it */

	if (mvb->updateSize)
	 { POS_TRSF *list;
	   int k;
	
	   ASSIGN_MEM (list, POS_TRSF*, mvb->updateSize*sizeof(POS_TRSF),mem);
	   for (k=0; k<mvb->updateSize; k++)
	    { list[k] = mvb->updatePosList[k];
	    }
	 }
#endif

	/* Asssign a number to the motion. Mark this number on the
	   MANIP structure and in the motion request */

	++rcclMotionNumber;
	mcb->headMotionNumber = rcclMotionNumber;	
	mvb->number = rcclMotionNumber;

	/* If the motion does not already have an MFLAG assigned,
	   assign one */

	if (mvb->mflagNum == UNDEF)
	 { MFLAG *mf;

	   motionId = rcclGmem->motionFlagNumber + NUM_USRMFLAGS;
	   mflagNum = ((rcclGmem->motionFlagNumber++)%NUM_SYSMFLAGS) 
		      + NUM_USRMFLAGS;
	   mvb->mflagNum = mflagNum;

	   /* initialize the motion event flag */

	   mf = &rcclGmem->mflagBuf[mflagNum];

	   mf->stopRequest = -1;
	   mf->stopValue = -1;
	   mf->startRequest = mvb->motionHold ? -1 : ON_NORMAL;
	   mf->startValue = -1;
	   mf->scale = 0.0;
	   mf->status = M_PENDING;
	   mf->startTime = UNDEF;
	   mf->stopTime = UNDEF;
	   mf->timeCmd = 0;
	 }
	else
	 { mflagNum = motionId = mvb->mflagNum;
	 }

	/* determine if there is a joint bias set */

	mvb->jbiasFixed = YES;
	if (mvb->jbiasEnable)
	 { if (mvb->jbiasFxn.fxn == constTrsf)
	    { readMem (mnp->jbias, &mvb->jbias);
	    }
	   else
	    { mvb->jbiasFixed = NO;
	    }
	 }
	
	/* now go through and tag all variable transforms with the 
	   motion number */

	if (mvb->goalPosition)
	 { referenceTransforms (mvb->goalPosition, mvb->number);
	 }

	/* Build the appropriate motion request parameters */

	setupMotionRequest (req, mcb, mvb, (rbt==NULL));
	confirm_queue_insert(mq);

	/* If the mnp->completed == 1, then wait at least one control
	   cycle for trajectory generator to notice the new packet */

	if (mnp->completed == 1)
	 { rcclBlock();
	 }

	/* Finally, block for an appropriate length as required by the
	   motion queue size */

	if (mcb->motionQueueSize == 0)
	 { rcclBlock();
	   while (!mnp->completed)
	    { delay ((float)rcclGetInterval());
#if 0
/* taken out because this is not necessarily a deadlock condition.
   The start code could be set from a monitor function */
	      if (mcb->waitingForStart && !mnp->completed)
	       { errorMsg (EDeadLock, 
	"%s deadlock waiting for motion to start\n", fxnName);
		 return (-1);
	       }
#endif
	    }
	 }
	else
	 { while (motions_queued(mq) > mcb->motionQueueSize-1)
	    { delay ((float)rcclGetInterval());
#if 0
	      if (mcb->waitingForStart && 
		  (motions_queued(mq) > mcb->motionQueueSize-1))
	       { errorMsg (EDeadLock, 
	"%s deadlock waiting for motion to start\n", fxnName);
		 return (-1);
	       }
#endif
	    }
	 }
	return(motionId);
}

static FORCE zeroforce = {0., 0., 0., 0., 0., 0.};
static DIFF  zerodiff  = {0., 0., 0., 0., 0., 0.};

motionRequestInit(mnp)
MANIP *mnp;
{
	/* 
	   Called each time we 'create' a robot with rcclCreate(). 
	   Sets up the motion template and initializes it.
	*/

	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	MOTION_REQUEST *req = (MOTION_REQUEST *)mnp->mr;
	RCI_RBT *rbt;

	extern float rcclDefaultTransVel;
	extern float rcclDefaultRotVel;
	extern float rcclDefaultSpeed;

	INIT_CHECK();

	mvb->goalPosition	= NULL;
	mvb->jointPositionSet	= NO;

	mvb->condition		= UNDEF;
	mvb->motionHold 	= NO;
	mvb->newInterModeSet	= NO;
	mcb->setInterMode = mnp->objectFlag ? CARTESIAN : DEFAULT_INTERMODE;
	mcb->activeInterMode[0] = mcb->setInterMode;
	mvb->interMode		= mcb->setInterMode;

	mvb->transitionTimeSet	= NO;
	mvb->segmentTimeSet	= NO;
	mvb->startTimeSet	= NO;
	mvb->stopTimeSet	= NO;
	mvb->tauSkew 		= 0.5;

	if (rbt = MANIP_ROBOT(mnp))
	 { computeDefaultVel   (rbt, &mcb->defaultJointVel,
				     &mcb->defaultTransVel,
				     &mcb->defaultRotVel);
	   computeDefaultAccel (rbt, &mcb->defaultJointAccel,
				     &mcb->defaultTransAccel,
				     &mcb->defaultRotAccel);
	 }
	else
	 { zeroVf (mcb->defaultJointVel.v, NUM_JNTS);

	   mcb->defaultTransAccel = DEFAULT_TRANS_ACCEL;
	   mcb->defaultRotAccel   = DEFAULT_ROT_ACCEL * DEGTORAD;
	   zeroVf (mcb->defaultJointAccel.v, NUM_JNTS);
	 }

	/* override computed setting with these for now ... */
	mcb->defaultTransVel = rcclDefaultTransVel;
	mcb->defaultRotVel   = rcclDefaultRotVel;

	mcb->setTransVel[0]  = mcb->defaultTransVel;
	mcb->setRotVel[0]    = mcb->defaultRotVel;
	mcb->setJointVel[0]  = mcb->defaultJointVel;
	mcb->cartVelIndex    = 0;
	mcb->jointVelIndex   = 0;

	mcb->activeTransVel[0]  = mcb->defaultTransVel;
	mcb->activeRotVel[0]    = mcb->defaultRotVel;
	mcb->activeJointVel[0]  = mcb->defaultJointVel;
/*
	mcb->activeTransVel[1]  = mcb->defaultTransVel;
	mcb->activeRotVel[1]    = mcb->defaultRotVel;
	mcb->activeJointVel[1]  = mcb->defaultJointVel;
*/
	mcb->setTransAccel[0] = mcb->defaultTransAccel;
	mcb->setRotAccel[0]  = mcb->defaultRotAccel;
	mcb->setJointAccel[0] = mcb->defaultJointAccel;
	mcb->cartAccelIndex  = 0;
	mcb->jointAccelIndex = 0;

	mcb->activeTransAccel[0] = mcb->defaultTransAccel;
	mcb->activeRotAccel[0]  = mcb->defaultRotAccel;
	mcb->activeJointAccel[0] = mcb->defaultJointAccel;
/*
	mcb->activeTransAccel[1] = mcb->defaultTransAccel;
	mcb->activeRotAccel[1]  = mcb->defaultRotAccel;
	mcb->activeJointAccel[1] = mcb->defaultJointAccel;
*/
	mcb->setSpeed 	     = 1.0;
	mcb->setJointSpeed   = 1.0;
	mcb->setAccelScale   = 1.0;
	mcb->setJointAccelScale = 1.0;
	
	mvb->transVel   = mcb->setTransVel[0] * rcclDefaultSpeed;
	mvb->rotVel     = mcb->setRotVel[0]   * rcclDefaultSpeed;
	scaleVf(mvb->jointVel.v, rcclDefaultSpeed, mcb->setJointVel[0].v, 6);

	mvb->transAccel = mcb->setTransAccel[0];
	mvb->rotAccel   = mcb->setRotAccel[0];
	mvb->jointAccel = mcb->setJointAccel[0];

	mvb->configSet		= 0;
	mvb->config		= UNDEF;

	mvb->newShiftSet	= NO;
	mvb->shiftSet		= NO;

	mvb->mode 		= rcclDefaultModes();
	mvb->mflagNum		= UNDEF; /* just set this for laughs */
	mvb->objectLoad 	= 0.0;	/* not used */
	mvb->stopOnNextRequest  = NO;

	mvb->numMotionFxns	= 0;	
	mvb->newJbiasSetting 	= NO;
	mvb->newJbiasEnable	= NO;
	mvb->newJbiasFxn.fxn 	= constTrsf;
	mvb->jbiasEnable	= NO;
	mvb->jbiasFxn.fxn 	= constTrsf;
	mvb->updatePos		= NULL;
#if KILROY
	mvb->updateSize		= 0;
	mvb->updateT6		= NULL;
#endif

	mvb->diffLimitMask	= 0;
	mvb->forceLimitMask 	= 0;
	mcb->setComplyMask[0]	= 0;
	mcb->complyIndex = 0;
}

setupMotionRequest (req, mcb, mvb, objectFrame)
MOTION_REQUEST *req;
MOTION_CONTROL_BLOCK *mcb;
MOTION_VALUE_BLOCK *mvb;
int objectFrame;
{
	/* Called once for every motion request to instantiate the
	   motion request block for that motion. In most cases,
	   it simply copies the equivalent fields from the motion
	   value block. Stop requests do not always respond to all
	   settings of motion parameters.
	 */

	register int i, k, idx;

	req->goalPosition = mvb->goalPosition;
	req->jointPositionSet = mvb->jointPositionSet;
	req->jointPosition = mvb->jointPosition;
	req->positionSize = mvb->positionSize;
	req->positionAddr = mvb->positionAddr;
	req->tC0 = mvb->tC0;
	req->jC0 = mvb->jC0;

	req->condition = mvb->condition;
	mvb->condition = UNDEF;

	req->motionHold	= mvb->motionHold;
	mvb->motionHold = NO;

	if (!mvb->stopMotion)
	 { if (mvb->newInterModeSet)
	    { mvb->interMode = mcb->setInterMode;
	      mvb->newInterModeSet = NO;
	    }
	 }
	req->interMode = mvb->interMode;

	if (req->transitionTimeSet = mvb->transitionTimeSet)
	 { req->transitionTime = mvb->transitionTime;
	   mvb->transitionTimeSet = NO;
	 }

	if (req->segmentTimeSet = mvb->segmentTimeSet)
	 { req->segmentTime = mvb->segmentTime;
	   mvb->segmentTimeSet = NO;
	 }

	if (req->stopTimeSet = mvb->stopTimeSet)
	 { req->stopTime = mvb->stopTime;
	   mvb->stopTimeSet = NO;
	 }

	if (req->startTimeSet = mvb->startTimeSet)
	 { req->startTime = mvb->startTime;
	   mvb->startTimeSet = NO;
	 }

	req->tauSkew = mvb->tauSkew;
	mvb->tauSkew = 0.5;

	req->transVel = mvb->transVel;
	req->rotVel = mvb->rotVel;

	req->jointVel = mvb->jointVel;

	req->transAccel = mvb->transAccel;
	req->rotAccel = mvb->rotAccel;

	req->jointAccel = mvb->jointAccel;

	/* included to save computation at control level */
	req->transAccelDist = (req->transVel*req->transVel)/req->transAccel;
	req->rotAccelDist = (req->rotVel*req->rotVel)/req->rotAccel;
	if (!objectFrame)
	 { for (k=0; k<NUM_JNTS; k++)
	    { req->jointAccelDist.v[k] = 
		(req->jointVel.v[k]*req->jointVel.v[k])/req->jointAccel.v[k];
	    }
	 }

	/* included to save computation at control level */
	req->transAccelTime = (2.0*req->transVel)/req->transAccel;
	req->rotAccelTime = (2.0*req->rotVel)/req->rotAccel;
	if (!objectFrame)
	 { for (k=0; k<NUM_JNTS; k++)
	    { req->jointAccelTime.v[k] = 
			   (2.0*req->jointVel.v[k])/req->jointAccel.v[k];
	    }
	 }

	if (!mvb->stopMotion && mvb->configSet)
	 { req->configSet = mvb->configSet;
	   req->config = mvb->config;
	   mvb->configSet = 0;
	 }
	else
	 { req->configSet = 0;
	   req->config = 0;
	 }

	if (req->shiftSet = mvb->shiftSet)
	 { req->shiftTransform = mvb->shiftTransform;
	 }

	req->mode = mvb->mode;
	req->mflagNum = mvb->mflagNum;
	mvb->mflagNum = UNDEF;	/* just clear this for laughs */
	req->objectLoad = mvb->objectLoad; /* not used */
	req->stopOnNextRequest = mvb->stopOnNextRequest;
	mvb->stopOnNextRequest = NO;

	req->number = mvb->number;
	req->doDrive = mvb->doDrive;
	req->stopMotion	= mvb->stopMotion;
	req->targetFixed = (mvb->goalPosFixed & mvb->jbiasFixed);
	req->goalPosFixed = mvb->goalPosFixed;
	req->jbiasFixed = mvb->jbiasFixed;
	req->relativePosition = mvb->relativePosition;
	req->targetInRange = mvb->targetInRange;
	req->configUsed = mvb->configUsed;

	if (req->numMotionFxns = mvb->numMotionFxns)
	 { for (i=0; i<req->numMotionFxns; i++)
	    { req->motionFxns[i] = mvb->motionFxns[i];
	    }
	   mvb->numMotionFxns = 0;
	 }
	req->jbiasEnable = mvb->jbiasEnable;
	req->jbias = mvb->jbias;
	req->jbiasFxn = mvb->jbiasFxn;

	if (req->updatePos = mvb->updatePos)
	 { req->updateTr = mvb->updateTr;
	   req->updateT1 = mvb->updateT1;
	   req->updateTx = mvb->updateTx;
	   mvb->updatePos = NULL;
	 }
#if KILROY
	req->updateSize = mvb->updateSize;
	mvb->updateSize = 0;
	req->updateT6 = mvb->updateT6;
	mvb->updateT6 = NULL;
#endif
	if (req->diffLimitMask = mvb->diffLimitMask)
	 { req->diffLimitValues = mvb->diffLimitValues;
	   mvb->diffLimitMask = 0;
	 }

	if (req->forceLimitMask = mvb->forceLimitMask)
	 { req->forceLimitValues = mvb->forceLimitValues;
	   mvb->forceLimitMask = 0;
	 }

	idx = mcb->complyIndex&1;
	if (req->complyMask = mcb->setComplyMask[idx])
	 { req->complyValues = mcb->setComplyValues[idx];
	 }

	/* finally, put a time stamp on the motion so we apply an
	   intrinsic service delay if we want ... */

	req->queueTime = rcclSysTime();
}
