/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 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:	updateTrans
 *
 * DESCRIPTION:	The routines in this file are sort of companions to the 
 *		'solveTrans()' routines: they request that the trajectory
 *		generator solve a position equation at the end
 *		of the next requested motion.
 *
 *   		At the moment, we have only implemented the analog to
 *		solveTrans().
 *
 *		For documentation on how position equations are solved,
 *		see the notes in solveTrans.c.
 *
 * AUTHOR/DATE:	Spring, 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

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

updateTrans(mnp, tr, pos, t1, tx) /*::*/
MANIP *mnp;
TRSF *tr, *t1, *tx;
POS *pos;
{
	/* RCCL primitive to request that the trajectory generator
	   solve 'pos' for transform 't1' at the end of the next requested 
	   motion for 'mnp'. The result is stored in 'tr', and 'tx' is 
	   used for the value of T6, if T6 is not the transform
	   being solved for.
	 */

	char *fxnName = "updateTrans()";
	MOTION_VALUE_BLOCK *mvb = (MOTION_VALUE_BLOCK *)mnp->mvb;
	POS_TERM *term;
	POS_TERM *solve = NULL;
	int numTerms;
	int k;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();

	mvb->updatePos = NULL;

	/* Do a bunch of argument checking */
	
	if (TRSF_GETDESC(tr)->magic != TRSF_MAGIC)
	 { errorMsg (EBadArg, rcclBadTrsfMsg, fxnName, tr);
	   RETURN_ERROR(-1);
	 }
	/* Locate the term in 'pos' corresponding to the transform 't1' */

	term = pos->startTerm;
	do
	 { if (term == pos->t6Term)
	    { if (t1 == T6 || t1 == TRANS)
	       { solve = term;
		 break;
	       }
	    }
	   else if (term->trsf == t1)
	    { solve = term;
	      break;
	    }
	   term = term->prev;
	 }
	while (term != pos->startTerm);

	if (!solve)
	 { errorMsg (ERange, rcclNoTrsfMsg, fxnName, t1);
	   RETURN_ERROR(-1);
	 }
	if (solve != pos->t6Term && pos->t6Term != NULL)
	 { if (tx == NULL || TRSF_GETDESC(tx)->magic != TRSF_MAGIC)
	    { errorMsg (EBadArg, rcclBadTrsfMsg, fxnName, tx);
	      RETURN_ERROR(-1);
	    }
	 }
	mvb->updatePos = pos;
	mvb->updateTr = tr;
	mvb->updateT1 = t1;
	mvb->updateTx = tx;
	RETURN_NORMAL(1);
}

