/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	setAccel
 *
 * DESCRIPTION:	Routines to set the desired acceleration factors for the
 *		manipulator. The effect of this routines is noticed at the
 *		beginning of the next motion request.
 *
 * 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"

#define EPSILON 0.0

float setAccelScale(mnp, s)
MANIP *mnp;
double s;
{
	/* RCCL primitive to set the acceleration scale factor. */

	char *fxnName = "setAccelScale()";

	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->setAccelScale;
	mcb->setAccelScale = s;

	/* Recomputethe actual acceleration factors that will be 
	   downloaded with the next motion request. */

	idx = mcb->cartAccelIndex&1;
	scale = mcb->setAccelScale;
	mvb->transAccel = mcb->setTransAccel[idx] * scale;
	mvb->rotAccel   = mcb->setRotAccel[idx] * scale;

	idx = mcb->jointAccelIndex&1;
	scale *= mcb->setJointAccelScale;
	scaleVf(mvb->jointAccel.v, scale, mcb->setJointAccel[idx].v, 6);

	RETURN_NORMAL(olds);
}

setCartAccel (mnp, taccel, raccel)
MANIP *mnp;
double taccel;
double raccel;
{
	/* RCCL primitive to set the nominal Cartesian accelerations. */

	char *fxnName = "setCartAccel()";

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

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

	if ((taccel <= EPSILON && taccel != F_UNDEF && taccel != F_DEFAULT) ||
	    (raccel <= EPSILON && raccel != F_UNDEF && raccel != F_DEFAULT))
	 { errorMsg (ERange, rcclArgLeqMsg, fxnName, EPSILON);
	   RETURN_ERROR (-1);
	 }
	if (taccel != F_UNDEF)
	 { if (taccel == F_DEFAULT)
	    { mcb->setTransAccel[new] = mcb->defaultTransAccel;
	    }
	   else
	    { mcb->setTransAccel[new] = taccel;
	    }
	 }
	else
	 { mcb->setTransAccel[new] = mcb->setTransAccel[old];
	 }
	if (raccel != F_UNDEF)
	 { if (raccel == F_DEFAULT)
	    { mcb->setRotAccel[new] = mcb->defaultRotAccel;
	    }
	   else
	    { mcb->setRotAccel[new] = DEGTORAD * raccel;
	    }
	 }
	else
	 { mcb->setRotAccel[new] = mcb->setRotAccel[old];
	 }
	mcb->cartAccelIndex++;
	
	/* Compute the actual Cartesian accelerations that will be sent down
	   with the next motion request.
	 */

	scale = mcb->setAccelScale;
	mvb->transAccel = mcb->setTransAccel[new] * scale;
	mvb->rotAccel = mcb->setRotAccel[new] * scale;

	RETURN_NORMAL (1);
}

setJointAccel (mnp, jaccel)
MANIP *mnp;
JNTS *jaccel;
{
	/* RCCL primitive to set the nominal joint accelerations. */

	char *fxnName = "setJointAccel()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK *)mnp->mcb;
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK*)mnp->mvb;
	JLS *jls;
	JNTS newacc;
	float scale;
	int new, old, j;

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

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

	if (jaccel == NULL)
	 { RETURN_NORMAL(1);
	 }
	newacc = mcb->setJointAccel[old];
	for (j=0; j<NUM_JNTS; j++)
	 { if (jaccel->v[j] <= EPSILON && jaccel->v[j] != F_UNDEF
				       && jaccel->v[j] != F_DEFAULT)
	    { errorMsg (ERange, rcclArgLeqMsg, fxnName, EPSILON);
	      RETURN_ERROR (-1);
	    }
	   if (jaccel->v[j] != F_UNDEF)
	    { if (jaccel->v[j] == F_DEFAULT)
	       { newacc.v[j] = jls->nominalAcc[j];
	       }
	      else
	       { if (jls->jtype[j] == 'P')
		  { newacc.v[j] = jaccel->v[j];
		  }
		 else
		  { newacc.v[j] = DEGTORAD * jaccel->v[j];
		  }
	       }
	    }
	 }
	mcb->setJointAccel[new] = newacc;
	mcb->jointAccelIndex++;

	/* Compute the actual joint accelerations that will be sent down
	   with the next motion request.
	 */

	scale = mcb->setAccelScale * mcb->setJointAccelScale;
	scaleVf (mvb->jointAccel.v, scale, mcb->setJointAccel[new].v, 6);

	RETURN_NORMAL (1);
}
