/*-=-=-=-=-=-=-=-=-=-=-=- 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:	getAccel
 *
 * DESCRIPTION:	Routines to read back the desired manipulator acceleration
 *		settings.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

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

float getAccelScale(mnp)
MANIP *mnp;
{
	/* RCCL primitive to read back the acceleration scale factor */

	char *fxnName = "getAccelScale()";

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

	return (mcb->setAccelScale);
}

getCartAccel (mnp, taccel, raccel)
MANIP *mnp;
float *taccel;
float *raccel;
{
	/* RCCL primitive to read back the nominal (non-scaled)
	   Cartesian accelerations */

	char *fxnName = "getCartAccel()";

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

	do
	 { index = mcb->cartAccelIndex;
	   if (taccel)
	    { *taccel = mcb->setTransAccel[index&1];
	    }
	   if (raccel)
	    { *raccel = RADTODEG * mcb->setRotAccel[index&1];
	    }
	 }
	while ((index&~1) != (mcb->cartAccelIndex&~1));
	
}

getJointAccel (mnp, jaccel)
MANIP *mnp;
JNTS *jaccel;
{
	/* RCCL primitive to read back the nominal (non-scaled)
	   joint accelerations */

	char *fxnName = "getJointAccel()";

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

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

 	if (jaccel)
	 { do
	    { index = mcb->jointAccelIndex;
	      jvals = mcb->setJointAccel[index&1].v;
	      for (j=0; j<NUM_JNTS; j++)
	       { if (jls->jtype[j] == 'P')
		  { jaccel->v[j] = jvals[j];
		  }
		 else
		  { jaccel->v[j] = RADTODEG * jvals[j];
		  }
	       }
	    }
	   while ((index&~1) != (mcb->jointAccelIndex&~1));
	 }
	
	return (1);
}

getActiveCartAccel (mnp, taccel, raccel)
MANIP *mnp;
float *taccel;
float *raccel;
{
	/* RCCL primitive to read back the currently active
	   Cartesian accelerations */

	char *fxnName = "getActiveCartAccel()";

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

	register int index;

	do
	 { index = mcb->pathsegIndex;
	   if (taccel)
	    { *taccel = mcb->activeTransAccel[index&1];
	    }
	   if (raccel)
	    { *raccel = RADTODEG * mcb->activeRotAccel[index&1];
	    }
	 }
	while ((index&~1) != (mcb->pathsegIndex&~1));
}

getActiveJointAccel (mnp, jaccel)
MANIP *mnp;
JNTS *jaccel;
{
	/* RCCL primitive to read back the currently active
	   joint accelerations */

	char *fxnName = "getActiveJointAccel()";

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

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

	if (jaccel)
	 { do
	    { index = mcb->pathsegIndex;
	      jvals = mcb->activeJointAccel[index&1].v;
	      for (j=0; j<NUM_JNTS; j++)
	       { if (jls->jtype[j] == 'P')
		  { jaccel->v[j] = jvals[j];
		  }
		 else
		  { jaccel->v[j] = RADTODEG * jvals[j];
		  }
	       }
	    }
	   while ((index&~1) != (mcb->pathsegIndex&~1));
	 }

	return (1);
}
