/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 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:	setModes
 *
 * DESCRIPTION:	set and get various control modes and options for the 
 *		a particular manipulator.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

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

unsigned long rcclSetModes (mnp, mask)
MANIP *mnp;
unsigned long mask;
{
	/* Set the modes specified by 'mask' */

	char *fxnName = "rcclSetModes()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;
	unsigned long old;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	old = mask & mcb->mode;
	mcb->mode |= mask;
	mvb->mode = mcb->mode;

	if (mask & INTEGRATE_MODE)
	 { RCI_RBT *rbt;
	   if (rbt = MANIP_ROBOT(mnp))
	    { SET_INTEGRATION (rbt, ~0);
	    }
	 }

	return (old);
}

unsigned long rcclClearModes (mnp, mask)
MANIP *mnp;
unsigned long mask;
{
	/* Clear the modes specified by 'mask' */

	char *fxnName = "rcclClearModes()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;
	unsigned long old;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	old = mask & mcb->mode;
	mcb->mode &= ~mask;
	mvb->mode = mcb->mode;

	if (mask & INTEGRATE_MODE)
	 { RCI_RBT *rbt;
	   if (rbt = MANIP_ROBOT(mnp))
	    { SET_INTEGRATION (rbt, 0);
	    }
	 }

	return (old);
}

unsigned long rcclGetModes (mnp, mask)
MANIP *mnp;
unsigned long mask;
{
	/* Return the value of the modes specified by 'mask' */
	
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	return (mcb->mode & mask);
}

#define DELAYED_MODES	(COMPLETE_AT_TB)

unsigned long rcclGetActiveModes (mnp, mask)
MANIP *mnp;
unsigned long mask;
{
	/* Return the modes currently in use by the manipulator */
	
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	return ((mcb->mode & ~DELAYED_MODES) |
		(mcb->activeMode & DELAYED_MODES) );
}

unsigned long rcclPutModes (mnp, mask)
MANIP *mnp;
unsigned long mask;
{
	/* Set the mode word verbatim to the value specified by 'mask' */

	char *fxnName = "rcclPutModes()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;
	unsigned long old;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	old = mcb->mode;
	mcb->mode = mask;
	mvb->mode = mcb->mode;

	if (mask & INTEGRATE_MODE)
	 { RCI_RBT *rbt;
	   if (rbt = MANIP_ROBOT(mnp))
	    { SET_INTEGRATION (rbt, ~0);
	    }
	 }

	return (old);
}

unsigned long rcclDefaultModes()
{
	/* Return the default mode settings */

	return (DEFAULT_MANIP_MODES);
}
