/*-=-=-=-=-=-=-=-=-=-=-=- 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:	setSpeed
 *
 * DESCRIPTION:	Routines to control the desired manipulator velocity.
 *		The specifications take effect at the start
 *		of the next requested motion.
 * 
 * AUTHOR/DATE:	John Lloyd
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

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

/* global speed scaling factor, read in from .rciparams file */
extern float rcclDefaultSpeed;

#define EPSILON 0.0

float setSpeed(mnp, s)
MANIP *mnp;
double s;
{
	/* RCCL primitive to set the speed scaling factor */

	char *fxnName = "setSpeed()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK *)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	float olds;
	float scale;
	int idx;

	if (!planningLevelCheck(fxnName))
	 { return (-1.0);
	 }
	if (s <= EPSILON)
	 { errorMsg (ERange, rcclArgLeqMsg, fxnName, EPSILON);
	   RETURN_ERROR(-1.0);
	 }
	olds = mcb->setSpeed;
	mcb->setSpeed = s;

	/* Recompute the velocities that will be sent down with the
	   next motion request */

	idx = mcb->cartVelIndex&1;
	scale = rcclDefaultSpeed * mcb->setSpeed;
	mvb->transVel = mcb->setTransVel[idx] * scale;
	mvb->rotVel   = mcb->setRotVel[idx] * scale;

	idx = mcb->jointVelIndex&1;
	scale *= mcb->setJointSpeed;
	scaleVf(mvb->jointVel.v, scale, mcb->setJointVel[idx].v, 6);

	RETURN_NORMAL(olds);
}

setCartVel(mnp, tvel, rvel)
MANIP *mnp;
double tvel, rvel;
{
	/* RCCL primitive to set the nominal Cartesian velocities */

	char *fxnName = "setCartVel()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK *)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	float scale;
	int new, old;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	old = mcb->cartVelIndex&1;
	new = old^1;

	mcb->setTransVel[new] = mcb->setTransVel[old];
	mcb->setRotVel[new] = mcb->setRotVel[old];

	if (tvel == F_DEFAULT)
	 { mcb->setTransVel[new] = mcb->defaultTransVel;
	 }
	else if (tvel == F_UNDEF)
	 { /* do nothing */
	 }
	else if (tvel <= EPSILON)
	 { errorMsg (ERange, rcclArgLeqMsg, fxnName, EPSILON);
	   RETURN_ERROR(-1);
	 }
	else
	 { mcb->setTransVel[new] = tvel;
	 }
	if (rvel == F_DEFAULT)
	 { mcb->setRotVel[new] = mcb->defaultRotVel;
	 }
	else if (rvel == F_UNDEF)
	 { /* do nothing */
	 }
	else if (rvel <= EPSILON)
	 { errorMsg (ERange, rcclArgLeqMsg, fxnName, EPSILON);
	   RETURN_ERROR(-1);
	 }
	else
	 { mcb->setRotVel[new] = rvel * DEGTORAD;
	 }

	mcb->cartVelIndex++;

	/* Recompute the actual Cartesian velocities that will be sent 
	   down with the next motion request */

	scale = mcb->setSpeed * rcclDefaultSpeed;
	mvb->transVel = mcb->setTransVel[new] * scale;
	mvb->rotVel   = mcb->setRotVel[new] * scale;

	RETURN_NORMAL(1);
}

setJointVel(mnp, jvel)
MANIP *mnp;
JNTS *jvel;
{
	/* RCCL primitive to set the nominal joint velocities */

	char *fxnName = "setJointVel()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK *)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	JNTS newval;
	JLS *jls;
	float scale;
	int new, old, k;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	old = mcb->jointVelIndex&1;
	new = old^1;

	if (!MANIP_ROBOT(mnp))
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR(-1);
	 }

	jls = MANIP_ROBOT(mnp)->jls;

	if (!jvel)
	 { mcb->setJointVel[new] = mcb->defaultJointVel;
	 }
	else
	 { newval = mcb->setJointVel[old];
	   for (k=0; k<NUM_JNTS; k++)
	    { if (jvel->v[k] == F_DEFAULT)
	       { newval.v[k] = mcb->defaultJointVel.v[k];
	       }
	      else if (jvel->v[k] == F_UNDEF)
	       { /* do nothing */
	       }
	      else if (jvel->v[k] > EPSILON)
	       { if (jls->jtype[k] == 'R')
		  { newval.v[k] = DEGTORAD * jvel->v[k];
		  }
		 else
		  { newval.v[k] = jvel->v[k];
		  }
	       }
	      else
	       { errorMsg (ERange, rcclArgLeqMsg, fxnName, EPSILON);
		 RETURN_ERROR(-1);
	       }
	    }
	 }

	mcb->setJointVel[new] = newval;
	mcb->jointVelIndex++;

	/* Recompute the actual joint velocities that will be sent 
	   down with the next motion request */

	scale = mcb->setSpeed * rcclDefaultSpeed * mcb->setJointSpeed;
	scaleVf(mvb->jointVel.v, scale, mcb->setJointVel[new].v, 6);

	RETURN_NORMAL(1);
}

float setJvelScale(mnp, s)
MANIP *mnp;
double s;
{
	/* RCCL primitive to set the joint velocity scale */

	char *fxnName = "getJvelScale()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK *)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	float olds;
	float scale;
	int idx;

	if (!planningLevelCheck(fxnName))
	 { return (-1.0);
	 }
	if (!MANIP_ROBOT(mnp))
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR(-1.0);
	 }

	if (s <= EPSILON)
	 { errorMsg (ERange, rcclArgLeqMsg, fxnName, EPSILON);
	   RETURN_ERROR(-1.0);
	 }
	olds = mcb->setJointSpeed;
	mcb->setJointSpeed = s;

	/* Recompute the actual joint velocities that will be sent 
	   down with the next motion request */

	idx = mcb->jointVelIndex&1;
	scale = mcb->setSpeed * rcclDefaultSpeed * mcb->setJointSpeed;
	scaleVf(mvb->jointVel.v, scale, mcb->setJointVel[idx].v, 6);

	RETURN_NORMAL(olds);
}
