/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Andre Sokalski.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	syncMotions
 *
 * DESCRIPTION:	Routines to provide motion syncronization and control.
 * 
 * AUTHOR/DATE:	Andre Sokalski and John Lloyd, Fall 1986.
 *
 * MODIFICATION HISTORY:
 *
 *------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------

	The basic object used to control and syncronize motions is
	the MFLAG (see the include file "rcclDefs.h").

	One MFLAG is assigned to each motion. The MFLAG contains the
	following fields:

	int stopRequest;

		(set by the RCCL program to stop or abort a motion)	
		The stop flag for the motion. If set to a non-negative value 
		with the primitive stopMotion(), the motion will be halted
	       	(if it is currently running) or aborted from the queue 
		(if it is still pending). The value placed in the field will
		then be copied by the trajectory generator into the
		'stopValue' field.

	int startRequest;

		(set by the RCCL program to start a motion)	
		The start flag associated with the motion. The trajectory
		generator will not proceed to service the motion until this
		field contains a non-negative value. Normally, the (positive)
		value ON_NORMAL is placed in this field when the motion is
		first allocated, and that is that. But the primitive
		setMotionHold() will disable this for the subsequent motion
		request, requiring the user to explicitly set the field with
		the startMotion() primitive at a later date. When the
		trajectory generator starts servicing the motion, it copies
		the value in this field into the 'startValue' field.

	int stopValue;

		(set only by the trajectory generator)		
		A condition code updated when the motion terminates.
		It is intended to give some information about how the motion
		terminated. Unless otherwise specified, the system will use
		the value ON_NORMAL. 

	int startValue;

		(set only by the trajectory generator)		
		A condition code set when the motion was first serviced.
		Unless otherwise specified, the system will use the value
		ON_NORMAL.

	int status;

		(set only by the trajectory generator)		
		Current status of the motion. The status values are:

			M_PENDING   Motion is on the queue
			M_RUNNING   Motion is being serviced
			M_PAUSED    Motion has been paused
			M_FINISHED  Motion is finished


	float scale;

		(set only by the trajectory generator)
		A parameter which nominally varies from 0.0 to 1.0 as the
		motion runs its course. When the transition time is non-zero,
		scale will assume a slightly negative value near the beginning
		of the motion since symmetrical transtioning requires that a
		motion actually begin a little BEFORE its nominal start time.

	int stopSig;

		Used internally by RCCL to specify a UNIX signal which should
		be delivered to the planning task when the motion completes.

 	A global array of MFLAGS is visible to all parts of the RCCL program. 
	A particular MFLAG is identifed within this array by a small
	integer. In particular, the first NUM_USRMFLAGS are reserved for use
	and allocation by the user. The remaining (NUM_SYSMFLAGS) are
	used by the system to automatically assign an MFLAG to every motion
	for which the programmer has not already assigned one.

  ------------------------------------------------------------------------*/

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

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

/* check if we are in planning task or not */
extern int rciTaskId;
#define INSIDE_PLANNING_TASK() (rciTaskId == 0)

static RCCL_GMEM *gmem;

mapFlag(e)
{
	/* Called to extract the MFLAG index from the programmers
	   specification (which will be either a motion ID or an
	   explicit flag number). Returns -1 if the flag is invalid.
	*/

	if (e < 0)
	 { return (-1);
	 }
	else if (e < NUM_USRMFLAGS)
	 { /* explicit flag value was given */
	   return (e);
	 }
	else
	 { /* motion ID was given. Flag value was allocated by system, and is 
	      buried in the low bits of the motion ID. */
	   e -= NUM_USRMFLAGS;
	   if (e >= (gmem->motionFlagNumber-NUM_SYSMFLAGS) && 
		 e < gmem->motionFlagNumber)
	    { return ((e%NUM_SYSMFLAGS)+NUM_USRMFLAGS);
	    }
	 }
	return (-1);
}

setMotionFlag (mnp, mflagNum)
MANIP *mnp;
int mflagNum;
{
	/* RCCL primitive specifying request that the next motion request
	   should use 'mflagNum' as the index for its MFLAG.
	 */
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	char *fxnName = "setMotionFlag()";
	MFLAG *mf;
	int mid;

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if (!planningLevelCheck (fxnName))
	 { return (-1);
	 }
	if(mflagNum < 0 || mflagNum >= NUM_USRMFLAGS)
	 { errorMsg (EDescriptor, "%s bad motion flag number\n", fxnName);
	   RETURN_ERROR (-1);
	 }
	if (gmem->mflagBuf[mflagNum].status & (M_RUNNING | M_PENDING))
	 { errorMsg (EBusy, "%s motion flag already in use\n", fxnName);
	   RETURN_ERROR(-1);
	 }

	/* Initialize the MFLAG */

	mf = &gmem->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;

	mvb->mflagNum = mflagNum;
	RETURN_NORMAL(1);
}

setMotionHold (mnp)
MANIP *mnp;
{
	/* RCCL primitive specifying that the next motion request
	   should not have the 'startRequest' field of its MFLAG set by
	   default, but rather it should be necessary for the program to set
	   the field with the 'startMotion()' primitive.
	*/
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	char *fxnName = "setMotionHold()";

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	mvb->motionHold = YES;
	RETURN_NORMAL(1);
}

stopCurrentMotion (mnp, stopCode)
MANIP *mnp;
int stopCode;
{
	/* RCCL primitive aborting any motion on 'mnp' that happens to be
	   current. The 'stopCode' value will be read into the 'stopValue'
	   field of the motion's MFLAG.
	 */
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	char *fxnName = "stopCurrentMotion()";

	if (stopCode < 0)
	 { mcb->terminateCode = ON_ABORT;
	 }
	else
	 { mcb->terminateCode = stopCode;
	 }
	mcb->terminateReq = 1;
	return (1);
}

flushMotionQueue (mnp)
MANIP *mnp;
{
	/* RCCL primitive to flush the motion queue. All we do is hit the
	   'flushReq' field of the motion control block, and the trajectory
	   generator will notice this and take the appropriate action.
	 */
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	char *fxnName = "flushMotionQueue()";

	if (INSIDE_PLANNING_TASK())
	 { MASK_RCI_AUTO_ABORT();
	   mcb->flushReq = 1;
	   rcclBlock();
	   if (mcb->flushReq)
	    { rciAbort (EFatal, "%s flush request not noticed\n", fxnName);
	      RETURN_ERROR (-1);
	    }
	   RETURN_NORMAL(1);
	 }
	else
	 { mcb->flushReq = 1;
	   return (1);
	 }
}

stopMotion (mflagNum, stopCode)
int mflagNum;
int stopCode;
{
	/* This routine sets a stop value of a given motion
	   request. This will cause the motion associated
	   with that MFLAG to be cancelled.
	 */

	char *fxnName = "stopMotion()";

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	if (stopCode < 0)
	 { gmem->mflagBuf[mflagNum].stopRequest = ON_ABORT;
	 }
	else
	 { gmem->mflagBuf[mflagNum].stopRequest = stopCode;
	 }
	RETURN_NORMAL(1);
}

startMotion (mflagNum, startCode)
int mflagNum;
int startCode;
{
	/* This routine copies the startCode into the motion flag
	   structure. This will give the "green light" for the motion to be
	   serviced by the trajectory generator.
	*/
	char *fxnName = "startMotion()";
	
	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	if (startCode < 0)
	 { gmem->mflagBuf[mflagNum].startRequest = ON_NORMAL;
	 }
	else
	 { gmem->mflagBuf[mflagNum].startRequest = startCode;
	 }
	RETURN_NORMAL(1);
}

adjustSpeed (mflagNum, speed, trans)
int mflagNum;
float speed;
float trans;
{
	/* This routine adjusts the execution speed of the
	   motion identified by "mflagNum".
	 */

	char *fxnName = "adjustSpeed()";
	register MFLAG *mf;

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	if (speed < 0.0 || (trans < 0.0 && trans != F_DEFAULT))
	 { errorMsg (ERange, rcclArgNegMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	/* XXX setting these three things should be made atomic */

	mf = &gmem->mflagBuf[mflagNum];
	mf->timeCmdVal = speed;
	mf->timeCmdTrans = trans;
	mf->timeCmd = MF_ADJUST_SPEED;
	RETURN_NORMAL(1);
}

adjustStopTime (mflagNum, time, trans)
int mflagNum;
float time;
float trans;
{
	/* This routine adjusts the desired termination time of the
	   motion identified by "mflagNum".
	 */

	char *fxnName = "adjustStopTime()";
	register MFLAG *mf;

	if ((gmem = globalMemoryCheck(fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	if (time < 0.0 || (trans < 0.0 && trans != F_DEFAULT))
	 { errorMsg (ERange, rcclArgNegMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	/* XXX setting these three things should be made atomic */

	mf = &gmem->mflagBuf[mflagNum];
	mf->timeCmdVal = time;
	mf->timeCmdTrans = trans;
	mf->timeCmd = MF_ADJUST_STOPTIME;

	RETURN_NORMAL(1);
}

motionStatus (mflagNum)
int mflagNum;
{
	/* Returns the status of the motion associated with the specified
	   MFLAG.
	 */
	char *fxnName = "motionStatus()";

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR(MSTAT_ERR);
	 }
	RETURN_NORMAL (gmem->mflagBuf[mflagNum].status);
}

motionStartCode (mflagNum)
int mflagNum;
{
	/* Returns the start code associated with a particular MFLAG. The
	   value UNDEF will be returned if the motion has not yet started.
	 */
	char *fxnName = "motionStartCode()";

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (ERange, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR(MSTAT_ERR);
	 }
	RETURN_NORMAL (gmem->mflagBuf[mflagNum].startValue);
}

motionStopCode (mflagNum)
int mflagNum;
{
	/* Returns the stop code associated with a particular MFLAG. The
	   value UNDEF will be returned if the motion has not yet
	   completed.
	 */
	char *fxnName = "motionStopCode()";

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR(MSTAT_ERR);
	 }
	RETURN_NORMAL (gmem->mflagBuf[mflagNum].stopValue);
}

float motionStopTime (mflagNum)
int mflagNum;
{
	/* Returns the time at which the indicated motion terminated */

	char *fxnName = "motionStopTime()";

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR(MSTAT_ERR);
	 }
	RETURN_NORMAL (gmem->mflagBuf[mflagNum].stopTime);
}

float motionStartTime (mflagNum)
int mflagNum;
{
	/* Returns the time at which the indicated motion started */

	char *fxnName = "motionStartTime()";

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR(MSTAT_ERR);
	 }
	RETURN_NORMAL (gmem->mflagBuf[mflagNum].startTime);
}

float motionScale (mflagNum)
int mflagNum;
{
	/* Returns the motion scale value associated with a 
	   particular MFLAG.
	 */
	char *fxnName = "motionScale()";

	if ((gmem = globalMemoryCheck (fxnName)) == NULL)
	 { return (-1.0);
	 }
	if((mflagNum = mapFlag(mflagNum)) < 0)
	 { errorMsg (EDescriptor, rcclBadMflagMsg, fxnName);
	   RETURN_ERROR(MSTAT_ERR);
	 }
	RETURN_NORMAL (gmem->mflagBuf[mflagNum].scale);
}


setMotionQueueSize (mnp, size)
MANIP *mnp;
int size;
{
	/* RCCL primitive to set the motion queue size */

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	char *fxnName = "setMotionQueueSize()";
	int oldsize;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (size > MAX_MQ_ENTRIES || size < 0)
	 { errorMsg (ERange, rcclArgRangeMsg, fxnName, 0, MAX_MQ_ENTRIES);
	   RETURN_ERROR(-1);
	 }
	oldsize = mcb->motionQueueSize;
	mcb->motionQueueSize = size;
	RETURN_NORMAL (oldsize);
}

getMotionQueueSize (mnp)
MANIP *mnp;
{
	/* Return the current motion queue size */

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	return (mcb->motionQueueSize);
}

numMotionsQueued (mnp)
MANIP *mnp;
{
	/* Return the number of motions currently on the queue */

	MOTION_QUEUE *mq = (MOTION_QUEUE*)mnp->mq;
	return (motions_queued(mq));	
}

checkMotionQueue (mnp, pos)
MANIP *mnp;
POS *pos;
{
	/* returns true if there is room on the motion queue for the
	   specified motion */
	
	char *fxnName = "checkMotionQueue()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	MOTION_QUEUE *mq = (MOTION_QUEUE*)mnp->mq;
	int posSize;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (pos)
	 { posSize = sizeof(POS) + numOptimizedTerms(pos, mvb->shiftSet)* 
				    (sizeof(POS_TERM)+sizeof(TRSF_DESC));
	 }
	else
	 { posSize = 0;
	 }
	return (check_queue (mq, sizeof(MOTION_VALUE_BLOCK) + posSize));
}

init_syncro ()
{
	/* Called once ar program startup time to initialize the 
	   global MFLAG pool.
	 */
	MFLAG eInit;
	int i;

	if ((gmem = globalMemoryCheck ("init_syncro")) == NULL)
	 { return (-1);
	 }
	
	eInit.startValue = eInit.startRequest = -1;
	eInit.stopValue = eInit.stopRequest = -1;
	eInit.status = 0;
	eInit.scale = 0.0;
	eInit.stopSig = 0;
	eInit.startTime = UNDEF;
	eInit.stopTime = UNDEF;
	eInit.timeCmd = 0;

	for (i=0; i< NUM_USRMFLAGS*2; i++)
	 { gmem->mflagBuf[i] = eInit;
	 }
}
