/*
 * MAC  Version 2.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : mac
 *      File    : moper.c
 *      Remarks : superviser  program
 *      Usage   : make install
 *
 *	modified: added asleep external variable see vec.s
 *		  for details
 */

#include "../h/which.h"
#include "../h/addefs.h"
#include "../h/jcom.h"
#include "../h/cmdk.h"
#include "../h/fifoio.h"
#include "../h/rtc.h"
#include "../h/adc.h"
#include "../h/stdio11.h"

#define DEBUG
#define HNDPROC         6

#define SMOOTH          8

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

static struct fifobuf cmdbuf;

static short calreq;
static short first;
static short ntest;
static short ref[NJOINTS];
static short idx[NJOINTS];
#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};

static jst[NJOINTS];
static ldac[NJOINTS];
static ddac[NJOINTS];
int buf;

short drive()
{
	register short *scan;
	register short i = 0;
	register unsigned short chks = 0;
	unsigned short set[NJOINTS];

	extern short tick;
	extern short asleep;


	if (ntest) {
		for (i = 0; i < FIFOBUFS; ++i) {
			how.howb.data[i] = ID;
			if (cmdbuf.data[i] != OD) {
				zexit(10);
			}
		}
		how.howb.wc = FIFOBUFS;
		if ((i = dr11(&cmdbuf, (struct fifobuf *)&how)) < 0) {
			zexit(i - 100);
		}
		--ntest;
		return;
	}
/*
 * gather data
 */

	how.howw.exio = getio(-1);
	get6j(READ, how.howw.pos);

#ifdef ADC
	for (i = 0; i < ADC; ++i) {
		if (adcmap[i] >= 0) {
			ADCV(how.howw.adcr[adcmap[i]], i);
		}
	}
#endif
	scan = (short *)how.howw.pos;
	chks = how.howw.exio;
	for (i = 0; i < NJOINTS; ++i) {
		 chks += *scan++;
	}
#ifdef ADC
	for (i = 0; i < nch; ++i) {
		 chks += *scan++;
	}
#endif
	*scan = chks;
/*
 * interrupt vax send data, get commands
 */
#ifdef ADC
	how.howw.wc = 1 + NJOINTS + nch + 1;
#else
	how.howw.wc = 1 + NJOINTS + 1;
#endif
	if ((i = dr11(&cmdbuf, (struct fifobuf *)&how)) < 0) {
		zexit(i);
	}
	if (cmdbuf.wc == 0) {
		return;
	}

/*
 * analyze commands and operate the arm
 */
	if (first) {
		first = NO;
		if (cmdbuf.data[0] != VERSION)
			zexit(13);
		if (cmdbuf.data[1] != ARMTYPE)
			zexit(14);
		for (i = 0; i < NJOINTS; ++i) {
			ref[i] = cmdbuf.data[i + 2];
			idx[i] = cmdbuf.data[i + 2 + NJOINTS];
		}
#ifdef PUMA
		putp(MODE, MESERVO | MEHLFCT, 5);
#endif
		return;
	}
	i = cmdbuf.wc - 1;
	scan = cmdbuf.data;
	chks = 0;
	while(i--) {
		chks += *scan++;
	}
	if (*scan != chks) {
		zexit(12);
	}
	*scan = -1;

	scan = cmdbuf.data;

	while (*scan != -1) {

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

			switch (chks & 0x0ff00) {
			case STOPP_I :
				putj(STOPP, *scan++, i);
				break;

			case STOPM_I :
				putj(STOPM, *scan++, i);
				break;

			case POS_I :
				if (jst[i]) {
					--(jst[i]);
					putj(GRAVTY, ldac[i], i);
					ldac[i] -= ddac[i];
				}
				putj(POSET, *scan++, i);
				break;

			case CUR_I :
				ldac[i] = *scan++;
				putj(DACSET, ldac[i], i);
				jst[i] = SMOOTH;
				ldac[i] >>= 1;
				ddac[i] = ldac[i] >> 3;
				break;

			case GRAV_I :
				putj(GRAVTY, *scan++, i);
				break;

			default :
				zexit(15);
			}
			continue;
		}

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

		case CMD_A :
			scan++;
			for (i = 0; i < NJOINTS; ++i) {
				set[i] = *scan++;
			}
			switch (chks) {

			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;

			case GRAV_A :
				put6j(GRAVTY, set);
				break;

			default :
				zexit(16);
			}
			break;

		case CMD_G :
			++scan;
			switch (chks) {
#ifdef ADC
			case ADCO_G :
				adcmap[*scan++] = nch++;
				break;
#endif

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

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

			default :
				zexit(17);
			}

			break;

		case CMD_E :
			switch (*scan++) {

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

			case CALIB_E :
				calreq = YES;
				break;

			case END_E :
				tick = 0;
				break;

			default :
				zexit(18);
			}
			break;

		default :
			zexit(19);
		}
	}
}


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

main()
{
	extern short (* onclk)();
	extern short tick;
	extern short asleep;
	extern short drive();

	register int i;
	register struct drcdevice *dp = DRC_ADDR;
	short dummy[6];

#ifdef DEBUG
	setbuf(stdin, NULL);
	setbuf(stdout, NULL);
#endif
	for (; ; ) {
		first = YES;
		calreq = NO;
		ntest = NTEST;
		for (i = 0; i < FIFOBUFS; ++i) {
			cmdbuf.data[i] = OD;
		}
#ifdef ADC
		for (i = 0; i < NJOINTS; ++i) {
			adcmap[i] = i;
		}
		for (i = NJOINTS; i < ADC; ++i) {
			adcmap[i] = -1;
		}
		nch = NJOINTS;
#endif
		dp->drccsr = 0;
		while ((dp->drccsr & FIFO_EMPTY) == 0) {
			i = dp->drcbuf;
		}

		for (i = 0; i < NJOINTS; ++i) {
			jst[i] = 0;
		}

		bicio(ARMPWR);
		bisio(ARMPWR);
		put6pg(MODE, MESERVO|MEPOSSV);
		put6pg(NINTER, ninter[DEFAULTRATE]);
		put6pg(INTSCL, intscl[DEFAULTRATE]);
#ifdef STAN
		putp(MODE, MESERVO, 6);
#endif
		put6j(STOPP, dummy);
		put6pg(MODE, MESERVO);
#ifdef PUMA
		putp(MODE, MESERVO | MEHLFCT, 5);
#endif
		while (!getio(ARMPWR)) {
			get6j(READ, how.howw.pos);
		}
#ifdef DEBUG
		printf("active\n");
#endif
		onclk = drive;
		tick = rate[DEFAULTRATE];
		while (tick)
			wait();
#ifdef DEBUG
		printf("iddle\nref idx:\n");
		for (i = 0; i < NJOINTS; ++i) {
			printf("%u %u ", ref[i], idx[i]);
		}
		putchar('\n');
#endif
		if (calreq) {
			if (!cal(ref, idx)) {
				zexit(99);
			}
		}
		else {
			for (i = 50000; --i; )
				;
			bicio(ARMPWR);
			for (i = 50000; --i; )
				;
		}
	}
}


zexit(n)
{
	register struct drcdevice *dp = DRC_ADDR;

	asleep = '1';
	tick = 0;
	dp->drccsr = 0;
	bicio(ARMPWR);
#ifdef DEBUG
	printf("\nexit %d\n", n);
#endif
	halt();
}
