/*-=-=-=-=-=-=-=-=-=-=-=- 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:	getSpeed
 *
 * DESCRIPTION:	Routines to read back the manipulator velocity and speed
 *		scale settings.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * 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"

float getSpeed(mnp)
MANIP *mnp;
{
	/* RCCL primitive to read back the default speed scale */

	char *fxnName = "getSpeed()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	return (mcb->setSpeed);
}

getCartVel(mnp, tvel, rvel)
MANIP *mnp;
float *tvel, *rvel;
{
	/* RCCL primitive to read back the nominal (non-scaled)
	   Cartesian velocities */

	char *fxnName = "getCartVel()";

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

	do
	 { index = mcb->cartVelIndex;
	   if (tvel)
	    { *tvel = mcb->setTransVel[index&1];
	    }
	   if (rvel)
	    { *rvel = mcb->setRotVel[index&1] * RADTODEG;
	    }
	 }
	while ((index&~1) != (mcb->cartVelIndex&~1));

	return (1);
}

getJointVel(mnp, jvel)
MANIP *mnp;
JNTS *jvel;
{
	/* RCCL primitive to read back the nominal (non-scaled)
	   joint velocities */

	char *fxnName = "getJointVel()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	register float *jvals;
	register int k, index;
	JLS *jls;

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

	if (jvel)
	 { jls = MANIP_ROBOT(mnp)->jls;
	   do
	    { index = mcb->jointVelIndex;
	      jvals = mcb->setJointVel[index&1].v;
	      for (k=0; k<NUM_JNTS; k++)
	       { if (jls->jtype[k] == 'R')
		  { jvel->v[k] = jvals[k] * RADTODEG;
		  }
		 else
		  { jvel->v[k] = jvals[k];
		  }
	       }
	    }
	   while ((index&~1) != (mcb->jointVelIndex&~1));
	 }
	return (1);
}

float getJvelScale (mnp)
MANIP *mnp;
{
	/* RCCL primitive to read back the joint velocity scale */

	char *fxnName = "getJvelScale()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

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

getDefaultCartVel(mnp, tvel, rvel)
MANIP *mnp;
float *tvel, *rvel;
{
	/* RCCL primitive to read back the default Cartesian velocities */

	char *fxnName = "getDefaultCartVel()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	if (tvel)
	 { *tvel = mcb->defaultTransVel;
	 }
	if (rvel)
	 { *rvel = mcb->defaultRotVel * RADTODEG;
	 }
	return (1);
}

getDefaultJointVel(mnp, jvel)
MANIP *mnp;
JNTS *jvel;
{
	/* RCCL primitive to read back the default joint velocities */

	char *fxnName = "getDefaultJointVel()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	JLS *jls;
	int k;

	if (!MANIP_ROBOT(mnp))
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	jls = MANIP_ROBOT(mnp)->jls;
	for (k=0; k<NUM_JNTS; k++)
	 { if (jls->jtype[k] == 'R')
	    { jvel->v[k] = mcb->defaultJointVel.v[k] * RADTODEG;
	    }
	   else
	    { jvel->v[k] = mcb->defaultJointVel.v[k];
	    }
	 }
	return (1);
}

getActiveCartVel(mnp, tvel, rvel)
MANIP *mnp;
float *tvel, *rvel;
{
	/* RCCL primitive to read back the currently active
	   Cartesian velocities */

	char *fxnName = "getActiveCartVel()";

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

	do
	 { index = mcb->pathsegIndex;
	   if (tvel)
	    { *tvel = mcb->activeTransVel[index&1];
	    }
	   if (rvel)
	    { *rvel = mcb->activeRotVel[index&1] * RADTODEG;
	    }
	 }
	while ((index&~1) != (mcb->pathsegIndex&~1));

	return (1);
}

getActiveJointVel(mnp, jvel)
MANIP *mnp;
JNTS *jvel;
{
	/* RCCL primitive to read back the currently active
	   joint velocities */

	char *fxnName = "getActiveJointVel()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	register float *jvals;
	register int k, index;
	JLS *jls;

	if (!MANIP_ROBOT(mnp))
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR(-1);
	 }
	jls = MANIP_ROBOT(mnp)->jls;
	if (jvel)
	 { do
	    { index = mcb->pathsegIndex;
	      jvals = mcb->activeJointVel[index&1].v;
	      for (k=0; k<NUM_JNTS; k++)
	       { if (jls->jtype[k] == 'R')
		  { jvel->v[k] = jvals[k] * RADTODEG;
		  }
		 else
		  { jvel->v[k] = jvals[k];
		  }
	       }
	    }
	   while ((index&~1) != (mcb->pathsegIndex&~1));
	 }
	return (1);
}
