/*-=-=-=-=-=-=-=-=-=-=-=- 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:	setMisc
 *
 * DESCRIPTION:	RCCL primitives for setting miscellaneous motion
 *		parameters.
 * 
 * AUTHOR/DATE:	Vincent Hayward, Purdue University, 1983.
 *		CVaRL installation and modification by John Lloyd.
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

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

setMod(mnp, mode)
MANIP *mnp;
int mode;
{
	/* Request either joint or Cartesian interpolation 
	   for the next motion.
	 */

	char *fxnName = "setMod()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (mode == 'j')
	 { if (!mnp->objectFlag)
	    { mcb->setInterMode = JOINT;
	      mvb->newInterModeSet = YES;
	    }
	   else
	    { errorMsg (ENotDefined, rcclNoVmJmodeMsg, fxnName);
	      RETURN_ERROR(-1);
	    }
	 }
	else if (mode == 'c')
	 { mcb->setInterMode = CARTESIAN;
	   mvb->newInterModeSet = YES;
	 }
	else
	 { errorMsg (ERange, rcclBadArgMsg, fxnName, mode);
	   RETURN_ERROR(-1);
	 }
	RETURN_NORMAL(1);
}

setTime(mnp, t_acc, t_segment)
MANIP *mnp;
double t_acc, t_segment;
{
	/* Request a specific transition and/or path segment time for
	   the next motion.
	 */

	char *fxnName = "setTime()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (t_acc < 0.0 && t_acc != F_DEFAULT)
	 { errorMsg (ERange, rcclArgNegMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	if (t_segment <= 0.0 && t_segment != F_DEFAULT
			     && t_segment != F_UNDEF)
	 { errorMsg (ERange, rcclArgLeqMsg, fxnName, 0.0);
	   RETURN_ERROR(-1);
	 }

	if (t_acc != F_DEFAULT)
	 { mvb->transitionTime = (int)t_acc;
	   mvb->transitionTimeSet = YES;
	 }

	if (t_segment != F_DEFAULT)
	 { if (t_segment == F_UNDEF)
	    { mvb->segmentTime = UNDEF;
	    }
	   else
	    { mvb->segmentTime = (int)t_segment;
	    }
	   mvb->segmentTimeSet = YES;
	   mvb->stopTimeSet = NO;
	 }

	RETURN_NORMAL(1);
}

setStartTime (mnp, time)
MANIP *mnp;
double time;
{
	/* Request that a motion begin at a specific time */

	char *fxnName = "setStartTime()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (time < 0.0)
	 { errorMsg (ERange, rcclArgNegMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	mvb->startTimeSet = YES;
	mvb->startTime = time;
	RETURN_NORMAL(1);
}

setStopTime (mnp, time)
MANIP *mnp;
double time;
{
	/* Request that a motion begin at a specific time */

	char *fxnName = "setStopTime()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (time < 0.0)
	 { errorMsg (ERange, rcclArgNegMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	mvb->stopTimeSet = YES;
	mvb->segmentTimeSet = NO;
	mvb->stopTime = time;
	RETURN_NORMAL(1);
}

setConf (mnp, string)
MANIP *mnp;
const char *string;
{
	/* Request a specific configuration for the mext motion */
	   
	char *fxnName = "setConf()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	int specified;
	int oldconf;
	int conf;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (mnp->objectFlag)
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	if ((specified = strToConfig (&conf, string, mnp->kyn)) == -1)
	 { errorMsg (ESyntax, rcclBadSpecStrMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	mvb->configSet = specified;
	oldconf = (mvb->config == UNDEF ? mnp->conf : mvb->config);
	mvb->config = (oldconf & ~specified) | conf;
	RETURN_NORMAL (specified);
}

setTransitionSkew (mnp, skew)
MANIP *mnp;
double skew;
{
	/* Request that the transition for the next motion be given
	   a particular skew value.
	 */

	char *fxnName = "setTransitionSkew()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (skew < 0.0 || skew > 1.0)
	 { errorMsg (ERange, rcclArgRangeMsg, fxnName, 0, 1);
	   RETURN_ERROR(-1);
	 }
	mvb->tauSkew = skew;
}

setCondition (mnp, status)
MANIP *mnp;
int status;
{
	/* Request that the next motion only be executed if the
	   previous motion exited with the indiacted status value.
	 */

	char *fxnName = "setCondition()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (status < 0)
	 { errorMsg (ERange, rcclArgNegMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	mvb->condition = status;
}

massis(mnp, mass)
MANIP *mnp;
double mass;
{
	/* Set the object mass parameter (this feature is currently
	   not used).
	 */

	char *fxnName = "massis()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (mnp->objectFlag)
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }
	mvb->objectLoad = mass * 9.81;
	RETURN_NORMAL (1);
}
