/*-=-=-=-=-=-=-=-=-=-=-=- 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:	setComply
 *
 * DESCRIPTION:	This file contains the RCCL primitives which set
 *		the compliance fields.
 *
 * AUTHOR/DATE:	John Lloyd, McGill University, 1989
 *		after Vincent Hayward, Purdue University.
 *
 * 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

comply(va_alist)
va_dcl
{
	/* Set motion compliance specification using a format string
	   and a variable length argument list */

	MANIP *mnp;
	char *c;
	double a;
	va_list ap;	

	char *fxnName = "comply()";
	MOTION_CONTROL_BLOCK *mcb;
	int cpySel;
	FORCE cpyForce;
	int new, old;

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

	old = mcb->complyIndex&1;
	new = old^1;
 
	c = va_arg(ap, char*);

	cpySel = mcb->setComplyMask[old];
	cpyForce = mcb->setComplyValues[old];

	while (*c)
	 { /* for each character in the string ... */

	   switch (*c++)
	    { case 'f' :
	       { switch (*c++)
		  { case 'x' :
		     { cpySel |= ALONG_X; /* Set flag and value */
		       cpyForce.f.x = va_arg(ap,double);
		       break;
		     }
		    case 'y' :
		     { cpySel |= ALONG_Y;
		       cpyForce.f.y = va_arg(ap,double);
		       break;
		     }
		    case 'z' :
		     { cpySel |= ALONG_Z;
		       cpyForce.f.z = va_arg(ap,double);
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case 't' :
	       { switch (*c++)
		  { case 'x' :
		     { cpySel |= ABOUT_X;
		       cpyForce.m.x = va_arg(ap,double) * NmToNmm;
		       break;
		     }
		    case 'y' :
		     { cpySel |= ABOUT_Y;
		       cpyForce.m.y = va_arg(ap,double) * NmToNmm;
		       break;
		     }
		    case 'z' :
		     { cpySel |= ABOUT_Z;
		       cpyForce.m.z = va_arg(ap,double) * NmToNmm;
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case ' ' :
	       { break;
	       }
	      default :
	       { goto bad;
		 break;
	       }
	    }
	 }
	mcb->setComplyMask[new] = cpySel;
	mcb->setComplyValues[new] = cpyForce;
	mcb->complyIndex++;
	va_end(ap);
	RETURN_NORMAL (1);
bad :
	errorMsg (ESyntax, rcclBadSpecStrMsg, fxnName);
	va_end(ap);
	RETURN_ERROR(-1);
}

lock (mnp, c)
MANIP *mnp;
const char *c;
{
	/* unset (lock) the compliance specification using a format
	   string */

	char *fxnName = "lock()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK *)mnp->mcb;

	int cpySel;
	FORCE cpyForce;
	int new, old;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	old = mcb->complyIndex&1;
	new = old^1;
 
	cpySel = mcb->setComplyMask[old];
	cpyForce = mcb->setComplyValues[old];

	while (*c)	/* For each character in the string ... */
	 {
	   switch (*c++)
	    { case 'f' :
	       { switch (*c++)
		  { case 'x' :
		     { if (cpySel & ALONG_X)
			{ cpySel &= ~ALONG_X; /* Clear flag and value */
			  cpyForce.f.x = 0.;
			}
		       break;
		     }
		    case 'y' :
		     { if (cpySel & ALONG_Y)
			{ cpySel &= ~ALONG_Y;
			  cpyForce.f.y = 0.;
			}
		       break;
		     }
		    case 'z' :
		     { if (cpySel & ALONG_Z)
			{ cpySel &= ~ALONG_Z;
			  cpyForce.f.z = 0.;
			}
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case 't' :
	       { switch (*c++)
		  { case 'x' :
		     { if (cpySel & ABOUT_X)
			{ cpySel &= ~ABOUT_X; /* Clear flag and value */
			  cpyForce.m.x = 0.;
			}
		       break;
		     }
		    case 'y' :
		     { if (cpySel & ABOUT_Y)
			{ cpySel &= ~ABOUT_Y;
			  cpyForce.m.y = 0.;
			}
		       break;
		     }
		    case 'z' :
		     { if (cpySel & ABOUT_Z)
			{ cpySel &= ~ABOUT_Z;
			  cpyForce.m.z = 0.;
			}
		       break;
		     }
		    default :
		     { goto bad;
		       break;
		     }
		  }
		 break;
	       }
	      case ' ' :
	       { break;
	       }
	      default :
	       { goto bad;
		 break;
	       }
	    }
	 }
	mcb->setComplyMask[new] = cpySel;
	mcb->setComplyValues[new] = cpyForce;
	mcb->complyIndex++;
	RETURN_NORMAL(1);
bad :
	errorMsg (ESyntax, rcclBadSpecStrMsg, fxnName);
	RETURN_ERROR(-1);
}

long setComply (mnp, mask, force)
MANIP *mnp;
unsigned long mask;
FORCE *force;
{
	/* set the maniplutor compliance specification using a mask
	   and a value vector.
	 */

	char *fxnName = "setComply()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	unsigned long oldmask;
	int old, new;

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

	if (!force && mask)
	 { errorMsg (EBadArg, rcclArgNullMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	oldmask = mcb->setComplyMask[old];
	if (mcb->setComplyMask[new] = mask)
	 { mcb->setComplyValues[new] = *force;
	   mcb->setComplyValues[new].m.x *= NmToNmm;
	   mcb->setComplyValues[new].m.y *= NmToNmm;
	   mcb->setComplyValues[new].m.z *= NmToNmm;
	 }
	mcb->complyIndex++;
	RETURN_NORMAL (oldmask);
}
