/*-=-=-=-=-=-=-=-=-=-=-=- 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:	jnts.c
 *
 * DESCRIPTION:	This module contains routines for manipulating objects
 *		of type JNTS (a set of robot joint angles).
 * 
 * AUTHOR/DATE:	John Lloyd, Winter 1988
 *
 *--------------------------------------------------------------------------*/

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

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

JNTS *copyJnts (y, j1)
register JNTS *y, *j1;
{
	/* Sort of obsolete given structure assigning compilers ... */

	*y = *j1;
	return (y);
}

JNTS *zeroJnts (jr)
register JNTS *jr;
{
	/* Computes jr = 0 */

#if NUM_JNTS == 6
	jr->v[0] = 0.0; jr->v[3] = 0.0; 
	jr->v[1] = 0.0; jr->v[4] = 0.0; 
	jr->v[2] = 0.0; jr->v[5] = 0.0; 
#else
	register int k;
	for (k=0; k<NUM_JNTS; k++)
	 { jr->v[k] = 0.0;
	 }
#endif
	return (jr);
}

JNTS *addJnts (jr, j1, j2)
register JNTS *jr, *j1, *j2;
{
	/* Computes jr = j1 + j2 */

#if NUM_JNTS == 6
	jr->v[0] = j1->v[0] + j2->v[0];
	jr->v[1] = j1->v[1] + j2->v[1];
	jr->v[2] = j1->v[2] + j2->v[2];
	jr->v[3] = j1->v[3] + j2->v[3];
	jr->v[4] = j1->v[4] + j2->v[4];
	jr->v[5] = j1->v[5] + j2->v[5];
#else
	register int k;
	for (k=0; k<NUM_JNTS; k++)
	 { jr->v[k] = j1->v[k] + j2->v[k];
	 }
#endif
	return (jr);
}

JNTS *subtractJnts (jr, j1, j2)
register JNTS *jr, *j1, *j2;
{
	/* Computes jr = j1 - j2 */

#if NUM_JNTS == 6
	jr->v[0] = j1->v[0] - j2->v[0];
	jr->v[1] = j1->v[1] - j2->v[1];
	jr->v[2] = j1->v[2] - j2->v[2];
	jr->v[3] = j1->v[3] - j2->v[3];
	jr->v[4] = j1->v[4] - j2->v[4];
	jr->v[5] = j1->v[5] - j2->v[5];
#else
	register int k;
	for (k=0; k<NUM_JNTS; k++)
	 { jr->v[k] = j1->v[k] - j2->v[k];
	 }
#endif
	return (jr);
}

JNTS *scaleJnts (jr, a1, j1)
register JNTS *jr, *j1;
double a1;
{
	/* Computes jr = a1 j1  */

#if NUM_JNTS == 6
	jr->v[0] = a1 * j1->v[0];
	jr->v[1] = a1 * j1->v[1];
	jr->v[2] = a1 * j1->v[2];
	jr->v[3] = a1 * j1->v[3];
	jr->v[4] = a1 * j1->v[4];
	jr->v[5] = a1 * j1->v[5];
#else
	register int k;
	for (k=0; k<NUM_JNTS; k++)
	 { jr->v[k] = a1 * j1->v[k];
	 }
#endif
	return (jr);
}

JNTS *combineJnts (jr, a1, j1, a2, j2)
register JNTS *jr, *j1, *j2;
double a1, a2;
{
	/* Computes jr = a1 j1 + a2 j2  */

#if NUM_JNTS == 6
	jr->v[0] = a1 * j1->v[0] + a2 * j2->v[0];
	jr->v[1] = a1 * j1->v[1] + a2 * j2->v[1];
	jr->v[2] = a1 * j1->v[2] + a2 * j2->v[2];
	jr->v[3] = a1 * j1->v[3] + a2 * j2->v[3];
	jr->v[4] = a1 * j1->v[4] + a2 * j2->v[4];
	jr->v[5] = a1 * j1->v[5] + a2 * j2->v[5];
#else
	register int k;
	for (k=0; k<NUM_JNTS; k++)
	 { jr->v[k] = a1 * j1->v[k] + a2 * j2->v[k];
	 }
#endif
	return (jr);
}

JNTS *extrapJnts (jr, j1, j2, s)
register JNTS *jr, *j1, *j2;
register double s;
{
	/* Computes jr = j1 + s j2 
	   
	   A ``special request'' function for the trajectory generator.
	*/

#if NUM_JNTS == 6
	jr->v[0] = j1->v[0] + s * j2->v[0];
	jr->v[1] = j1->v[1] + s * j2->v[1];
	jr->v[2] = j1->v[2] + s * j2->v[2];
	jr->v[3] = j1->v[3] + s * j2->v[3];
	jr->v[4] = j1->v[4] + s * j2->v[4];
	jr->v[5] = j1->v[5] + s * j2->v[5];
#else
	register int k;
	for (k=0; k<NUM_JNTS; k++)
	 { jr->v[k] = j1->v[k] + s * j2->v[k];
	 }
#endif
	return (jr);
}

JNTS *degreeToJnts (jr, deg)
JNTS *jr;
register float *deg;
{
	/* Computes jr from an input set in degrees or millimeters  */

	register float *v;
	register char *type;

	v = jr->v;
	type = jr->type;

#if NUM_JNTS == 6
	v[0] = (type[0]=='R' ? DEGTORAD : 1.0) * deg[0];
	v[1] = (type[1]=='R' ? DEGTORAD : 1.0) * deg[1];
	v[2] = (type[2]=='R' ? DEGTORAD : 1.0) * deg[2];
	v[3] = (type[3]=='R' ? DEGTORAD : 1.0) * deg[3];
	v[4] = (type[4]=='R' ? DEGTORAD : 1.0) * deg[4];
	v[5] = (type[5]=='R' ? DEGTORAD : 1.0) * deg[5];
#else
	register int k;
	for (k=0; k<jr->num; k++)
	 { v[k] = (type[k]=='R' ? DEGTORAD : 1.0) * deg[k];
	 }
#endif
	return (jr);
}

float *jntsToDegree (deg, j1)
register float *deg;
JNTS *j1;
{
	/* Computes jr from an input set in degrees or millimeters  */

	register float *v;
	register char *type;

	v = j1->v;
	type = j1->type;

#if NUM_JNTS == 6
	deg[0] = (type[0]=='R' ? RADTODEG : 1.0) * v[0];
	deg[1] = (type[1]=='R' ? RADTODEG : 1.0) * v[1];
	deg[2] = (type[2]=='R' ? RADTODEG : 1.0) * v[2];
	deg[3] = (type[3]=='R' ? RADTODEG : 1.0) * v[3];
	deg[4] = (type[4]=='R' ? RADTODEG : 1.0) * v[4];
	deg[5] = (type[5]=='R' ? RADTODEG : 1.0) * v[5];
#else
	register int k;
	for (k=0; k<j1->num; k++)
	 { deg[k] = (type[k]=='R' ? RADTODEG : 1.0) * v[k];
	 }
#endif
	return (deg);
}

JNTS *initJntsByName (jr, name)
JNTS *jr;
const char *name;
{
	char *fxnName = "initJntsByName()";
	JLS *jls;
	
	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	MASK_RCI_AUTO_ABORT();
	if ((jls = getJls(name)) == NULL)
	 { if (getErrorCode(0) != EMemory &&
	       getErrorCode(0) != ENotDefined)
	    { errorMsg (EInternal, rcclNullMsg, fxnName);
	    }
	   else
	    { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	    }
	   RETURN_ERROR(NULL);
	 }
	else
	 { jr->num = jls->numJ;
	   strcpy (jr->type, jls->jtype);
	   RETURN_NORMAL(jr);
	 }
}

JNTS *initJntsByManip (jr, mnp)
JNTS *jr;
MANIP *mnp;
{
	char *fxnName = "initJntsByManip()";
	RCI_RBT *rbt = NULL;

	if (mnp)
	 { rbt = MANIP_ROBOT(mnp);
	 }
	if (!mnp || !rbt)
	 { errorMsg (ENotDefined, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR(NULL);
	 }
	else
	 { jr->num = rbt->jls->numJ;
	   strcpy (jr->type, rbt->jls->jtype);
	   RETURN_NORMAL(jr);
	 }
}
