/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : equat.c
 *      Remarks : Implements the equation structures.
 *                The chosen ring structure does not make me quite happy.
 *      Usage   : part of the library
 */

/*LINTLIBRARY*/

#include "../h/rccl.h"
#include "../h/manip.h"

/*
 * sets up the data struct. representing a transform equation
 * pt = makeposition(name, &L1, ..., &Ln, EQ, &R1, ..., &Rn, TL, toolptr);
 */

#define ISCONS(t)       ((t)->trsf->fn == const)
#define ISHOLD(t)       ((t)->trsf->fn == hold)
#define ISVARB(t)       ((t)->trsf->fn == varb)
#define ISFUND(t)       (!(ISCONS(t) || ISHOLD(t) || ISVARB(t)))

/* VARARGS */
POS_PTR makeposition(sp, a)  /*::*/
char *sp;
TRSF_PTR a;
{
	int n2;                 /* counter                              */
	char altname[80];       /* some storage for string management   */
	TRSF_PTR mark;          /* for searching in the arg list        */
	TRSF_PTR tp;            /* tool transform pointer               */
	TRSF_PTR *pa;           /* pointer  to arg list in the stack    */
	PST_PTR k;              /* what's being returned                */
	TERM_PTR n, m, p, s, f; /* aux pointers to walk around          */
	TERM_PTR newterm_n();   /* dyn. alloc                           */
	PST_PTR newposition_n();  /* ........                           */
	char *strcpy(),         /* those cause trouble for lint         */
	     *strcat(),         /* because bug in llib-lc               */
	     *strsave();

	pa = &a;                /* pa points on the first arg.          */
	/*
	 * search for tp and check arg list
	 */
	for(mark = (TRSF_PTR)EQ, n2 = 2; n2--; mark = (TRSF_PTR)TL) {
		for (; *pa != mark; pa++) {
			if (*pa == NULL) {
				fprintf(stderr, "position \"%s\" :", sp);
				giveup(
			"transform not initialized - makeposition", YES);
			}
		}
		pa++;           /* skip mark */
	}

	tp = *pa;               /* toolptr */

	pa = &a;                /* reset pa */

	k = newposition_n();    /* create a position record     */
	k->t6ptr = NULL;        /* for later checking           */
	k->tlptr = NULL;        /* for later checking           */
	k->name = sp;           /* pointer to string            */

	n = m = f = p = s = newterm_n();   /* create first term */
	p->oldt = p->altr = p->trsf = (*pa);    /* first arg    */
	p->rhs = NO;                            /* in lhs       */
	if (p->trsf == t6) k->t6ptr = p;        /* set t6ptr if t6 met */
	if (n->trsf == tp) k->tlptr = n;        /* set tlptr if tool met */

	while (*++pa != (TRSF_PTR)EQ) {   /* connect terms in given order */
		n = newterm_n();
		p->next = n;
		n->prev = p;
		n->oldt = n->altr = n->trsf = (*pa);
		n->rhs = NO;
		if (n->trsf == t6) k->t6ptr = n; /* set t6ptr if t6 met */
		if (n->trsf == tp) k->tlptr = n; /* set tlptr if tool met */
		p = n;              /* progress */
	}

	while (*++pa != (TRSF_PTR)TL) { /* reverse connect */
		m = newterm_n();
		s->prev = m;
		m->next = s;
		m->oldt = m->altr = m->trsf = (*pa);
		m->rhs = YES;
		if (m->trsf == tp) k->tlptr = m;/* set tlptr if tool met */
		s = m;            /* progress */
	}

	n->next = m; /* finish the ring */
	m->prev = n;


	/*
	 * check if t6ptr and tlptr set
	 */

	if (k->t6ptr == NULL || k->tlptr == NULL) {
		fprintf(stderr, "position \"%s\" :", k->name);
		giveup("missing t6 or tool -  makeposition", YES);
	}
	if (f == m) {
		fprintf(stderr, "position \"%s\" :", k->name);
		giveup("missing rhs -  makeposition", YES);
	}

	/*
	 * set the pos field
	 */

	k->pos = k->tlptr->next;
	if (ISFUND(k->pos)) {
		fprintf(stderr, "position \"%s\", transform \"%s\" :",
			k->name, k->pos->trsf->name);
		giveup("pos functionally defined - makeposition", YES);
	}
	if (k->pos == k->t6ptr) {
		fprintf(stderr, "position \"%s\", :", k->name);
		giveup("pos cannot seriously be t6 - makeposition", YES);
	}

	/*
	 * print equation
	 */

	if (prints_out) {
		fprintf(fpi, "makeposition, pos\t\"%s\"\t", k->name);
		for (s = f; !s->rhs; s = s->next) {
			fprintf(fpi, "%s ", s->trsf->name);
			if (s->next == f) {
				break;
			}
		}
		fprintf(fpi, " = ");
		for (s = f->prev; s->rhs; s = s->prev) {
			fprintf(fpi, "%s ", s->trsf->name);
		}
		fprintf(fpi, "\n");
	}

	/*
	 * optimize
	 */

	optimize(k);

	/*
	 * allocate storage for the hold feature
	 */

	for (n = k->t6ptr->prev; n != k->t6ptr; n = n->prev) {
		if (ISHOLD(n)) {
			n->altr = newtrans(strsave(strcat(strcpy(altname,
					n->trsf->name), "(a)")), hold);
			Assigntr(n->altr,  n->trsf);
			n->oldt = newtrans(strsave(strcat(strcpy(altname,
					n->trsf->name), "(o)")), hold);
			Assigntr(n->oldt,  n->trsf);
		}
	}


	/*
	 * set the coord , tool, functional coord, functional tool predicates
	 * and print the canonized equation
	 */

	k->cfnsp = k->tfnsp = k->coorp = k->toolp = NO;
	if (prints_out) {
		fprintf(fpi,"\t\"COORD\":");
	}
	for (n = k->t6ptr->prev; n != k->pos; n = n->prev) {
		k->coorp = YES;
		if (ISFUND(n)) {
			k->cfnsp = YES;
		}
		if (prints_out) {
			fprintf(fpi,
				n->rhs ? " %s%s" : " -%s%s", n->trsf->name,
	ISCONS(n) ? "" : ISHOLD(n) ? "(h)" : ISVARB(n) ? "(v)" : "(s)");
		}
	}
	if (prints_out) {
		fprintf(fpi,"  \"TOOL\":");
	}
	for (n = k->pos->prev; n != k->t6ptr; n = n->prev) {
		k->toolp = YES;
		if (ISFUND(n)) {
			k->tfnsp = YES;
		}
		if (prints_out) {
			fprintf(fpi,
				n->rhs ? " %s%s" : " -%s%s", n->trsf->name,
	ISCONS(n) ? "" : ISHOLD(n) ? "(h)" : ISVARB(n) ? "(v)" : "(s)");
		}
	}
	if (prints_out) {
		fprintf(fpi,
		k->pos->rhs ? "  \"POS\": %s%s\n\n" : "  \"POS\": -%s%s\n\n",
			k->pos->trsf->name,
			ISVARB(k->pos) ? "(v)" : ISHOLD(k->pos) ? "(h)" : "");
	}
	return((POS_PTR)k);
}


/*
 * free an equation
 */

freepos(p) /*::*/
POS_PTR p;
{
	TERM_PTR t;

	for (t = ((PST_PTR)p)->t6ptr->prev;
	     t != ((PST_PTR)p)->t6ptr;
	     t = t->prev) {
		if (ISHOLD(t)) {
			free(t->altr->name);
			freetrans(t->altr);
			free(t->oldt->name);
			freetrans(t->oldt);
		}
		if (t->trsf->name[0] == '_') {
			free(t->trsf->name);
			freetrans(t->trsf);
		}
		free((char *)t);
	}
	free((char *)t);
	free((char *)p);
}


/*
 * solve differential coordinate transforms
 * #defining DEBUG helped
 * those functions make sure that user's functions are called only once
 * per sample time
 * They are pretty shaky because they assume canonized equations
 * where everything is in the rhs expect t6
 * also they do not keep the inverses once calculated
 * however they do not have to choose the best path, which is dictated
 * format is : solve..(result, term1, term2)
 */

#ifdef  DEBUG
#define PREAMB  printf("%s %d %s : ",goalpos->name, rtime, r->name);
#define POSTAMB printf("\n");
#else
#define PREAMB
#define POSTAMB
#endif

/*
 * solve the inverse when in rhs, direct in lhs
 */

#ifdef  DEBUG
#define CTRF(t) (printf((t->rhs)?" -%s":" %s",t->altr->name),t->altr)
#else
#define CTRF(t) t->altr
#endif

solvei_n(r, v, w) /*::*/
TRSF_PTR r;
TERM_PTR v, w;
{
	PREAMB
	w = w->next;
	if (w->trsf->timeval != rtime) {
		(* w->trsf->fn)(w->trsf);
		w->trsf->timeval = rtime;
	}
	if (w->rhs) {
		Invert(r, CTRF(w));
	}
	else {
		Assigntr(r, CTRF(w));
	}
	for (w = w->next; w != v; w = w->next) {
		if (w->trsf->timeval != rtime) {
			(* w->trsf->fn)(w->trsf);
			w->trsf->timeval = rtime;
		}
		if (w->rhs) {
			Trmultinv(r, CTRF(w));
		}
		else {
			Trmultinp(r, CTRF(w));
		}
	}
	POSTAMB
}


/*
 * solve the direct when in rhs, inverse in lhs
 */

#undef  CTRF
#ifdef  DEBUG
#define CTRF(t) (printf((t->rhs)?" %s":" -%s",t->altr->name),t->altr)
#else
#define CTRF(t) t->altr
#endif

solved_n(r, v, w) /*::*/
TRSF_PTR r;
TERM_PTR v, w;
{
	PREAMB
	v = v->prev;
	if (v->trsf->timeval != rtime) {
		(* v->trsf->fn)(v->trsf);
		v->trsf->timeval = rtime;
	}
	if (v->rhs) {
		Assigntr(r, CTRF(v));
	}
	else {
		Invert(r, CTRF(v));
	}
	for (v = v->prev; v != w; v = v->prev) {
		if (v->trsf->timeval != rtime) {
			(* v->trsf->fn)(v->trsf);
			v->trsf->timeval = rtime;
		}
		if (v->rhs) {
			Trmultinp(r, CTRF(v));
		}
		else {
			Trmultinv(r, CTRF(v));
		}
	}
	POSTAMB
}



/*
 * same thing, but use 'old' transforms for calculating transitions
 * with 'hold' transforms which may have changed meanhile
 */

#undef  CTRF
#ifdef  DEBUG
#define CTRF(t) (printf((t->rhs)?" -%s":" %s",t->oldt->name),t->oldt)
#else
#define CTRF(t) t->oldt
#endif

solveio_n(r, v, w) /*::*/
TRSF_PTR r;
TERM_PTR v, w;
{
	PREAMB
	w = w->next;
	if (w->trsf->timeval != rtime) {
		(* w->trsf->fn)(w->trsf);
		w->trsf->timeval = rtime;
	}
	if (w->rhs) {
		Invert(r, CTRF(w));
	}
	else {
		Assigntr(r, CTRF(w));
	}
	for (w = w->next; w != v; w = w->next) {
		if (w->trsf->timeval != rtime) {
			(* w->trsf->fn)(w->trsf);
			w->trsf->timeval = rtime;
		}
		if (w->rhs) {
			Trmultinv(r, CTRF(w));
		}
		else {
			Trmultinp(r, CTRF(w));
		}
	}
	POSTAMB
}


#undef  CTRF
#ifdef  DEBUG
#define CTRF(t) (printf((t->rhs)?" %s":" -%s",t->oldt->name),t->oldt)
#else
#define CTRF(t) t->oldt
#endif

solvedo_n(r, v, w) /*::*/
TRSF_PTR r;
TERM_PTR v, w;
{
	PREAMB
	v = v->prev;
	if (v->trsf->timeval != rtime) {
		(* v->trsf->fn)(v->trsf);
		v->trsf->timeval = rtime;
	}
	if (v->rhs) {
		Assigntr(r, CTRF(v));
	}
	else {
		Invert(r, CTRF(v));
	}
	for (v = v->prev; v != w; v = v->prev) {
		if (v->trsf->timeval != rtime) {
			(* v->trsf->fn)(v->trsf);
			v->trsf->timeval = rtime;
		}
		if (v->rhs) {
			Trmultinp(r, CTRF(v));
		}
		else {
			Trmultinv(r, CTRF(v));
		}
	}
	POSTAMB
}


/*
 * shift the old values of the hold transforms
 */

shifttr_n(p) /*::*/
PST_PTR p;
{
	register TERM_PTR t;

	for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
		if (ISHOLD(t)) {
			Assigntr(t->oldt, t->altr);
		}
	}
}
