/*-=-=-=-=-=-=-=-=-=-=-=- 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:	solveTrans
 *
 * DESCRIPTION:	Routines to solve various components of a position equation.
 *		
 *		Position equations are represented as circular linked lists
 *		of transform terms. In evaluating portions of them, we simply
 *		step along this list and multiply together adjacent
 *		transforms. Depending on where the transform occurs in the
 *	 	defining equation, it is sometimes necessary to multiply by
 *		the inverse of the transform instead of the forward value.
 *
 *		I will illustrate this with an example. Suppose I have
 *		the position equation
 *
 *				A T6 B = C D E
 *
 *		This will be represented by the following circular list
 *		structure:
 *
 *		        +--> A <------> T6 <------> B <--+
 *			|				 |
 *			|				 |
 *			+--> C <------> D <-------> E <--+
 *
 *		Now suppose we have to solve for D. This is amounts
 *		to evaluating
 *				      -1            -1
 *			       D  =  C   A  T6  B  E
 *
 *		which can be computed by starting at C and going around
 *		the list clockwise to E, multiplying together each 
 *		transform's value. If the transform is on the same side of
 *		the defining equation as D, we need to multiply by the inverse
 *		value; otherwise, we use the forward value.
 *
 * AUTHOR/DATE:	John Lloyd, Spring 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

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

static dosolve();
static doinvsolve();

TRSF *solveTrans (tr, pos, t1, tx)
TRSF *tr, *t1, *tx;
POS *pos;
{
	/* Solve for the transform 't1' inside the position equation
	   'pos', using 'tx' as the value of the T6 transform (unless
	   T6 is what we are solving for, in which case we don't need 
	   'tx').
	 */

	char *fxnName = "solveTrans()";
	POS_TERM *term;
	POS_TERM *solve = NULL;

	/* Go through all the terms in the position equation and pick
	   out the one corresponding to '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(NULL);
	 }
	if (solve != pos->t6Term && pos->t6Term != NULL)
	 { if (tx == NULL)
	    { errorMsg (EBadArg, rcclBadTrsfMsg, fxnName, tx);
	      RETURN_ERROR(NULL);
	    }
	 }

	/* Solve for either this term or its inverse, depending on the
	   relative direction of 't1' in the equation. */

	if (solve->leftSide)
	 { doinvsolve (tr, pos, solve->prev, solve->next, tx);
	 }
	else
	 { dosolve (tr, pos, solve->next, solve->prev, tx);
	 }
	
	RETURN_NORMAL(tr);
}

TRSF *solveChain (tr, pos, t1, t2, tx)
TRSF *tr, *t1, *t2, *tx;
POS *pos;
{
	/* Solve for the portion (chain) of the position equation 'pos' 
	   delimited by the transforms 't1' and 't2'. Go around the
	   loop clockwise. If the T6 transform is part of the chain, use the
	   transform 'tx' to denote it's value. 
	 */	   

	char *fxnName = "solveChain()";
	POS_TERM *term;
	POS_TERM *start = NULL;
	POS_TERM *stop = NULL;
	int t6Included = 0;

	/* pick out the equation term corresponding to the start 
	   transform 't1' */

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

	if (!start)
	 { errorMsg (ERange, rcclNoTrsfMsg, fxnName, t1);
	   RETURN_ERROR(NULL);
	 }

	/* pick out the equation term corresponding to the stop
	   transform 't2' */

	term = start;
	do
	 { if (term == pos->t6Term)
	    { t6Included = 1;
	      if (t2 == T6 || t2 == TRANS)
	       { stop = term;
		 break;
	       }
	    }
	   else if (term->trsf == t2)
	    { stop = term;
	      break;
	    }
	   term = term->next;
	 }
	while (term != start);

	if (!stop)
	 { errorMsg (ERange, rcclNoTrsfMsg, fxnName, t2);
	   RETURN_ERROR(NULL);
	 }

	/* Get a value for T6 if it is part of the chain to be solved */

	if (t6Included)
	 { if (tx == NULL)
	    { errorMsg (EBadArg, rcclBadTrsfMsg, fxnName, tx);
	      RETURN_ERROR(NULL);
	    }
	 }

	/* Multiply the terms together */

	dosolve (tr, pos, start, stop, tx);

	RETURN_NORMAL(tr);
}

TRSF *solveInvChain (tr, pos, t1, t2, tx)
TRSF *tr, *t1, *t2, *tx;
POS *pos;
{
	/* Solve for the portion (chain) of the position equation 'pos' 
	   delimited by the transforms 't1' and 't2'. Go around the
	   loop clockwise. If the T6 transform is part of the chain, use the
	   transform 'tx' to denote it's value. 
	 */	   

	char *fxnName = "solveChain()";
	POS_TERM *term;
	POS_TERM *start = NULL;
	POS_TERM *stop = NULL;
	int t6Included = 0;

	/* pick out the equation term corresponding to the start 
	   transform 't1' */

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

	if (!start)
	 { errorMsg (ERange, rcclNoTrsfMsg, fxnName, t1);
	   RETURN_ERROR(NULL);
	 }

	/* pick out the equation term corresponding to the start 
	   transform 't2' */

	term = start;
	do
	 { if (term == pos->t6Term)
	    { t6Included = 1;
	      if (t2 == T6 || t2 == TRANS)
	       { stop = term;
		 break;
	       }
	    }
	   else if (term->trsf == t2)
	    { stop = term;
	      break;
	    }
	   term = term->prev;
	 }
	while (term != start);

	if (!stop)
	 { errorMsg (ERange, rcclNoTrsfMsg, fxnName, t2);
	   RETURN_ERROR(NULL);
	 }

	/* Get a value for T6 if it is part of the chain to be solved */

	if (t6Included)
	 { if (tx == NULL)
	    { errorMsg (EBadArg, rcclBadTrsfMsg, fxnName, tx);
	      RETURN_ERROR(NULL);
	    }
	 }

	/* Multiply the terms together. */

	doinvsolve (tr, pos, start, stop, tx);

	RETURN_NORMAL(tr);
}

#define READ_TRANS(s,d)	\
	if (TRSF_GETDESC(s)->magic != TRSF_MAGIC) \
	 { *(d) = *(s); \
	 } \
	else \
	 { readTrans (s, d); \
	 }

static dosolve(tr, pos, start, stop, tx)
TRSF *tr, *tx;
POS *pos;
POS_TERM *start, *stop;
{
	/* Go counter-clockwise around the loop and multiply together adjacent
	   terms between 'start' and 'stop' inclusive. Invert terms
	   on the right-hand side. Use 'tx' to substitute for the value
	   of T6 if necessary. */

	POS_TERM *term = start;
	TRSF result, temp;

	if (start == pos->t6Term)
	 { READ_TRANS (tx, &result);
	 }
	else
	 { READ_TRANS (term->trsf, &result);
	 }
	if (!start->leftSide)
	 { invertTrsf (&result, &result);
	 }
	if ((term = start) == stop)
	 { *tr = result;
	   return;
	 }
	while (term != stop)
	 { term = term->next;
	   if (term == pos->t6Term)
	    { READ_TRANS (tx, &temp);
	    }
	   else
	    { READ_TRANS (term->trsf, &temp);
	    }
	   if (!term->leftSide)
	    { multRiTrsf (&result, &result, &temp);
	    }
	   else
	    { multTrsf (&result, &result, &temp);
	    }
	 }
	*tr = result;
}

static doinvsolve(tr, pos, start, stop, tx)
TRSF *tr, *tx;
POS *pos;
POS_TERM *start, *stop;
{
	/* Go clockwise around the loop and multiply together adjacent
	   terms between 'start' and 'stop' inclusive. Invert terms
	   on the left-hand side. Use 'tx' to substitute for the value
	   of T6 if necessary. */

	POS_TERM *term = start;
	TRSF result, temp;

	if (start == pos->t6Term)
	 { READ_TRANS (tx, &result);
	 }
	else
	 { READ_TRANS (start->trsf, &result);
	 }
	if (start->leftSide)
	 { invertTrsf (&result, &result);
	 }
	if ((term = start) == stop)
	 { *tr = result;
	   return;
	 }
	while (term != stop)
	 { term = term->prev;
	   if (term == pos->t6Term)
	    { READ_TRANS (tx, &temp);
	    }
	   else
	    { READ_TRANS (term->trsf, &temp);
	    }
	   if (term->leftSide)
	    { multRiTrsf (&result, &result, &temp);
	    }
	   else
	    { multTrsf (&result, &result, &temp);
	    }
	 }
	*tr = result;
}

#if TEST

#include <stdio.h>

#include <ctype.h>
#include "../h/ctree.h"

char key_buf[256];


struct trsfTab {
	char *name;
	TRSF *trsf;
  };

static char *namep;

getTrsfInit (cp)
char *cp;
{
	namep = cp;
}

TRSF *getTrsf(tab)
struct trsfTab *tab;
{
	char name[256];
	char *cp, *np;
	int k;

	cp = namep;
	while (isspace(*cp))	/* skip spaces */
	 { cp++;
	 }
	np = name;
	while (!isspace(*cp) && *cp)	/* copy non spaces into name */
	 { *np++ = *cp++;
	 }
	*np = '\0';
	namep = cp;
	for (k=0; tab[k].name; k++)
	 { if (strcmp (tab[k].name, name) == 0)
	    { return (tab[k].trsf);
	    }
	 }
	return (NULL);
}

main()
{
	char *keytree;
	char name[32];

	struct trsfTab tab[32];

	TRSF_PTR a, b, c, t6, tool, bogus;
	TRSF_PTR start, stop, tx, tr;
	TRSF realbogus, result;
	POS *pos;

	rcclSetOptions (RCCL_ERROR_EXIT);

	a  = allocTransXyz ("a", 0,  10.0, 0.0, 0.0);
	b  = allocTransXyz ("b", 0,  20.0, 0.0, 0.0);
	c  = allocTransXyz ("c", 0,  30.0, 0.0, 0.0);
	t6 = allocTransXyz ("t6", 0,  40.0, 0.0, 0.0);
	tool = allocTransXyz ("tool", 0, 0.0, 10.0, 0.0);
	bogus = &realbogus;

	identTrsf (bogus);

	tab[0].name = "a";	tab[0].trsf = a;
	tab[1].name = "b";	tab[1].trsf = b;
	tab[2].name = "c";	tab[2].trsf = c;
	tab[3].name = "t6";	tab[3].trsf = t6;
	tab[4].name = "tool";	tab[4].trsf = tool;
	tab[5].name = "bogus";	tab[5].trsf = bogus;
	tab[6].name = NULL;

	pos = makePosition ("P", T6, tool, EQ, a, b, c, TL, tool);

	rcclClearOptions (RCCL_ERROR_EXIT);

	keytree = tree_match_parse("					\
(	solveTrans							\
	 ([a b c t6 tool bogus]						\
	   ([a b c t6 tool bogus]))					\
	[solveChain solveInvChain]					\
	 ([a b c t6 tool bogus]						\
	   ([a b c t6 tool bogus]					\
	     ([a b c t6 tool bogus])))					\
	print								\
	 (a b c t6 tool bogus)						\
	quit								\
)", 0);
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	do
 	 { tree_match (keytree, "SOLVEPOSITION> ", key_buf);

	   /* --- set the pointers to the functions --- */

	   tr = &result;

	   if (COMMANDS ("solveTrans"))
	    { getTrsfInit (index(key_buf, ' ')+1);
	      if ((start = getTrsf(tab)) == t6)
	       { start = T6;
	       }
	      tx = getTrsf(tab);
	    }
	   else if (COMMANDS ("solve"))
	    { getTrsfInit (index(key_buf, ' ')+1);
	      if ((start = getTrsf(tab)) == t6)
	       { start = T6;
	       }
	      if ((stop = getTrsf(tab)) == t6)
	       { stop = T6;
	       }
	      tx = getTrsf(tab);
	    }

	   if (COMMANDS("solveTrans"))
	    { if (solveTrans (tr, pos, start, tx) != tr)
	       { printErrors();
		 clearErrors();
	       }
	      else
	       { printTrsf ("Result: %m\n", tr);
	       }
	    }
	   else if (COMMANDS("solveChain"))
	    { if (solveChain (tr, pos, start, stop, tx) != tr)
	       { printErrors();
		 clearErrors();
	       }
	      else
	       { printTrsf ("Result: %m\n", tr);
	       }
	    }
	   else if (COMMANDS("solveInvChain"))
	    { if (solveInvChain (tr, pos, start, stop, tx) != tr)
	       { printErrors();
		 clearErrors();
	       }
	      else
	       { printTrsf ("Result: %m\n", tr);
	       }
	    }
	   else if (COMMANDS("print"))
	    { getTrsfInit (index(key_buf, ' ')+1);
	      printTrsf ("%m\n", getTrsf(tab));
	    }
	 }
	while (!(COMMAND("quit")));
}





#endif
