/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 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:	setDistance
 *
 * DESCRIPTION:	This file contains the RCCL primitives 'distance()' and
 *		'setDistance()', which allow a programmer to specify
 *		a temporary offset to target of the next motion.
 * 
 * AUTHOR/DATE:	Vincent Hayward, Purdue University, 1983, and
 *		John Lloyd, McGill University, Fall 1988.
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <varargs.h>

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

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

distance(va_alist)
va_dcl
{
	/* Accepts a request for target offset presented as a format
	   string and a variable length argument list describing
	   delta positions and yaw-pitch-roll angles.
	*/

	MANIP *mnp;
	char *c;
	va_list ap;

	char *fxnName = "distance()";
	MOTION_VALUE_BLOCK *mvb;
	TRSF diffTrans;
	DIFF shift3D;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	va_start (ap);
	mnp = va_arg (ap, MANIP*);
	c = va_arg (ap, char*);

	mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;

	/* Parse the input string for the directions of the changes */

	bzero (&shift3D, sizeof(DIFF));

	while (*c)
	 { switch (*c++)
	    { case 'd' :
	       { switch (*c++)
		  { case 'x' :
		     { shift3D.t.x = va_arg(ap,double);
		       break;
		     }
		    case 'y' :
		     { shift3D.t.y = va_arg(ap,double);
		       break;
		     }
		    case 'z' :
		     { shift3D.t.z = va_arg(ap,double);
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case 'r' :
	       { switch (*c++)
		  { case 'x' :
		     { shift3D.r.x = va_arg(ap,double);
		       break;
		     }
		    case 'y' :
		     { shift3D.r.y = va_arg(ap,double);
		       break;
		     }
		    case 'z' :
		     { shift3D.r.z = va_arg(ap,double);
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case ' ' :
	       { break;
	       }
	      default :
	       { goto bad;
	       }
	    }
	 }

	if (shift3D.t.x != 0. || shift3D.t.y != 0. || shift3D.t.z != 0. ||
	    shift3D.r.z != 0. || shift3D.r.y != 0. || shift3D.r.x != 0.)
	 { 
	   xyzToTrsf (&diffTrans, shift3D.t.x, shift3D.t.y, shift3D.t.z);
	   rpyToTrsf  (&diffTrans, shift3D.r.z, shift3D.r.y, shift3D.r.x);
	   mvb->newShiftTransform = diffTrans;
	   mvb->newShiftSet = YES;
	 }
	va_end(ap);
	RETURN_NORMAL(1);
bad :
	errorMsg (ESyntax, rcclBadSpecStrMsg, fxnName);
	va_end(ap);
	RETURN_ERROR(-1);
}

setDistance(mnp, tr)
MANIP *mnp;
TRSF *tr;
{
	/* Accepts a request for target offset presented as 
	   a 4X4 transform.
	*/

	char *fxnName = "setDistance()";
	MOTION_VALUE_BLOCK *mvb;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	mvb->newShiftTransform = *tr;
	mvb->newShiftSet = YES;

	RETURN_NORMAL(1);
}
