/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : moves.c
 *      Remarks : This file contains the functions that make the
 *                the interface between the user and setpoint.
 *      Usage   : part of the lib
 */

/*LINTLIBRARY*/

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

/*
 * static motion record that is set by the user copied into the queue
 * and reset for each move request
 */

ROOT mqueue_n = {NULL, NULL};           /* queue header record  */
MOT motionreq_n = {
	0, 0,                           /* acct, sgt    */
	DEF_TRSV, DEF_ROTV,             /* velt, velr   */
	NULL,                           /* pst          */
	'j',                            /* mod          */
	"",                             /* cfg          */
	NULL,                           /* upd          */
	NULL,                           /* fnt          */
	0,                              /* smpl         */
	0.,                             /* fmass        */
	0,                              /* fsp          */
	0., 0., 0., 0., 0., 0.,         /* frc          */
	0,                              /* csp          */
	0., 0., 0., 0., 0., 0.,         /* cpy          */
	0,                              /* dsp          */
	0., 0., 0., 0., 0., 0.,         /* dst          */
	0,                              /* exp          */
	0., 0., 0., 0., 0., 0.          /* exd          */
};

static TRSF_PTR updtr = NULL;           /* the transorm to update       */
static PST_PTR updpos = NULL;           /* in this pos equation         */


/*
 * the following functions merely set the different fields of the
 * motion record.
 * replacing them by macros would reduce the umber of entries in the
 * library for the benefit of the loader but would make
 * interlanguage communication impossible
 */


setmod(m) /*::*/
int m;
{
	motionreq_n.mod = m;
}


setime(ta, ts) /*::*/
int ta, ts;
{
	motionreq_n.acct = ta;
	motionreq_n.sgt = ts;
}


setvel(t, r) /*::*/
int t, r;
{
	motionreq_n.velt = t;
	motionreq_n.velr = r;
}


evalfn(fn) /*::*/
TRFN fn;
{
	motionreq_n.fnt = fn;
}


setconf(s) /*::*/
char *s;
{
	motionreq_n.cfg = s;
}



sample(m) /*::*/
int m;
{
	if (m < MINSAMPLE) {
		m = MINSAMPLE;
	}
	if (m > MAXSAMPLE) {
		m = MAXSAMPLE;
	}
	motionreq_n.smpl = m - m % HARDCLOCK;
}


massis(m) /*::*/
real m;
{
	motionreq_n.fmass = m * 9.81;
}


/*
 * this one is a special case, only a TERM_PTR need to be transmitted
 * to the setpoint for solving, the corresponding term is set in movereq
 */

update(u, p) /*::*/
TRSF_PTR u;
POS_PTR p;
{
	updtr = u;
	updpos = (PST_PTR)p;
}


#define NmTONmm         1000.
#define DegTORad        0.01745329251994330

/*
 * parses the argument string, and set the motion record limit entries
 */

/* VARARGS */
limit(c, a) /*::*/
char *c;
double a;
{
	double *pa = &a;

	while (*c) {
		switch (*c++) {
		case 'f' :
			switch (*c++) {
			case 'x' :
				motionreq_n.fsp |= SELFX;
				motionreq_n.frc.f.x = *pa++;
				break;

			case 'y' :
				motionreq_n.fsp |= SELFY;
				motionreq_n.frc.f.y = *pa++;
				break;

			case 'z' :
				motionreq_n.fsp |= SELFZ;
				motionreq_n.frc.f.z = *pa++;
				break;

			default :
				goto bad;
			}
			break;

		case 't' :
			switch (*c++) {
			case 'x' :
				motionreq_n.fsp |= SELMX;
				motionreq_n.frc.m.x = *pa++ * NmTONmm;
				break;

			case 'y' :
				motionreq_n.fsp |= SELMY;
				motionreq_n.frc.m.y = *pa++ * NmTONmm;
				break;

			case 'z' :
				motionreq_n.fsp |= SELMZ;
				motionreq_n.frc.m.z = *pa++ * NmTONmm;
				break;

			default :
				goto bad;
			}
			break;

		case 'd' :
			switch (*c++) {
			case 'x' :
				motionreq_n.dsp |= SELTX;
				motionreq_n.dst.t.x = *pa++;
				break;

			case 'y' :
				motionreq_n.dsp |= SELTY;
				motionreq_n.dst.t.y = *pa++;
				break;

			case 'z' :
				motionreq_n.dsp |= SELTZ;
				motionreq_n.dst.t.z = *pa++;
				break;

			default :
				goto bad;
			}
			break;

		case 'r' :
			switch (*c++) {
			case 'x' :
				motionreq_n.dsp |= SELRX;
				motionreq_n.dst.r.x = *pa++ * DegTORad;
				break;

			case 'y' :
				motionreq_n.dsp |= SELRY;
				motionreq_n.dst.r.y = *pa++ * DegTORad;
				break;

			case 'z' :
				motionreq_n.dsp |= SELRZ;
				motionreq_n.dst.r.z = *pa++ * DegTORad;
				break;

			default :
				goto bad;
			}
			break;

		case ' ' :
			break;

		default :
			goto bad;
		}
	}
	return;
bad :
	giveup("bad spec. - limit", YES);
}



/*
 * comply and lock parse the string argument and the set the comply entry
 * perform set operations to make sure specs are consistent
 */

/* VARARGS */
comply(c, a) /*::*/
char *c;
double a;
{
	double *pa = &a;

	while (*c) {
		switch (*c++) {
		case 'f' :
			switch (*c++) {
			case 'x' :
				if (motionreq_n.csp & SELFX) {
					goto bad;
				}
				motionreq_n.csp |= SELFX;
				motionreq_n.cpy.f.x = *pa++;
				break;

			case 'y' :
				if (motionreq_n.csp & SELFY) {
					goto bad;
				}
				motionreq_n.csp |= SELFY;
				motionreq_n.cpy.f.y = *pa++;
				break;

			case 'z' :
				if (motionreq_n.csp & SELFZ) {
					goto bad;
				}
				motionreq_n.csp |= SELFZ;
				motionreq_n.cpy.f.z = *pa++;
				break;

			default :
				goto bad;
			}
			break;

		case 't' :
			switch (*c++) {
			case 'x' :
				if (motionreq_n.csp & SELMX) {
					goto bad;
				}
				motionreq_n.csp |= SELMX;
				motionreq_n.cpy.m.x = *pa++ * NmTONmm;
				break;

			case 'y' :
				if (motionreq_n.csp & SELMY) {
					goto bad;
				}
				motionreq_n.csp |= SELMY;
				motionreq_n.cpy.m.y = *pa++ * NmTONmm;
				break;

			case 'z' :
				if (motionreq_n.csp & SELMZ) {
					goto bad;
				}
				motionreq_n.csp |= SELMZ;
				motionreq_n.cpy.m.z = *pa++ * NmTONmm;
				break;

			default :
				goto bad;
			}
			break;

		case ' ' :
			break;

		default :
			goto bad;
		}
	}
	return;
bad :
	giveup("bad force spec. - comply", YES);
}




lock(c) /*::*/
char *c;
{
	while (*c) {
		switch (*c++) {
		case 'f' :
			switch (*c++) {
			case 'x' :
				if (!(motionreq_n.csp & SELFX)) {
					goto bad;
				}
				motionreq_n.csp &= ~SELFX;
				motionreq_n.cpy.f.x = 0.;
				break;

			case 'y' :
				if (!(motionreq_n.csp & SELFY)) {
					goto bad;
				}
				motionreq_n.csp &= ~SELFY;
				motionreq_n.cpy.f.y = 0.;
				break;

			case 'z' :
				if (!(motionreq_n.csp & SELFZ)) {
					goto bad;
				}
				motionreq_n.csp &= ~SELFZ;
				motionreq_n.cpy.f.z = 0.;
				break;

			default :
				goto bad;
			}
			break;

		case 't' :
			switch (*c++) {
			case 'x' :
				if (!(motionreq_n.csp & SELMX)) {
					goto bad;
				}
				motionreq_n.csp &= ~SELMX;
				motionreq_n.cpy.m.x = 0.;
				break;

			case 'y' :
				if (!(motionreq_n.csp & SELMY)) {
					goto bad;
				}
				motionreq_n.csp &= ~SELMY;
				motionreq_n.cpy.m.y = 0.;
				break;

			case 'z' :
				if (!(motionreq_n.csp & SELMZ)) {
					goto bad;
				}
				motionreq_n.csp &= ~SELMZ;
				motionreq_n.cpy.m.z = 0.;
				break;

			default :
				goto bad;
			}
			break;

		case ' ' :
			break;

		default :
			goto bad;
		}
	}
	return;
bad :
	giveup("bad force spec. - lock", YES);
}



/*
 * same principle as above but for distances, no check necessary
 */

/* VARARGS */
distance(c, a) /*::*/
char *c;
double a;
{
	double *pa = &a;

	while (*c) {
		switch (*c++) {
		case 'd' :
			switch (*c++) {
			case 'x' :
				motionreq_n.exp |= SELTX;
				motionreq_n.exd.t.x = *pa++;
				break;

			case 'y' :
				motionreq_n.exp |= SELTY;
				motionreq_n.exd.t.y = *pa++;
				break;

			case 'z' :
				motionreq_n.exp |= SELTZ;
				motionreq_n.exd.t.z = *pa++;
				break;

			default :
				goto bad;
			}
			break;

		case 'r' :
			switch (*c++) {
			case 'x' :
				motionreq_n.exp |= SELRX;
				motionreq_n.exd.r.x = *pa++ * DegTORad;
				break;

			case 'y' :
				motionreq_n.exp |= SELRY;
				motionreq_n.exd.r.y = *pa++ * DegTORad;
				break;

			case 'z' :
				motionreq_n.exp |= SELRZ;
				motionreq_n.exd.r.z = *pa++ * DegTORad;
				break;

			default :
				goto bad;
			}
			break;

		case ' ' :
			break;

		default :
			goto bad;
		}
	}
	return;
bad :
	giveup("bad distance spec. - distance", YES);
}




/*
 * move, set up the  position and issue the request
 */

move(p) /*::*/
POS_PTR p;
{
	motionreq_n.pst = (PST_PTR)p;
	movereq();
}


/*
 * stop issue the request without pos, means redo the last one
 */

stop(t) /*::*/
int t;
{
	motionreq_n.sgt = t;
	movereq();
}


/*
 * 1) make sure specs are consitent
 * 2) print the requests
 * 3) put them in the queue
 * 4) reset the record
 */

movereq() /*::*/
{
	static int sam = DEF_SAMPLE;
	static FORCE zeroforce = {0., 0., 0., 0., 0., 0.};
	static DIFF zerodiff = {0., 0., 0., 0., 0., 0.};

	char *new();

	if (*(motionreq_n.cfg) != '\0' && motionreq_n.mod != 'j') {
		giveup("conf must change in joint mode", YES);
	}
	if (motionreq_n.mod != 'j' && motionreq_n.mod != 'c') {
		motionreq_n.mod = 'j';
	}

	if (motionreq_n.smpl != 0) {
		sam = motionreq_n.smpl;
	}
	motionreq_n.acct /= sam;
	motionreq_n.sgt /= sam;

	if (motionreq_n.sgt == 0) {
		if (motionreq_n.velt <= 0) {
			motionreq_n.velt = DEF_TRSV;
		}
		if (motionreq_n.velr <= 0) {
			motionreq_n.velr = DEF_ROTV;
		}
		if (motionreq_n.acct == 0) {
			motionreq_n.acct = 2 +
			((motionreq_n.velt + motionreq_n.velr) / (20 * sam));
		}
	}
	else {
		if (motionreq_n.acct == 1) {
			motionreq_n.acct = 2;
		}
		if (2 * motionreq_n.acct > motionreq_n.sgt) {
			motionreq_n.sgt = 2 * motionreq_n.acct;
		}
	}
	if (motionreq_n.pst != NULL) {
		PST_PTR p = motionreq_n.pst;
		TERM_PTR t;
		TRSF_PTR h;
		ITEM_PTR ih;

		for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
			if (t->trsf->fn == hold) {
				if ((ih = (ITEM_PTR)new(sizeof(ITEM) +
						       sizeof(TRSF)))
				       == NULL) {
					giveup("alloc err", YES);
				}
				h = (TRSF_PTR)((char *)ih + sizeof(ITEM));
				Assigntr(h, t->trsf);
				enqueue_n(&(t->hd), ih);
			}
		}
		if (updtr != NULL) {
			if (updtr->fn != varb) {
				giveup("invalid update transform type", YES);
			}
			p = updpos;
			for (t = p->t6ptr->prev; t != p->t6ptr; t = t->prev) {
				if (t->trsf == updtr) {
					motionreq_n.upd = t;
					break;
				}
			}
			if (motionreq_n.upd == NULL) {
				giveup("could'nt find updatable transform", YES);
			}
		}
	}

	if (prints_out) {
		fprintf(fpi,
		"request %s mode %c acct %d  sgt %d  velt %d velr %d\n",
			(motionreq_n.pst) ? motionreq_n.pst->name : "STOP",
			motionreq_n.mod,
			motionreq_n.acct * sam,
			motionreq_n.sgt * sam,
			motionreq_n.velt,
			motionreq_n.velr);
		fprintf(fpi,
		"conf %s  %s upd : %s smpl %d mass %f\n",
			motionreq_n.cfg,
			(motionreq_n.fnt) ? "EVAL" : "",
			(motionreq_n.upd) ? motionreq_n.upd->trsf->name : "",
			motionreq_n.smpl,
			motionreq_n.fmass);
		if (motionreq_n.fsp & SELFX) {
			fprintf(fpi, "stop fx : %-5g\n", motionreq_n.frc.f.x);
		}
		if (motionreq_n.fsp & SELFY) {
			fprintf(fpi, "stop fy : %-5g\n", motionreq_n.frc.f.y);
		}
		if (motionreq_n.fsp & SELFZ) {
			fprintf(fpi, "stop fz : %-5g\n", motionreq_n.frc.f.z);
		}
		if (motionreq_n.fsp & SELMX) {
			fprintf(fpi, "stop mx : %-5g\n", motionreq_n.frc.m.x);
		}
		if (motionreq_n.fsp & SELMY) {
			fprintf(fpi, "stop my : %-5g\n", motionreq_n.frc.m.y);
		}
		if (motionreq_n.fsp & SELMZ) {
			fprintf(fpi, "stop mz : %-5g\n", motionreq_n.frc.m.z);
		}

		if (motionreq_n.csp & SELFX) {
			fprintf(fpi, "cply fx : %-5g\n", motionreq_n.cpy.f.x);
		}
		if (motionreq_n.csp & SELFY) {
			fprintf(fpi, "cply fy : %-5g\n", motionreq_n.cpy.f.y);
		}
		if (motionreq_n.csp & SELFZ) {
			fprintf(fpi, "cply fz : %-5g\n", motionreq_n.cpy.f.z);
		}
		if (motionreq_n.csp & SELMX) {
			fprintf(fpi, "cply mx : %-5g\n", motionreq_n.cpy.m.x);
		}
		if (motionreq_n.csp & SELMY) {
			fprintf(fpi, "cply my : %-5g\n", motionreq_n.cpy.m.y);
		}
		if (motionreq_n.csp & SELMZ) {
			fprintf(fpi, "cply mz : %-5g\n", motionreq_n.cpy.m.z);
		}

		if (motionreq_n.dsp & SELTX) {
			fprintf(fpi, "stop dx : %-5g\n", motionreq_n.dst.t.x);
		}
		if (motionreq_n.dsp & SELTY) {
			fprintf(fpi, "stop dy : %-5g\n", motionreq_n.dst.t.y);
		}
		if (motionreq_n.dsp & SELTZ) {
			fprintf(fpi, "stop dz : %-5g\n", motionreq_n.dst.t.z);
		}
		if (motionreq_n.dsp & SELRX) {
			fprintf(fpi, "stop rx : %-5g\n", motionreq_n.dst.r.x);
		}
		if (motionreq_n.dsp & SELRY) {
			fprintf(fpi, "stop ry : %-5g\n", motionreq_n.dst.r.y);
		}
		if (motionreq_n.dsp & SELRZ) {
			fprintf(fpi, "stop rz : %-5g\n", motionreq_n.dst.r.z);
		}

		if (motionreq_n.exp & SELTX) {
			fprintf(fpi, "dist dx : %-5g\n", motionreq_n.exd.t.x);
		}
		if (motionreq_n.exp & SELTY) {
			fprintf(fpi, "dist dy : %-5g\n", motionreq_n.exd.t.y);
		}
		if (motionreq_n.exp & SELTZ) {
			fprintf(fpi, "dist dz : %-5g\n", motionreq_n.exd.t.z);
		}
		if (motionreq_n.exp & SELRX) {
			fprintf(fpi, "dist rx : %-5g\n", motionreq_n.exd.r.x);
		}
		if (motionreq_n.exp & SELRY) {
			fprintf(fpi, "dist ry : %-5g\n", motionreq_n.exd.r.y);
		}
		if (motionreq_n.exp & SELRZ) {
			fprintf(fpi, "dist rz : %-5g\n", motionreq_n.exd.r.z);
		}
		fprintf(fpi, "\n");
	}

	++requestnb;

	{
		ITEM_PTR im;
		MOT_PTR m;

		if ((im = (ITEM_PTR)new(sizeof(ITEM) + sizeof(MOT))) == NULL) {
			giveup("alloc err", YES);
		}
		m = (MOT_PTR)((char *)im + sizeof(ITEM));

		m->acct = motionreq_n.acct;
		m->sgt = motionreq_n.sgt;
		m->velt = motionreq_n.velt;
		m->velr = motionreq_n.velr;
		m->pst = motionreq_n.pst;
		m->mod = motionreq_n.mod;
		m->cfg = motionreq_n.cfg;
		m->fnt = motionreq_n.fnt;
		m->upd = motionreq_n.upd;
		m->smpl = motionreq_n.smpl;
		m->fmass = motionreq_n.fmass;
		m->fsp = motionreq_n.fsp;
		Assignforce(&m->frc, &motionreq_n.frc);
		m->csp = motionreq_n.csp;
		Assignforce(&m->cpy, &motionreq_n.cpy);
		m->dsp = motionreq_n.dsp;
		Assigndiff(&m->dst, &motionreq_n.dst);
		m->exp = motionreq_n.exp;
		Assigndiff(&m->exd, &motionreq_n.exd);

		enqueue_n(&mqueue_n, im);
	}
	motionreq_n.acct = 0;
	motionreq_n.sgt = 0;
	/* no velocities reset */
	motionreq_n.pst = NULL;
	motionreq_n.cfg = "";
	motionreq_n.upd = NULL;
	motionreq_n.fnt = NULL;
	updtr = NULL;
	updpos = NULL;
	motionreq_n.smpl = 0;
	/* no mass reset */
	motionreq_n.fsp = 0;
	Assignforce(&motionreq_n.frc, &zeroforce);
	/* no comply stuff reset */
	motionreq_n.dsp = 0;
	Assigndiff(&motionreq_n.dst, &zerodiff);
	motionreq_n.exp = 0;
	Assigndiff(&motionreq_n.exd, &zerodiff);
}






/*
 * queue and dequeue motion requests
 * queues have a header with two pointers put and get and
 * are a singly linked list with one pointer and a data field
 * items of any size can be handled
 * enqueue uses a pointer to a preallocated area where the data is stored
 * dequeue returns a pointer to the data and free the allocated area
 */

static int enqbusy = NO, deqbusy = NO;


enqueue_n(r, l) /*:: put a record in a queue */
register ROOT_PTR r;
register ITEM_PTR l;
{
	enqbusy = YES;
	while (deqbusy) {
		fprintf(stderr, "*** could'nt queue at %d\n", rtime);
		nap(5);
	       /*NOTREACHED*/
	}
	if (r->put == NULL) {
		r->get = l;
	}
	else {
		r->put->next = l;
	}
	r->put = l;
	r->put->next = NULL;
	enqbusy = NO;
}



ITEM_PTR dequeue_n(r) /*::*/
register ROOT_PTR r;
{
	register ITEM_PTR g;

	deqbusy = YES;
	if (enqbusy) {
		deqbusy = NO;
		return(NULL);   /* stop the arm */
	}
	if (r->get == NULL) {
		deqbusy = NO;
		return(NULL);
	}
	g = r->get;
	r->get = r->get->next;
	if (r->get == NULL) {
		r->put = NULL;
	}
	disp((char *)g);
	deqbusy = NO;
	return((ITEM_PTR)((char *)g + sizeof(ITEM)));
}



/*
 * storage allocator for the private use of queue/dequeue
 * inspired from Kernighan
 */

#define NPOOL   4096

struct s {
	union head *ptr;
	unsigned size;
};

typedef union head {
	struct s s;
	ALIGN   x;
} HEAD;

typedef struct pl {
	struct s s;
	HEAD st[NPOOL];
} POOL;

static POOL pool;
static HEAD *allocp = NULL;


static char *new(nb) /*##*/
int nb;
{
	register HEAD *p, *q;
	register int nunits = 1 + (nb + sizeof(HEAD) - 1) / sizeof(HEAD);

	if ((q = allocp) == NULL) {
		pool.s.ptr = allocp = q = (HEAD *)&pool;
		pool.s.size = NPOOL;
	}
	for (p = q->s.ptr; ; q = p, p = p->s.ptr) {
		if (p->s.size >= nunits) {
			if (p->s.size == nunits) {
				q->s.ptr = p->s.ptr;
			}
			else {
				p->s.size -= nunits;
				p += p->s.size;
				p->s.size = nunits;
			}
			allocp = q;
			return((char *)(p + 1));
		}
		if (p == allocp) {
			return(NULL);
		}
	}
}



static disp(ap)  /*##*/
register char *ap;
{
	register HEAD *p, *q;

	p = (HEAD *) ap - 1;
	for (q = allocp; !(p > q && p < q->s.ptr); q = q->s.ptr)
		if (q >= q->s.ptr && (p > q || p < q->s.ptr))
			break;
	if (p + p->s.size == q->s.ptr) {
		p->s.size += q->s.ptr->s.size;
		p->s.ptr = q->s.ptr->s.ptr;
	}
	else {
		p->s.ptr = q->s.ptr;
	}
	if (q + q->s.size == p) {
		q->s.size += p->s.size;
		q->s.ptr = p->s.ptr;
	}
	else {
		q->s.ptr = p;
	}
	allocp = q;
}
