/*
 * MAC  Version 2.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : mac
 *      File    : mcal.c
 *      Remarks : calibration function
 *      Usage   : part of maint and moper
 */

#include "../h/which.h"
#include "../h/addefs.h"
#include "../h/jcom.h"

#ifdef VALII
#define TACK    8
#else
#define TACK    4
#endif

#define YES     1
#define NO      0

extern short (* onclk)();
extern short tick;

unsigned short index[NJOINTS];
unsigned short calpos[NJOINTS];

static short timeout;

static short calstep;


static unsigned short calrefp[NJOINTS], calidxp[NJOINTS], off[NJOINTS];

cal(refp, idxp)
unsigned short *refp, *idxp;
{
	short armcal(), steper(), refarm();
	register short i;

	tick = 0;
	for (i = 0; i < NJOINTS; ++i) {
		calrefp[i] = refp[i];
		calidxp[i] = idxp[i];
		off[i] = YES;
	}
	put6j(STOPM, calrefp);
	get6j(READ, calpos);

	if (!getio(ARMPWR)) {
		bisio(ARMPWR);
		while(!getio(ARMPWR))
		       ;
	}

	put6pg(MODE, MESERVO|MEPOSSV);
	get6j(READ, calpos);

	onclk = steper;
	timeout = 7;
	calstep = -16;
	tick = TACK;
	while(tick) {
		wait();
	}
	put6pg(MODE, MESERVO|MEPOSSV|MLOOKZI);
	onclk = armcal;
	timeout = 250;
	calstep = 16;
	tick = TACK;
	while(tick) {
		wait();
	}
	if(timeout) {
		put6j(STOPM, calidxp);
		for (i = 0; i < NJOINTS; ++i) {
			calpos[i] = calidxp[i];
		}
		onclk = refarm;
		calstep = 16;
		tick = TACK;
		while (tick) {
			wait();
		}
		put6j(POSET, calrefp);
		return(YES);
	}
	else {
		return(NO);
	}
}


short armcal()
{
	register short i = NJOINTS;
	unsigned short stat[NJOINTS];

	if (--timeout) {
		get6j(JSTAT, stat);
		while (i > 0 && stat[--i] & JFNDIXS)
		       ;
		if (i) {
			for(i = 0; i < NJOINTS; ++i)
				calpos[i] += calstep;
			put6j(POSET, calpos);
		}
		else {
			tick = 0;
		}
	}
	else {
		tick = 0;
	}
}


short steper()
{
	register short i;

	if (--timeout) {
		for(i = 0; i < NJOINTS; ++i)
			calpos[i] += calstep;
		put6j(POSET, calpos);
	}
	else {
		tick = 0;
	}
}



short refarm()
{
	short i, done = YES;

	for (i = 0; i < NJOINTS; ++i) {
		if (off[i]) {
			done = NO;
		}
		else {
			continue;
		}
		if (calidxp[i] > calrefp[i]) {
			calpos[i] -= calstep;
			if (calpos[i] < calrefp[i]) {
				off[i] = NO;
			}
		}
		if (calidxp[i] < calrefp[i]) {
			calpos[i] += calstep;
			if (calpos[i] > calrefp[i]) {
				off[i] = NO;
			}
		}
		if (calidxp[i] == calrefp[i]) {
			off[i] = NO;
		}
	}
	put6j(POSET, calpos);
	if (done) {
		tick = 0;
	}
}
