/*
 * RTC  Version 2.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : rtc
 *      File    : pack.c
 *      Remarks : Contains all the robot interface: channel setup,
 *                data checking, command translation, arm monitoring, and
 *                user interface.
 *                Also the manual step mode and home return.
 *      Usage   : part of the library
 */

/*LINTLIBRARY*/

#include <stdio.h>
#include <sys/types.h>
#include <sys/drc.h>
#include <signal.h>
#include <sgtty.h>
#include "../h/exiod.h"
#include "../h/cmdk.h"
#include "../h/fifoio.h"
#include "../h/which.h"
#include "../h/rtc.h"
#include "../h/umac.h"
#ifdef PUMA
#include "../h/pumadata.h"
#endif
#ifdef STAN
#include "../h/standata.h"
#endif


struct chg chg;         /* global control  structure    */

static struct fifobuf cmdbuf = {2, VERSION, ARMTYPE};

union howu {
	struct fifobuf howb;
	struct how     howw;
} how;

int terminate = NO;     /* shuts up the pack routine    */
char *mess;             /* printed by release           */
int Magic_Circus = YES; /* secret entry point           */


static unsigned short ref[NJOINTS] = {ECCL1, ECCL2, ECCL3, ECCL4, ECCL5, ECCL6};
static unsigned short idx[NJOINTS] = {XCCL1, XCCL2, XCCL3, XCCL4, XCCL5, XCCL6};
static unsigned short max[NJOINTS] = {ECMX1, ECMX2, ECMX3, ECMX4, ECMX5, ECMX6};
static unsigned short min[NJOINTS] = {ECMN1, ECMN2, ECMN3, ECMN4, ECMN5, ECMN6};
static unsigned short mvl[NJOINTS] = {EMXV1, EMXV2, EMXV3, EMXV4, EMXV5, EMXV6};
static unsigned short mxdc[NJOINTS] = {MXDC1, MXDC2, MXDC3, MXDC4, MXDC5, MXDC6}
static unsigned short mxoc[NJOINTS] = {MXOC1, MXOC2, MXOC3, MXOC4, MXOC5, MXOC6}
#ifdef ADC
static int nch = NJOINTS;
static int adcmap[ADC] =
		{0, 1, 2, 3, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};
#endif
static int rate;
static int inserf = NO;
static int active = NO;
static int first = YES;
static int cbrkset = NO;
static int checking = YES;
static struct sgttyb mode;

/*
 * set of macros to test requests contained in the chg structure
 * and fill the output buffer
 */

#define IRQST(x)        *(k = &chg. x [i].set)

#define STUFFI(x,C)     if (stack > top - 4)\
				goto fatal;\
			*stack++ = C | i;\
			*stack++ = chg. x [i].vali;\
			*k = NO;

#define CMDI(x,C)       if(IRQST(x)) {STUFFI(x,C)}

#define ARQST(x)        *(k = &chg. x .set)

#define STUFFA(x,C)     if (stack > top - NJOINTS + 2)\
				goto fatal;\
			*stack++ = C;\
			for (i = 0; i < NJOINTS; ++i) {\
				*stack++ = chg. x .vala[i];\
			} *k = NO;

#define CMDA(x,C)       if(ARQST(x)) {STUFFA(x,C)}

#define GRQST(x)        *(k = &chg. x .set)

#define STUFFG(x,C)     if (stack > top - 4)\
				goto fatal;\
			*stack++ = C;\
			*stack++ = chg. x .valg;\
			*k = NO;

#define CMDG(x,C)       if(GRQST(x)) {STUFFG(x,C)}

#define RQSTE(x)        *(k = &chg. x)

#define STUFFE(C)       if (stack > top - 2)\
				goto fatal;\
			*stack++ = C;\
			*k = NO;

#define CMDE(x,C)       if (RQSTE(x)) {STUFFE(C)}

/*
 * the pack function is called after execution of the interrupt
 * user function 1 to translates the requests expressed in the
 * chg structure into the communication language understandable
 * by the lsi code.
 * If for a reason or another, the flag terminate is raised,
 * the user interrupt functions will no longer be called and
 * pack will refuse to send anything but a rate command witc neg val which
 * cause the lsi code to stop interrupting the vax and turn off
 * the arm power.
 * If the power is turned off, interrupts are ignored and execution
 * resume when power is back on.
 * To add more commands, simply add more chg strcture entries, corresponding
 * macro calls in pack, resets in control and interpretations in moper.
 */


static pack() /*##*/
{
	register int i;
	register char *k;
	register short *stack = cmdbuf.data;
	register short *top = cmdbuf.data + FIFOBUFS;
	register int inc;
	register unsigned short mcc;

	if (terminate) {
terminated:     chg.end = YES;
		CMDE(end, END_E)
		return(stack - cmdbuf.data);    /* return word count */
	}

	for (i = 0; i < NJOINTS; ++i) {
		switch (IRQST(i_motion)) {
		case NO :
			break;

		case POS :
			inc = chg.i_motion[i].vali - how.howw.pos[i];
			inc = (inc < 0) ? -inc : inc;
			if (checking) {
				if (inc > mvl[i] * rate) {
					goto toofast;
				}
				inc = chg.i_motion[i].vali;
				if (inc > max[i] || inc < min[i]) {
					goto toofar;
				}
			}
			STUFFI(i_motion,POS_I)
			break;

		case CUR :
			mcc = chg.i_motion[i].vali;
			mcc = (mcc > 0100000) ? -mcc : mcc;
			if (mcc > mxdc[i]) {
				goto toostrong;
			}
			STUFFI(i_motion,CUR_I)
			break;

		case STOP :
			STUFFI(i_motion,STOPP_I);
			break;

		case STOPCAL :
			STUFFI(i_motion,STOPM_I);
			break;
		default :
			goto fatal;
		}
		CMDI(i_gravty,GRAV_I);
	}

	switch (ARQST(a_motion)) {
	case NO :
		break;

	case POS :
		if (checking) {
			for (i = 0; i < NJOINTS; ++i) {
				inc = chg.a_motion.vala[i] - how.howw.pos[i];
				inc = (inc < 0) ? -inc : inc;
				if (inc > mvl[i] * rate) {
					goto toofast;
				}
				inc = chg.a_motion.vala[i];
				if (inc > max[i] || inc < min[i]) {
					goto toofar;
				}
			}
		}
		STUFFA(a_motion,POS_A)
		break;

	case CUR :
		for (i = 0; i < NJOINTS; ++i) {
			mcc = chg.a_motion.vala[i];
			mcc = (mcc > 0100000) ? -mcc : mcc;
			if (mcc > mxdc[i]) {
				goto toostrong;
			}
		}
		STUFFA(a_motion,CUR_A);
		break;
	case STOP :
		STUFFA(a_motion,STOPP_A);
		break;
	case STOPCAL :
		STUFFA(a_motion,STOPM_A);
		break;
	default :
		goto fatal;
	}

	CMDA(a_gravty, GRAV_A);

	CMDG(g_hand, HAND_G);
#ifdef ADC
	if (chg.g_adco.set) {
		++nch;
	}
	CMDG(g_adco, ADCO_G)
#endif
	CMDG(g_rate, RATE_G)
	if (chg.g_rate.set) {
		rate = (1 << chg.g_rate.valg) * HARDCLOCK;
	}
	CMDE(stop, STOP_E)
	CMDE(calib, CALIB_E)

	return(stack - cmdbuf.data);    /* return word count */

fatal :
	stack = cmdbuf.data;
	mess = "** too many commands";
	terminate = YES;
	goto terminated;

toofast :
	stack = cmdbuf.data;
	terminate = YES;
	mess = "** too large position increment";
	goto terminated;

toofar :
	stack = cmdbuf.data;
	terminate = YES;
	mess = "** out of range";
	goto terminated;

toostrong :
	stack = cmdbuf.data;
	terminate = YES;
	mess = "** too much current";
	goto terminated;
}


/*
 * - set up the driver
 * - have the SIGINT caught to terminate interaction
 * - have the SIGHUP caught on device error
 *
 * On IT :
 * 1) The driver gets a buffer from the controller
 * 2) User fn1 is called and is meant to process the arm state
 * 3) Commands are put in buffer by pack
 * 3) The driver sends a buffer to the controller (previous commands)
 * 4) User fn2 is called and is meant to compute next command
 *
 * One may code any sequence of 'control-release' in the user programs
 * Each of these sequence corresponds to a sequence
 * 'interrupt/send state/execute commands - turn off armpower in the lsi'
 */

static dummy(){} /*##*/

#define BAD     2
#define TOOF    3

static struct drcROBOT drcROBOT  = {
	(caddr_t) &cmdbuf,      /* this write buffer */
	(caddr_t) &how,         /* this is read buffer */
	dummy,
	dummy
};


static int fdr;
static int (* userfn1)();
static int (* userfn2)();
static int exch  = 0;
static int ntest = NTEST;
static int hang;

control(fn1, fn2) /*::*/
int (* fn1)(), (* fn2)();
{
	int count;
	int release();
	int ondrcerr();
	int onintr1();
	int onintr2();
	char *ttyname();

	if (active) {
		terminate = YES;
		mess = "** exit";
		release("** channel not released");
		exit(4);
	}
	/*
	#ifdef PUMA
		if (strcmp(ttyname(0), "/dev/tty02") != 0) {
			printf("not the Puma's terminal\n");
			exit(5);
		}
	#endif
	#ifdef STAN
		if (strcmp(ttyname(0), "/dev/tty03") != 0) {
			printf("not the Stanford arm's terminal\n");
			exit(5);
		}
#endif
	*/
	userfn1 = fn1;
	userfn2 = fn2;
	drcROBOT.R_routine = onintr1;
	drcROBOT.R_routine2 = onintr2;

 (void) signal(SIGINT, release);
 (void) signal(SIGHUP, ondrcerr);

	terminate = NO;
	active = YES;
	hang = YES;
	mess = "";
	exch = 0;
	ntest = NTEST;
	rate = (1 << DEFAULTRATE) * HARDCLOCK;
	checking = Magic_Circus;

	for (count = 0; count < NJOINTS; ++count) {
		chg.i_motion[count].set = NO;
	}
	chg.a_motion.set = NO;
	chg.g_adco.set = NO;
	chg.g_hand.set = NO;
	chg.g_rate.set = NO;
	chg.end = NO;
	chg.stop = NO;
	chg.calib = NO;
#ifdef PUMA
	if ((fdr = open("/dev/drc0", 2)) < 0) {
		printf("Can't open /dev/drc0 for write\n");
		exit(1);
	}
#endif
#ifdef STAN
	{
		printf("Interface not implemented\n");
		exit(1);
	}
#endif
	if ((count = write(fdr, (char *)&drcROBOT,
	 sizeof (struct drcROBOT))) != sizeof (struct drcROBOT)) {
		printf("write error initing drcROBOT, count:%d\n", count);
		exit(2);
	}
	printf("** channel opened, turn on ARM POWER\n");
	while (hang) {
		nap(10);
	}
}

adcopen(ch) /*::*/
int ch;
{
#ifdef ADC
	if (!active || ch < NJOINTS || ch >= ADC || adcmap[ch] >= 0) {
		return(-1);
	}
	chg.g_adco.set = YES;
	chg.g_adco.valg = ch;
	while (chg.g_adco.set) {
		nap(1);
	}
	return(adcmap[ch] = nch - 1);
#else
	return(-1);
#endif
}




static onintr1() /*##*/
{
	register short chks;
	register short *sp;
	register int i;
	register short inc;
	static short old[NJOINTS];

	if (inserf) {
		mess = "** interrupt occurred before end of user function";
		terminate = TOOF;
		return;
	}
	inserf = YES;
	if (terminate == TOOF || terminate == BAD) {
		inserf = NO;
		return;
	}
	if (ntest) {
		--ntest;
		for (i = 0; i < FIFOBUFS; ++i) {
			cmdbuf.data[i] = OD;
			if (how.howb.data[i] != (short)ID) {
				cmdbuf.data[0] = 0;
				mess = "**** hardware failure";
				terminate = BAD;
				return;
			}
		}
		cmdbuf.wc = FIFOBUFS;
		hang = YES;
		return;
	}
	sp = (short *)how.howw.pos;
	chks = how.howw.exio;
	for (i = 0; i < NJOINTS; ++i) {
		chks += *sp++;
	}
#ifdef ADC
	for (i = 0; i < nch; ++i) {
		chks += *sp++;
	}
#endif
	if(*sp != chks) {
		terminate = BAD;
		mess = "** bad checksum";
		return;
	}
	if (checking) {
#ifdef ADC
		for (i = 0; i < NJOINTS; ++i) {
			inc = how.howw.adcr[i];
			inc = (inc < 0) ? -inc : inc;
			if (inc > mxoc[i]) {
				mess = "** too large observed current";
				terminate = YES;
				break;
			}
		}
#endif
		if (exch == 0) {
			for (i = 0; i < NJOINTS; ++i) {
				old[i] = how.howw.pos[i];
			}
		}
		else {
			for (i = 0; i < NJOINTS; ++i) {
				inc = how.howw.pos[i] - old[i];
				inc = (inc < 0) ? -inc : inc;
				if (inc > mvl[i] * rate) {
					mess = "** too large observed velocity";
					terminate = YES;
					break;
				}
				inc = old[i] = how.howw.pos[i];
				if (inc > max[i] || inc < min[i]) {
					mess = "** joint(s) out of range";
					terminate = YES;
					break;
				}
			}
		}
	}
	if (exch != 0) {
		if (!(terminate || (hang = !(how.howw.exio & ARMPWR)))) {
			(* userfn1)();  /* process arm state */
		}
		i = cmdbuf.wc = pack();     /* word count */
		chks = 0;
		for (sp = cmdbuf.data; i--;)
			chks += *sp++;
		*sp = chks;
		++cmdbuf.wc;                /* for chks   */
	}
	else {
		cmdbuf.wc = 2 + 2 * NJOINTS;
		cmdbuf.data[0] = VERSION;
		cmdbuf.data[1] = ARMTYPE;
		for (i = 0; i < NJOINTS; ++i) {
			cmdbuf.data[i + 2] = ref[i];
			cmdbuf.data[i + 2 + NJOINTS] = idx[i];
		}
	}
	++exch;
}



static onintr2() /*##*/
{
	if (terminate) {
		inserf = NO;
		return;
	}
	if (!hang) {
		(* userfn2)();  /* command arm */
	}
	inserf = NO;
}


static ondrcerr() /*##*/
{
 (void) close(fdr);
	printf("**** driver error...exch = %d\n", exch);
	printf("\nterminate %d\n", terminate);
	exit(3);
}


release(s)
char *s;
{
	int c;

	terminate = YES;
	active = NO;
	nap(10);
#ifdef ADC
	nch = NJOINTS;
	for (c = NJOINTS; c < ADC; ++c) {
		adcmap[c] = -1;
	}
#endif
 (void) close(fdr);
	if ((int)s == SIGINT) {
		printf("\n** Interrupted\n");
		printf("%s\n", mess);
	}
	else {
		printf("%s\n", s);
		printf("%s\n", mess);
	}
	printf("** exch = %d\n", exch);

	if ((int)s == SIGINT) {
		if (cbrkset) {
			mode.sg_flags &= ~CBREAK;
			ioctl(1, TIOCSETP, &mode);
		}
		printf("** attempt an automatic home return ? ");
		QUERY(c);
		if (c == 'y') {
			home();
		}
		exit(1);
	}
}


/*
 * manual stepping mode
 */

#define GO(x ,y) printf("%c=%c%c" ,'\033' ,' '+x ,' '+y)

#define CALREQ  2
#define BS      '\010'
#define CR      '\015'
#define MAXMAG 100
#define HANDSTEP 10

static int enough = NO;
static int mag = 0;
static int poshd = 0;
static unsigned short ideal[NJOINTS];
static int offset[NJOINTS] = {0, 0, 0, 0, 0, 0};



stepmode() /*::*/
{
	int dummy(), soft();
	int c;
	int sig = YES;

	enough = NO;
	first = YES;
	Magic_Circus = NO;
	mag = 0;
	poshd = 0;
	for (c = 0; c < NJOINTS; ++c) {
		offset[c] = 0;
	}

	if (gtty(1, &mode) < 0) {
		printf("** can't gtty\n");
		exit(1);
	}
	control(dummy, soft);
	while(first) {
		nap(10);
	}
	mode.sg_flags |= CBREAK;
	ioctl(1, TIOCSETP, &mode);
	cbrkset = YES;
	GO(22, 0);
	printf(
"faster(.) slower(,) reverse(-) joints[1-6] hand(o/c) exit(return)\n\n");
	printf(
"** exit ? (y/n) _  calibrate ? (y/n) _  sure ? (y/n) _");
	GO(22, 0);
	printf("*     ");
	for (c = 0; c < NJOINTS; ++c) {
		GO(22, ((c + 1) * 8));
		printf("%6u", how.howw.pos[c]);
	}
	while (!enough) {
		putchar(CR);
		putchar(':');
		c = getchar();
		putchar(BS);
		switch (c) {
		case '1' :
		case '2' :
		case '3' :
		case '4' :
		case '5' :
		case '6' :
			offset[c -= '1'] += (sig) ? mag : -mag;
			GO(22, ((c + 1) * 8));
			printf("%6u", how.howw.pos[c]);
			break;
#ifdef PUMA
		case 'o' :
			poshd = 'o';
			break;
		case 'c' :
			poshd = 'c';
			break;
#endif
#ifdef STAN
		case 'o' :
			poshd += HANDSTEP;
			break;
		case 'c' :
			poshd -= HANDSTEP;
			break;
#endif
		case '.' :
			(mag < MAXMAG) ? ++mag : mag;
			break;
		case ',' :
			(mag) ? --mag : mag;
			break;
		case '-' :
			sig = !sig;
			break;
		case '\n' :
			printf("** exit ? (y/n) ");
			if (getchar() == 'y') {
				printf("  calibrate ? (y/n) ");
				if (getchar() == 'y') {
					printf("  sure ? (y/n) ");
					if (getchar() == 'y') {
						chg.calib = YES;
						enough = CALREQ;
						nap(20);
					}
					else {
						GO(22, 0);
					}
				}
				else {
					printf("  sure ? (y/n) ");
					if (getchar() == 'y') {
						enough = YES;
					}
					else {
						GO(22, 0);
					}
				}
			}
			else {
				GO(22, 1);
			}
			break;
		default :
			break;
		}
	}
	mode.sg_flags &= ~CBREAK;
	ioctl(1, TIOCSETP, &mode);
	cbrkset = NO;
	release("\n** end of step mode");
	Magic_Circus = YES;
	if (enough == CALREQ) {
		printf("** calibrating\n");
		sleep(5);
	}
}



static soft() /*##*/
{
	register int i;

	if (enough) {
		return;
	}
	if (first) {
		first = NO;
		for (i = 0; i < NJOINTS; ++i) {
			ideal[i] = how.howw.pos[i];
		}
	}
	for (i = 0; i < NJOINTS; ++i) {
		if (how.howw.pos[i] <= MAXMAG) {
			ideal[i] = chg.i_motion[i].vali = 077777;
			chg.i_motion[i].set = STOPCAL;
		}
		if (how.howw.pos[i] >= 65535-MAXMAG) {
			ideal[i] = chg.i_motion[i].vali = 077777;
			chg.i_motion[i].set = STOPCAL;
		}
		if (offset[i] > 0) {
			offset[i] -= mag;
			ideal[i] -= mag;
		}
		if (offset[i] < 0) {
			offset[i] += mag;
			ideal[i] += mag;
		}
		chg.a_motion.vala[i] = ideal[i];
	}
	chg.a_motion.set = POS;
	chg.g_hand.valg = poshd;
	chg.g_hand.set = YES;
}


/*
 * Automatic home position return
 */

#define SMALL   100
#define STABTIM 50

static home() /*##*/
{
	int dummy(), back();

	first = YES;
	Magic_Circus = NO;
	printf("** attempting a home return\n");
	control(dummy, back);
	while (!terminate) {
		nap(10);
	}
	nap(10);
 (void) close(fdr);
	printf("** back home, calibrated\n");
	Magic_Circus = YES;
	nap(10);
}



static back() /*##*/
{
	static int finish;
	static int count;
	static unsigned short   where[NJOINTS],
				inc[NJOINTS],
				half[NJOINTS],
				done[NJOINTS];
	register int i;

	if (first) {
		first = NO;
		for (i = 0; i < NJOINTS; ++i) {
			where[i] = how.howw.pos[i];
			if (where[i] > ref[i]) {
				half[i] = (where[i] - ref[i]) / 2 + ref[i];
				half[i] -= SMALL;
			}
			else {
				half[i] = ref[i] - (ref[i] - where[i]) / 2;
				half[i] += SMALL;
			}
			finish = NO;
			count = STABTIM;
			inc[i] = 0;
		}
	}
	if (!finish) {
		for (i = 0, finish = YES; i < NJOINTS; ++i) {
			if (where[i] > ref[i]) {
				done[i] = where[i] - ref[i] < SMALL;
			}
			else {
				done[i] = ref[i] - where[i] < SMALL;
			}
			if (!done[i]) {
				finish = NO;
			}
		}
	}
	else {
		if (--count == 0) {
			terminate = YES;
		}
	}
	for (i = 0; i < NJOINTS; ++i) {
		if (!done[i]) {
			if (where[i] > ref[i]) {
				if (where[i] > half[i]) {
					--inc[i];
				}
				else {
					++inc[i];
				}
			}
			if (where[i] < ref[i]) {
				if (where[i] < half[i]) {
					++inc[i];
				}
				else {
					--inc[i];
				}
			}
		}
		else {
			where[i] = ref[i];
		}
		where[i] += inc[i];
		chg.a_motion.vala[i] = where[i];
	}
	chg.a_motion.set = POS;
}
