#include "../h/which.h"
#include "../h/addefs.h"
#include "../h/jcom.h"
#include "../h/unim.h"
#include "../h/rtc.h"
#include "../h/adc.h"

#define HNDPROC         6

static short ibuff[COMSIZE];    /* input buffer */
static struct how how;          /* output buffer */
static short ref[NJOINTS];
static short idx[NJOINTS];
static short calreq;
static short first;
#ifdef ADC
static short adcmap[ADC];
static short nch;
#endif
#ifdef VALII
static short rate[6] = {2, 4, 8, 16, 32, 64};
#else
static short rate[6] = {1, 2, 4, 8, 16, 32};
#endif
static short ninter[6] = {8, 16, 32, 64, 128, 256};
static short intscl[6] = {5, 4, 3, 2, 1, 0};

#define GETSHORT        value = *scan++; value <<= 8; value |= *scan++ & 0xff;

short drive()
{
	register char *scan = (char *)ibuff;
	register unsigned short value = 0;
	register short i = 0;
	unsigned short chks = 0;
	short wc, dr11();
	short *sp;
	unsigned short set[6];

	extern short tick;

/*
 * gather data
 */

	how.exio = getio(-1);
	get6j(READ, how.pos);
#ifdef ADC
	for (i = 0; i < ADC; ++i) {
		if (adcmap[i] >= 0) {
			ADCV(how.adcr[adcmap[i]], i);
		}
	}
#endif
	sp = (short *)how.pos;
	chks = how.exio;
	for (i = 0; i < NJOINTS; ++i) {
		 chks += *sp++;
	}
#ifdef ADC
	for (i = 0; i < nch; ++i) {
		 chks += *sp++;
	}
#endif
	*sp = chks;
/*
 * interrupt vax send data, get commands
 */
#ifdef ADC
	if ((wc = dr11(ibuff, &how, 1 + NJOINTS + nch + 1)) < 0) {
		zexit(wc);
	}
#else
	if ((wc = dr11(ibuff, &how, 1 + NJOINTS + 1)) < 0) {
		zexit(wc);
	}
#endif
	if (wc == 0) {
		return;
	}
/*
 * analyze commands and operate the arm
 */
	if (first) {
		first = NO;
		if (ibuff[0] != VERSION)
			zexit(13);
		if (ibuff[1] != ARMTYPE)
			zexit(14);
#ifdef STAN
		putp(MODE, MESERVO, 6);
#endif
		for (i = 0; i < NJOINTS; ++i) {
			ref[i] = ibuff[i + 2];
			idx[i] = ibuff[i + 2 + NJOINTS];
		}
		put6j(STOPP, /* dummy arg to satisfy macro */ set);
		put6pg(MODE, MESERVO);
#ifdef PUMA
		putp(MODE, MESERVO | MEHLFCT, 5);
#endif
		return;
	}
	i = wc - 1;
	sp = ibuff;
	chks = 0;
	while(i--) {
		chks += *sp++;
	}
	if (*sp != chks) {
		zexit(12);
	}
	*sp = -1;

	scan = (char *)ibuff;

	while (*scan != -1) {

		if ((i = (chks = *scan) & 0x0f) <= CMD_I) {
			++scan;
			GETSHORT

			switch (chks & 0x0f0) {
			case STOPP_I :
				putj(STOPP, value, i);
				break;

			case STOPM_I :
				putj(STOPM, value, i);
				break;

			case POS_I :
				putj(POSET, value, i);
				break;

			case CUR_I :
				putj(DACSET, value, i);
				break;

			default :
				zexit(15);
			}
			continue;
		}

		switch ((chks = *scan) & 0x0f) {

		case CMD_A :
			scan++;
			GETSHORT
			set[0] = value;
			GETSHORT
			set[1] = value;
			GETSHORT
			set[2] = value;
			GETSHORT
			set[3] = value;
			GETSHORT
			set[4] = value;
			GETSHORT
			set[5] = value;

			switch (chks & 0xff) {

			case STOPP_A :
				put6j(STOPP, set);
				break;

			case STOPM_A :
				put6j(STOPM, set);
				break;

			case POS_A :
				put6j(POSET, set);
				break;

			case CUR_A :
				put6j(DACSET, set);
				break;

			default :
				zexit(16);
			}
			break;

		case CMD_G :
			++scan;
			GETSHORT
			switch (chks & 0xff) {

#ifdef ADC
			case ADCO_G :
				adcmap[value] = nch++;
				break;
#endif

			case HAND_G :
#ifdef PUMA
				if(value == 'o') {
					bicio(HNDOH);
				}
				if (value == 'c') {
					bisio(HNDOH);
				}
#endif
#ifdef STAN
				putj(DACSET, value, HNDPROC);
#endif
				break;

			case RATE_G :
				if (value < 6) {
					tick = rate[value];
					put6pg(NINTER, ninter[value]);
					put6pg(INTSCL, intscl[value]);
				}
				break;

			default :
				zexit(17);
			}

			break;

		case CMD_E :
			switch (*scan & 0xff) {

			case STOP_E :
				put6jg(STOPP, 0);
				break;

			case CALIB_E :
				calreq = YES;
				break;

			case END_E :
				tick = 0;
				break;

			default :
				zexit(18);
			}
			scan += 1;
			break;

		default :
			zexit(19);
		}
	}
}


#ifdef DEBUG
#include "../h/stdio11.h"
#endif

main()
{
	extern short (* onclk)();
	extern short tick;
	extern short drive();
	int i;

#ifdef DEBUG
	setbuf(stdin, NULL);
	setbuf(stdout, NULL);
#endif
	for (; ; ) {
		first = YES;
		calreq = NO;
#ifdef ADC
		for (i = 0; i < NJOINTS; ++i) {
			adcmap[i] = i;
		}
		for (i = NJOINTS; i < ADC; ++i) {
			adcmap[i] = -1;
		}
		nch = NJOINTS;
#endif
		bicio(ARMPWR);
		bisio(ARMPWR);
		put6pg(MODE, MESERVO|MEPOSSV);
		put6pg(NINTER, ninter[DEFAULTRATE]);
		put6pg(INTSCL, intscl[DEFAULTRATE]);
		while (!getio(ARMPWR))
			;
#ifdef DEBUG
		printf("active\n");
#endif
		onclk = drive;
		tick = rate[DEFAULTRATE];
		while (tick)
			wait();
#ifdef DEBUG
		printf("iddle\n");
#endif
		if (calreq) {
			if (!cal(ref, idx)) {
				zexit(99);
			}
		}
		else {
			bicio(ARMPWR);
			for (i = 50000; --i; )
				;
		}
	}
}


zexit(n)
{
	tick = 0;
	*VAXCSR = 0;
	bicio(ARMPWR);
#ifdef DEBUG
	printf("\nexit %d\n", n);
#endif
	halt();
}
