/*-=-=-=-=-=-=-=-=-=-=-=- 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:	solveEqn
 *
 * DESCRIPTION:	Routines used by the trajectory generator for solving
 *		target position equations.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/
/* --------------------------------------------------------------------------

	The routines in this file used by the trajectory generator
	to compute various components of the manipulator position
	equation. 

	The routines are quite simple: they basically just run through
	the requested portion of the position structure, multiplying
	(and possibly inverting) adjacent transforms. 

	The solution is done on the trajectory generator's LOCAL copy of
	the position equation, and the transform values themselves are
	local as well. This is of no consequence if the transform is
	constant (unchanging) but if it is not, then we need to make sure
	the most recent value has been read in from the original (parent)
	transform. Whether or not we need to do this is determined by
	reading a time stamp (for which we use the current system time). 
	This function is performed by the macro UPDATE_TRSF_RENDEZ().

	UPDATE_TRSF_RENDEZ() also has a strange call to setpRendezvous() in 
	it, which I will try to explain. There is a possibility that a
	transform is being modified by a trajectory gernerator residing on
	another CPU. Before updating any transform, we want to make sure
	that we will get the most recent value by waiting until all other
	trajectory generators have finished their calling their transform
	modification functions. This is accomplished by calling
	setpRendezvous(). 

	Functions which check local timestamps and rendezvous are
	suffixed with a capital 'R'. Sometimes we just want to update the 
	value of the transform and we don't care about the timestamp or
	rendezvousing with the  other CPUS. Routines meeting this
	condition use the UPDATE_TRSF() macro, and are suffixed with 
	a capital 'U'.

	-------------------------------------------------------------- */
	
#include "../h/rcclDefs.h"
#include "../rccl/transform.h"

#define UPDATE_TRSF_RENDEZ(t,time) \
	if ((t)->fxn != constTrsf && (t)->timestamp != (time)) \
	 { setpRendezvous(); \
	   readTrans ((TRSF*)((t)->parent), (TRSF*)(t)); \
	   (t)->timestamp = (time); \
	 }

#define UPDATE_TRSF(t) \
	if ((t)->fxn != constTrsf) \
	 { readTrans ((TRSF*)((t)->parent), (TRSF*)(t)); \
	 }

TRSF *solveCoordR (r, pos, timestamp)
register TRSF *r;
register POS *pos;
register int timestamp;
{
	/* 		    
	   Compute R = COORD	(the coord part of the position loop)
	 */

	register POS_TERM *term;
	register TRSF_DESC *t;
	POS_TERM *coordTerm;
	int t6OnLeft;

	t6OnLeft = pos->t6Term->leftSide;
	coordTerm = t6OnLeft ? pos->toolTerm->next : pos->toolTerm->prev;

	if (pos->t6Term == coordTerm)
	 { identTrsf (r);
	   return (r);
	 }
	term = t6OnLeft ? pos->t6Term->prev : pos->t6Term->next;
	t = TRSF_GETDESC(term->trsf);
	UPDATE_TRSF_RENDEZ(t, timestamp);				
	if (term->t6Side)
	 { invertTrsf (r, term->trsf);
	 }
	else
	 { *r = *term->trsf;
	 }
	if (term == coordTerm)
	 { return (r);
	 }
	do
	 { term = t6OnLeft ? term->prev : term->next;
	   t = TRSF_GETDESC(term->trsf);
	   UPDATE_TRSF_RENDEZ(t, timestamp);
	   if (term->t6Side)
	    { multRiTrsf (r, r, term->trsf);
	    }
	   else
	    { multTrsf (r, r, term->trsf);
	    }
	 }
	while (term != coordTerm);
	return (r);
}

TRSF *solveCoordInvR (r, pos, timestamp)
register TRSF *r;
register POS *pos;
register int timestamp;
{
	/* 		    -1
	   Compute R = COORD	(the coord part of the position loop)
	 */

	register POS_TERM *term;
	register TRSF_DESC *t;
	POS_TERM *coordTerm;
	int t6OnLeft;

	t6OnLeft = pos->t6Term->leftSide;
	coordTerm = t6OnLeft ? pos->toolTerm->next : pos->toolTerm->prev;

	if (pos->t6Term == coordTerm)
	 { identTrsf (r);
	   return (r);
	 }
	term = t6OnLeft ? pos->t6Term->prev : pos->t6Term->next;
	t = TRSF_GETDESC(term->trsf);
	UPDATE_TRSF_RENDEZ(t, timestamp);				
	if (!term->t6Side)
	 { invertTrsf (r, term->trsf);
	 }
	else
	 { *r = *term->trsf;
	 }
	if (term == coordTerm)
	 { return (r);
	 }
	do
	 { term = t6OnLeft ? term->prev : term->next;
	   t = TRSF_GETDESC(term->trsf);
	   UPDATE_TRSF_RENDEZ(t, timestamp);
	   if (!term->t6Side)
	    { multLiTrsf (r, term->trsf, r);
	    }
	   else
	    { multTrsf (r, term->trsf, r);
	    }
	 }
	while (term != coordTerm);
	return (r);
}

TRSF *solveToolR (r, pos, timestamp)
register TRSF *r;
register POS *pos;
register int timestamp;
{
	/* 		   
	   Compute R = TOOL	(the tool part of the position loop)
	 */

	register POS_TERM *term;
	register TRSF_DESC *t;
	int t6OnLeft;

	t6OnLeft = pos->t6Term->leftSide;

	if (pos->t6Term == pos->toolTerm)
	 { identTrsf (r);
	   return (r);
	 }
	term = t6OnLeft ? pos->t6Term->next : pos->t6Term->prev;
	t = TRSF_GETDESC(term->trsf);
	UPDATE_TRSF_RENDEZ(t, timestamp);				
	if (term->t6Side)
	 { *r = *term->trsf;
	 }
	else
	 { invertTrsf (r, term->trsf);
	 }
	if (term == pos->toolTerm)
	 { return (r);
	 }
	do
	 { term = t6OnLeft ? term->next : term->prev;
	   t = TRSF_GETDESC(term->trsf);
	   UPDATE_TRSF_RENDEZ(t, timestamp);
	   if (term->t6Side)
	    { multTrsf (r, r, term->trsf);
	    }
	   else
	    { multRiTrsf (r, r, term->trsf);
	    }
	 }
	while (term != pos->toolTerm);
	return (r);
}

TRSF *solveToolInvR (r, pos, timestamp)
register TRSF *r;
register POS *pos;
register int timestamp;
{
	/* 		   -1
	   Compute R = TOOL	(the tool part of the position loop)
	 */

	register POS_TERM *term;
	register TRSF_DESC *t;
	int t6OnLeft;

	t6OnLeft = pos->t6Term->leftSide;

	if (pos->t6Term == pos->toolTerm)
	 { identTrsf (r);
	   return (r);
	 }
	term = t6OnLeft ? pos->t6Term->next : pos->t6Term->prev;
	t = TRSF_GETDESC(term->trsf);
	UPDATE_TRSF_RENDEZ(t, timestamp);				
	if (!term->t6Side)
	 { *r = *term->trsf;
	 }
	else
	 { invertTrsf (r, term->trsf);
	 }
	if (term == pos->toolTerm)
	 { return (r);
	 }
	do
	 { term = t6OnLeft ? term->next : term->prev;
	   t = TRSF_GETDESC(term->trsf);
	   UPDATE_TRSF_RENDEZ(t, timestamp);
	   if (!term->t6Side)
	    { multTrsf (r, term->trsf, r);
	    }
	   else
	    { multLiTrsf (r, term->trsf, r);
	    }
	 }
	while (term != pos->toolTerm);
	return (r);
}

TRSF *solveT6U (t6, pos)
TRSF *t6;
POS *pos;
{
	/*			   -1
	   computes T6 = COORD TOOL   without worrying about timestamps
	   or rendezvousing.
	*/

	POS_TERM *term;
	TRSF_DESC *t;
	int t6OnLeft;

	t6OnLeft = pos->t6Term->leftSide;

	/* equation is garanteed to have at least one other transform */
	term = t6OnLeft ? pos->t6Term->prev : pos->t6Term->next;
	t = TRSF_GETDESC(term->trsf);
	UPDATE_TRSF(t);				
	if (term->t6Side)
	 { invertTrsf (t6, term->trsf);
	 }
	else
	 { *t6 = *term->trsf;
	 }
	for (term = t6OnLeft ? term->prev : term->next; 
	     term != pos->t6Term; 
	     term = t6OnLeft ? term->prev : term->next)
	 { t = TRSF_GETDESC(term->trsf);
	   UPDATE_TRSF(t);
	   if (term->t6Side)
	    { multRiTrsf (t6, t6, term->trsf);
	    }
	   else
	    { multTrsf (t6, t6, term->trsf);
	    }
	 }
	return (t6);
}
