/*-=-=-=-=-=-=-=-=-=-=-=- 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:	getQueue
 *
 * DESCRIPTION:	Routines with which the trajectory task gets items off
 *		of the motion queue.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1986
 *
 * MODIFICATION HISTORY:
 *
 *-------------------------------------------------------------------------*/

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

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

MOTION_REQUEST *motionRequestReady (mq, m)
MOTION_QUEUE *mq;
MOTION_PARAMETER_BLOCK *m;
{
	/* return the address of the next motion request ready to
	   for servicing.
	 */

	register MOTION_REQUEST *mr;
	register MFLAG *mflag;

	MOTION_REQUEST *nextRequest;

	/* First we have to go through and remove any request from the 
	   end of the queue which have been cancelled.
	 */
	mr = NULL;	
	while (test_queue(mq) && mr == NULL)	
	 { mr = get_queue(mq);
	   if (mr->mflagNum != UNDEF && (mflag = &m->mflagBuf[mr->mflagNum]) &&
	       mflag->stopRequest >= 0)
	    { deleteMotion (m, mr, mflag);
	      free_queue(mq);
	      mr = NULL;
	    }
	 }

	/* Return whatever we found */

	return (mr);
}

freeMotionRequest(mq)
MOTION_QUEUE *mq;
{
	/* Free the queue memory for the last motion request */

	free_queue(mq);
}
	
doFlushMotionQueue (mq, m)
register MOTION_QUEUE *mq;
register MOTION_PARAMETER_BLOCK *m;
{
	/* go through the motion queue and delete all the entries.
	   We need to touch each one seperately so that we can
	   set the appropriate codes in its motion flag.
	 */
	 
	register MOTION_REQUEST *mr;
	register MFLAG *mflag;

	while (test_queue(mq))
	 { mr = get_queue(mq);
	   if (mr->mflagNum != UNDEF)
	    { mflag = &(m->mflagBuf[mr->mflagNum]);
	    }
	   else
	    { mflag = NULL;
	    }
	   deleteMotion (m, mr, mflag);
	   free_queue(mq);
	 }
}

deleteMotion (m, mr, mf)
MOTION_PARAMETER_BLOCK *m;
MOTION_REQUEST *mr;
MFLAG *mf;
{
	/* Set the motion flag for a particular motion to 
	   the values appropriate for cancellation.
	 */

	if (mf)
	 { int signum = mf->stopSig;
	   mf->stopSig = 0;
	   mf->stopValue = mf->stopRequest;
	   mf->stopTime = m->sysTime;
	   mf->startValue = ON_ABORT;

	   mf->status = M_FINISHED;
	   if (signum)
	    { rciSignal (signum);
	    }
	 }
	/* Trajectory task needs the number of the motion: */
	m->flushedMotionNumber = mr->number;
}
