/*-=-=-=-=-=-=-=-=-=-=-=- 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:	setLimit
 *
 * DESCRIPTION:	This file contains the RCCL primitives which set
 *		the motion limit fields.
 *
 * AUTHOR/DATE:	Vincent Hayward, Purdue University, 1983
 *		CVaRL installation by John Lloyd.
 *
 * 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"

#define NmToNmm         1000.0

FORCE zeroforce = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
DIFF zerodiff   = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };

limit(va_alist)
va_dcl
{
	/* Set force and differential motion limits using a format
	   string and a variable length argument list */

	MANIP *mnp;
	char *c;
	va_list ap;

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

	int diffSelect = 0;
	int forceSelect = 0;
	FORCE forceValues;
	DIFF diffValues;
	
	forceValues = zeroforce;
	diffValues = zerodiff;

	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 to recognize limit specifiers */

	while (*c) 		/* for each character in the string ... */
	 {
	   switch (*c++)
	    { case 'f' :
	       { switch (*c++)
		  { case 'x' :
		     { forceSelect |= ALONG_X; /* Set flag and value */
		       forceValues.f.x = va_arg(ap,double);
		       break;
		     }
		    case 'y' :
		     { forceSelect |= ALONG_Y;
		       forceValues.f.y = va_arg(ap,double);
		       break;
		     }
		    case 'z' :
		     { forceSelect |= ALONG_Z;
		       forceValues.f.z = va_arg(ap,double);
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case 't' :
	       { switch (*c++)
		  { case 'x' :
		     { forceSelect |= ABOUT_X;
		       forceValues.m.x = va_arg(ap,double) * NmToNmm;
		       break;
		     }
		    case 'y' :
		     { forceSelect |= ABOUT_Y;
		       forceValues.m.y = va_arg(ap,double) * NmToNmm;
		       break;
		     }
		    case 'z' :
		     { forceSelect |= ABOUT_Z;
		       forceValues.m.z = va_arg(ap,double) * NmToNmm;
		       break;
		     }
		    default :
				goto bad;
		  }
		 break;
	       }
	      case 'd' :
	       { switch (*c++)
		  { case 'x' :
		     { diffSelect |= ALONG_X;
		       diffValues.t.x = va_arg(ap,double);
		       break;
		     }
		    case 'y' :
		     { diffSelect |= ALONG_Y;
		       diffValues.t.y = va_arg(ap,double);
		       break;
		     }
		    case 'z' :
		     { diffSelect |= ALONG_Z;
		       diffValues.t.z = va_arg(ap,double);
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case 'r' :
	       { switch (*c++)
		  { case 'x' :
		     { diffSelect |= ABOUT_X;
		       diffValues.r.x = va_arg(ap,double) * DEGTORAD;
		       break;
		     }
		    case 'y' :
		     { diffSelect |= ABOUT_Y;
		       diffValues.r.y = va_arg(ap,double) * DEGTORAD;
		       break;
		     }
		    case 'z' :
		     { diffSelect |= ABOUT_Z;
		       diffValues.r.z = va_arg(ap,double) * DEGTORAD;
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case ' ' :
	       { break;
	       }
	      default :
	       { goto bad;
		 break;
	       }
	    }
	 }
	if ((mvb->diffLimitMask = diffSelect) != 0)
	 { mvb->diffLimitValues = diffValues;
	 }
	if ((mvb->forceLimitMask = forceSelect) != 0)
	 { mvb->forceLimitValues = forceValues;
	 }
	va_end(ap);
	RETURN_NORMAL (1);
bad :
	errorMsg (ESyntax, rcclBadSpecStrMsg, fxnName);
	va_end(ap);
	RETURN_ERROR(-1);
}

setForceLimit(mnp, mask, force)
MANIP *mnp;
unsigned long mask;
FORCE *force;
{
	/* Set force limits using a mask and a value vector */

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

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	if ((mvb->forceLimitMask = mask) != 0)
	 { mvb->forceLimitValues = *force;
	   mvb->forceLimitValues.m.x *= NmToNmm;
	   mvb->forceLimitValues.m.y *= NmToNmm;
	   mvb->forceLimitValues.m.z *= NmToNmm;
	 }
	RETURN_NORMAL (1);
}

setDispLimit(mnp, mask, diff)
MANIP *mnp;
unsigned long mask;
DIFF *diff;
{
	/* Set differential motion limits using a mask and a value vector */

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

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	if ((mvb->diffLimitMask = mask) != 0)
	 { mvb->diffLimitValues = *diff;
	   mvb->diffLimitValues.r.x *= DEGTORAD;
	   mvb->diffLimitValues.r.y *= DEGTORAD;
	   mvb->diffLimitValues.r.z *= DEGTORAD;
	 }
	RETURN_NORMAL (1);
}
