#include "../h/rccl.h"
#include "../h/rtc.h"
#include "../h/hand.h"
#include "../h/umac.h"

extern struct how how;
extern struct chg chg;
int sensor;

pumatask()
{
	TRSF_PTR z, k1, k2, finf, abov;
	TRSF_PTR b1, b2, b3, b4, b5, over, fing, getit, flip;
	POS_PTR  q1, q2, got;
	POS_PTR  p1, p2, p3, p4, p5, pp1, pp2, pp3, pp4, pp5, get;
	int touchfn();
	int fingfn();
	int q;

	finf = newtrans("FINK",fingfn);
	rot(finf, zunit, -90.);
	fing = gentr_rot("FINS", 0., 0., 0., zunit, -90.);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
	k1 = gentr_rot("K1", 600. ,-300., 450., yunit, 180.);
	k2 = gentr_rot("K2", 600. , 200., 450., yunit, 180.);
	abov = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
	b1 = gentr_trsl("B1", 600. ,-200., 300.);
	b2 = gentr_trsl("B2", 600. ,-100., 300.);
	b3 = gentr_trsl("B3", 600. , 000., 300.);
	b4 = gentr_trsl("B4", 600. , 100., 300.);
	b5 = gentr_trsl("B5", 600. , 200., 300.);
	over = gentr_rot("OVER", 0., 0., 100., yunit, 180.);
	flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);
	getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);


	q1 = makeposition("Q1" , z, t6, fing, EQ, k1, TL, fing);
	q2 = makeposition("Q2" , z, t6, finf, EQ, k2, TL, finf);
	got = makeposition("GOT", z, t6, EQ, abov, TL, t6);
	p1 = makeposition("P1" , z, t6, fing, EQ, b1, over,TL , fing);
	p2 = makeposition("P2" , z, t6, fing, EQ, b2, over,TL , fing);
	p3 = makeposition("P3" , z, t6, fing, EQ, b3, over,TL , fing);
	p4 = makeposition("P4" , z, t6, fing, EQ, b4, over,TL , fing);
	p5 = makeposition("P5" , z, t6, fing, EQ, b5, over,TL , fing);
	pp1 = makeposition("PP1" , z, t6, fing, EQ, b1, flip, TL , fing);
	pp2 = makeposition("PP2" , z, t6, fing, EQ, b2, flip, TL , fing);
	pp3 = makeposition("PP3" , z, t6, fing, EQ, b3, flip, TL , fing);
	pp4 = makeposition("PP4" , z, t6, fing, EQ, b4, flip, TL , fing);
	pp5 = makeposition("PP5" , z, t6, fing, EQ, b5, flip, TL , fing);
	get = makeposition("GET", z, t6, EQ, getit, TL, t6);


	movejnts(get, 400, 1500);
	waitfor(completed);
	OPEN;
	printf("put the sensor in the jaws ");
	QUERY(q);
	CLOSE;
	printf("go ahead ");
	QUERY(q);
	if (q == 'n') {
		movejnts(park, 400, 2500);
		return;
	}
	sensor = adcopen(7);
	for (; ; ) {
		movecart(q1, 400, 1500);
		movecart(q2, 400, 10000);
		waitfor(completed);
		QUERY(q); if (q == 'n') break;
		movecart(got, 400, 1500);
	}
	movejnts(get, 400, 1500);
	printf("type 'y' for poking");
	QUERY(q);
	if (q == 'n') {
		movejnts(park, 400, 2500);
		waitfor(completed);
		OPEN;
		return;
	}
	for (; ; ) {
		movejnts(p1, 60, 1500);
		evalfn(touchfn);
		movejnts(pp1, 60, 1500);
		movejnts(p2, 60, 1500);
		evalfn(touchfn);
		movejnts(pp2, 60, 1500);
		movejnts(p3, 60, 1500);
		evalfn(touchfn);
		movejnts(pp3, 60, 1500);
		movejnts(p4, 60, 1500);
		evalfn(touchfn);
		movejnts(pp4, 60, 1500);
		movejnts(p5, 60, 1500);
		evalfn(touchfn);
		movejnts(pp5, 60, 1500);
		movecart(get, 60, 1500);
		printf("more ? ");
		QUERY(q); if (q == 'n') break;
	}
	movejnts(park, 400, 1500);
	waitfor(completed);
	OPEN;
}


touchfn()
{
	nextmove = how.adcr[sensor] > 1;
}


fingfn(t)
TRSF_PTR t;
{
	t->p.z += (how.adcr[sensor] * .01 - 3.) / 3.;
}




