/*-=-=-=-=-=-=-=-=-=-=-=- 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:	getMisc
 *
 * DESCRIPTION:	Routines to read back compliance settings.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

#include "../rccl/motionRequest.h"

#define NmToNmm         1000.

getComply(mnp, mask, force)
MANIP *mnp;
unsigned long *mask;
FORCE *force;
{
	/* RCCL primitive to read back the set compliance specification.
	 */

	char *fxnName = "getComply()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	register int index;

	do
	 { index = mcb->complyIndex;
	   if (force)
	    { *force = mcb->setComplyValues[index&1];
	      force->m.x /= NmToNmm;
	      force->m.y /= NmToNmm;
	      force->m.z /= NmToNmm;
	    }
	   *mask = mcb->setComplyMask[index&1];
	 }
	while ((index&~1) != (mcb->complyIndex&~1));
	return (1);
}

getActiveComply(mnp, mask, force)
MANIP *mnp;
unsigned long *mask;
register FORCE *force;
{
	/* RCCL primitive to read back the compliance spec. currently
	   active on a particular manipulator.
	 */

	char *fxnName = "getActiveComply()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	register int index;

	do
	 { index = mcb->pathsegIndex;
	   if (force)
	    { *force = mcb->activeComplyVals[index&1];
	      force->m.x /= NmToNmm;
	      force->m.y /= NmToNmm;
	      force->m.z /= NmToNmm;
	    }
	   *mask = mcb->activeComplyMask[index&1];
	 }
	while ((index&~1) != (mcb->pathsegIndex&~1));
	return (1);
}
