/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : teach.c
 *      Remarks : Manual teach mode, uses the rest of the system.
 *      Usage   : part of the library
 */

/*LINTLIBRARY*/

#include <ctype.h>
#include "../h/rccl.h"
#include "../h/hand.h"
#include "../h/which.h"
#include "../h/kine.h"
#include "../h/umac.h"


/*
 * the parse function put all user's requests in those statics
 */

static TRSF wtct, wrct, ttct, trct;             /* static alloc of transfor */
static TRSF_PTR wtc = &wtct,                    /* world translation change */
		wrc = &wrct,                    /* ..... rotation    ...... */
		ttc = &ttct,                    /* tool  translation ...... */
		trc = &trct;                    /* ....  rotation    ...... */
static TRSF_PTR e;                              /* tool transform           */
static int vts, vrs;                            /* velocities               */
static char *doom;                              /* message string pointer   */
static double flx, fly, flz, tlx, tly, tlz;     /* force limit values       */
static int flimp;                               /* limit on / off           */
static real object;                             /* mass of object           */


teach(t, p) /*::*/
POS_PTR p;
TRSF_PTR t;
{
	POS_PTR teach;                  /* the pos equations    */
	TRSF_PTR tea;                   /* T6 E = TEA           */
	TRSF wtt,
	     wrt,                       /* dyn. alloc.          */
	     ttt,
	     trt;
	TRSF_PTR wt = &wtt,             /* world transl. part of tea    */
		 wr = &wrt,             /* ..... rot. ..............    */
		 tt = &ttt,             /* tool  transl. part of tea    */
		 tr = &trt;             /* ....  rot. ..............    */
	TRSF temp;
	int action,                     /* returned by parse            */
	    q;
	bool recorded = NO;             /* set when recorded            */
	double getreal(), atof();
	int check();

	if (fddb > 0) {
		waitfor(completed);
		if (gettr(t, fddb) >= 0) {      /* we've got it         */
			return(YES);
		}
	}

	e = gentr_trsl("E", 0., 0., 170.);      /* make a tool  */
	e->fn = hold;                           /* hold it      */
	tea = newtrans("TEA", hold);            /* make tea     */

	teach = makeposition("TEACH", t6, e, EQ, tea, TL, e);

	stop(100);
	waitfor(completed);
	printf("teach mode V1.0, transform %s, position %s\n",
		t->name, p->name);
	setmod('c');
	action = 'n';
	vts = 100;
	vrs = 10;
	doom = "";
	flx = fly = flz = tlx = tly = tlz = 0.;
	flimp = NO;
	object = 0.;
	for (; ; ) {
		waitfor(completed);
		Assigntr(wt, unitr);
		Assigntr(wr, unitr);
		Assigntr(tt, unitr);
		Assigntr(tr, unitr);

		Trmult(&temp, t6, e);
		Taketrsl(wt, &temp);
		Takerot(tr, &temp);
		if (action == 'y') {
			Trmultinp(wt, wtc);
			Trmultinp(wr, wrc);
			Trmultinp(tt, ttc);
			Trmultinp(tr, trc);
		}
		if (action == 'q') {
			if (!recorded) {
				printf("nothing taught, exit ? ");
				QUERY(q);
				if (q == 'y') {
					break;
				}
			}
			else {
				break;
			}
		}

		if (action == 'x') {
			recorded = NO;
			break;
		}
		Trmult(tea, wt, wr);
		Trmult(&temp, tr, tt);
		Trmultinp(tea, &temp);
		setvel(vts, vrs);
		evalfn(check);
		if (flimp) {
			limit("fx fy fz tx ty tz", flx, fly, flz, tlx, tly, tlz);
		}
		massis(object);
		move(teach);
		if (action == 'r') {
			update(t, p);
			recorded = YES;
		}
		if (action == 's') {
			if (fddb < 0) {
				printf("no data base specified\n");
			}
			else {
				if (!recorded) {
					printf("nothing recorded\n");
				}
				else {
					if (savetr(t, fddb) < 0) {
						recorded = NO;
						break;
					}
				}
			}
		}
		move(there);
		Assigntr(wtc, unitr);
		Assigntr(wrc, unitr);
		Assigntr(ttc, unitr);
		Assigntr(trc, unitr);
		printf("%s", doom);
		if (teach->code == ONF) {
			printf("stopped on force\n");
		}

		while ((action = parse()) == 'e' ||
			action == 'S' || action == 'p') {
			if (action == 'S') {
				nextmove = YES;
				printf("%s", doom);
				if (teach->code == ONF) {
					printf("stopped on force\n");
				}
				printf(">> stopped\n");
			}
			if (action == 'p') {
				display();
			}
		}
	}
	move(there);
	waitfor(completed);
	freeposition(teach);
	freetrans(tea);
	freetrans(e);
	return(recorded);
}


/*
 * check for joint limits
 */

#define BOUND   10. * DEGTORAD
#define SAFEV   20.

static check() /*##*/
{
	real bound = BOUND;

	if (*doom == '\0') {
		if (j6->th1 < bound || j6->th1 > jrng_c.th1 - bound ||
		    j6->th2 < bound || j6->th2 > jrng_c.th2 - bound ||
		    j6->th3 < bound || j6->th3 > jrng_c.th3 - bound ||
		    j6->th4 < bound || j6->th4 > jrng_c.th4 - bound ||
		    j6->th5 < bound || j6->th5 > jrng_c.th5 - bound ||
		    j6->th6 < bound || j6->th6 > jrng_c.th6 - bound) {
			nextmove = YES;
			doom = "next to limit(s)\n";
		}
	}
	else {
		doom = "";
	}
	if (FABS(jd->th1 * SAFEV) > jmxv_c.th1 ||
	    FABS(jd->th2 * SAFEV) > jmxv_c.th2 ||
	    FABS(jd->th3 * SAFEV) > jmxv_c.th3 ||
	    FABS(jd->th4 * SAFEV) > jmxv_c.th4 ||
	    FABS(jd->th5 * SAFEV) > jmxv_c.th5 ||
	    FABS(jd->th6 * SAFEV) > jmxv_c.th6) {
		nextmove = YES;
		doom = "not so fast\n";
	}
}



/*
 * display current settings
 */

static display() /*##*/
{
	printf("T6:\n");
	printr(t6, stdout);
	printf("E:\n");
	printr(e, stdout);
	printf("veloc t:%d r:%d\n", vts, vrs);
	dial(j6->th1, jrng_c.th1, '1');
	dial(j6->th2, jrng_c.th2, '2');
	dial(j6->th3, jrng_c.th3, '3');
	dial(j6->th4, jrng_c.th4, '4');
	dial(j6->th5, jrng_c.th5, '5');
	dial(j6->th6, jrng_c.th6, '6');
	putchar('\n');
	if (flimp) {
		printf(
"force limits :fx %-5.2f  fy %-5.2f  fz %-5.2f   fX %-5.2f  fY %-5.2f  fZ %-5.2f
		flx, fly, flz, tlx, tly, tlz);
	}
	else {
		printf("no force limit\n");
	}
	printf("mass of object : %f kg\n", object);
}

#define WIDTH 12

/*
 * display joint positions within their ranges
 */

static dial(v, m, c) /*##*/
real v, m;
int c;
{
	int i, p;

	v -= BOUND;
	m -= BOUND + BOUND;
	p = (v / m) * (WIDTH - 1);

	for (i = 0; i < WIDTH; ++i) {
		putchar((i == p) ? c : '_');
	}
	putchar(' ');
}


/*
 * analyze command line, return world and tool modifications.
 * return :
 *              S       stop
 *              q       quit
 *              x       forced exit
 *              r       record
 *              p       print
 *              s       save
 *              e       error
 *              n       do nothing
 *              y       parse ok, intructions in globals
 */

static char coml[80], *c;

static parse() /*##*/
{
	putchar('?');
 (void) gets(coml);
	if (strlen(coml) == 0) {
		return('S');
	}
	for (c = coml; *c != '\0';) {
		while (*c != '\0' && isspace(*c)) {
			++c;
		}
		switch (*c) {
		case 'q' :
			++c;
			if (*c == '!') {
				return('x');
			}
			else {
				return('q');
			}

		case 'r' :
			return('r');

		case 'p' :
			return('p');

		case 's' :
			return('s');

		case 'o' :
			++c;
			OPEN;
			break;

		case 'c' :
			++c;
			CLOSE;
			break;

		case 'w' :
			switch (*++c) {
			case 'x' :
				Trslm(wtc, getreal(), 0., 0.);
				break;

			case 'y' :
				Trslm(wtc, 0., getreal(), 0.);
				break;

			case 'z' :
				Trslm(wtc, 0., 0., getreal());
				break;

			case 'X' :
				Rotm(wrc, xunit, getreal());
				break;

			case 'Y' :
				Rotm(wrc, yunit, getreal());
				break;

			case 'Z' :
				Rotm(wrc, zunit, getreal());
				break;

			default :
				err();
				return('e');
			}
			break;

		case 't' :
			switch (*++c) {
			case 'x' :
				Trslm(ttc, getreal(), 0., 0.);
				break;

			case 'y' :
				Trslm(ttc, 0., getreal(), 0.);
				break;

			case 'z' :
				Trslm(ttc, 0., 0., getreal());
				break;

			case 'X' :
				Rotm(trc, xunit, getreal());
				break;

			case 'Y' :
				Rotm(trc, yunit, getreal());
				break;

			case 'Z' :
				Rotm(trc, zunit, getreal());
				break;

			default :
				err();
				return('e');
			}
			break;

		case 'e' :
			switch (*++c) {
			case 'x' :
				Trslm(e, getreal(), 0., 0.);
				break;

			case 'y' :
				Trslm(e, 0., getreal(), 0.);
				break;

			case 'z' :
				Trslm(e, 0., 0., getreal());
				break;

			case 'X' :
				Rotm(e, xunit, getreal());
				break;

			case 'Y' :
				Rotm(e, yunit, getreal());
				break;

			case 'Z' :
				Rotm(e, zunit, getreal());
				break;

			default :
				err();
				return('e');
			}
			break;

		case 'f' :
			switch (*++c) {
			case 'x' :
				flx = getreal();
				break;

			case 'y' :
				fly = getreal();
				break;

			case 'z' :
				flz = getreal();
				break;

			case 'X' :
				tlx = getreal();
				break;

			case 'Y' :
				tly = getreal();
				break;

			case 'Z' :
				tlz = getreal();
				break;

			default :
				err();
				return('e');
			}
			break;

		case 'l' :
			flimp = !flimp;
			return('n');

		case 'm' :
			object = getreal();
			return('n');

		case 'v' :
			vts = getreal();
			vrs = getreal();
			return('n');

		case '?' :
			help();
			return('n');

		case '\0':
			break;

		default :
			err();
			return('e');
		}
	}
	return('y');
}


/*
 *                  -------        -< d >-
 *                \|       |     \|       |
 *                 --< d >---< . >---------
 *       -< + >-  |        |              |
 *      |       | |         --------------|/
 *   >--------------< . >----< d >-------------->
 *      |       |          |\      |
 *       -< - >-            -------
 */

static double getreal() /*##*/
{
	char *f;
	double v;

	++c;
	while (isspace(*c)) {
		++c;
	}
	f = c;
	switch (*c) {
	case '\0' :
		err();
		return(0.);

	case '+' :
		++c;
		break;

	case '-' :
		++c;
		break;
	}
	switch (*c) {
	case '\0' :
		err();
		return(0.);

	case '.' :
		++c;
		if (!isdigit(*c)) {
			err();
			return(0.);
		}
		while (isdigit(*c)) {
			++c;
		}
		break;

	default :
		if (!isdigit(*c)) {
			err();
			return(0.);
		}
		while (isdigit(*c)) {
			++c;
		}
		if (*c == '.') {
			++c;
			while(isdigit(*c)) {
				++c;
			}
		}
		break;
	}
	v = atof(f);
	if (FABS(v) > 10000) {
		err();
		v = 0.;
	}
	return(v);
}



static err() /*##*/
{
	int n;

	*c = '\0';
	n = strlen(coml) + 1;
	while (n--) {
		putchar(' ');
	}
	putchar('^');
	putchar('\n');
}



static help() /*##*/
{
	static char msg[] =
"\
These commands are executed one per line:\n\
\t<return>\tinterrupt arm motion\n\
\tq\t\tquit\n\
\tq!\t\tquit, ignore not recorded\n\
\tr\t\trecord transform\n\
\tp\t\tdisplay current settings\n\
\ts\t\tsave transform on data base\n\
\tl\t\ttoggle force monitor\n\
\tv<t r>\t\tset velocities\n\
\tm<m>\t\tset mass of object\n\
\t?\t\tthis message\n\
These commands cumulate:\n\
\to\t\t\topen hand\n\
\tc\t\t\tclose hand\n\
\tw<x/y/z/X/Y/Z><k>\tmove world coordinates\n\
\tt<x/y/z/X/Y/Z><k>\tmove tool coordinates\n\
\te<x/y/z/X/Y/Z><k>\tchange tool transform\n\
\tf<x/y/z/X/Y/Z><k>\tset force limits\n\
";
	printf("%s", msg);
}
