#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, b1, e, h, search, track, pt, over;
	POS_PTR  p1, p2, p3, get;
	int searchfn(), trackfn();
	int q;

	search = newtrans("FING",searchfn);
	track = newtrans("FING",trackfn);
	h = newtrans("H", const);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
	e = gentr_eul("E" , 0. , 0. , 200. , 0. , 0.,  0.);
	b1 = gentr_rot("B1", 600. ,-400., 450., yunit, 180.);
	pt = gentr_rot("PT", 0., 0., 0., zunit, 90.);
	over = gentr_rot("OVER", 600., 0., 600., yunit, 180.);

	p1 = makeposition("P1" , z, t6, e, EQ, b1, pt, TL, pt);
	p2 = makeposition("P2" , z, t6, e, search, EQ, b1, pt, TL, pt);
	p3 = makeposition("P3" , z, t6, e, h, track, EQ, b1, pt, TL, e);
	get = makeposition("GET", z, t6, EQ, over, TL, t6);


	movejnts(get, 400, 1500);
	waitfor(completed);
	OPEN
	printf("put the sensor ");
	QUERY(q);
	CLOSE
	printf("go ahead ");
	QUERY(q);
	if (q == 'n') {
		movejnts(park, 100, 20000);
		return;
	}
	sensor = adcopen(7);
	movecart(p1, 400, 1500);
	movecart(p2, 100, 20000);
	movecart(p3, 100, 20000);
	OPEN
	movejnts(park, 400, 1500);
}

#define CV      .010
#define STEP   1.

double ZT;

searchfn(t)
TRSF_PTR t;
{
	double alpha = 0., inc;
	int fl;

	t->p.z -= STEP;
	fl = how.adcr[sensor];
	if ((fl * CV - 2.5) * .1 > 0.) {
		nextmove = YES;
		ZT = t->p.z;
	}
}

trackfn(t)
TRSF_PTR t;
{
	double alpha = 0., inc;
	int fl;
	static int first = YES;

	if (first) {
		first = NO;
		t->p.z = ZT;
	}
	t->p.x -= STEP;
	fl = how.adcr[sensor];
	inc = (fl * CV - 2.5);
	t->p.z += inc * .05;
	alpha = atan2(inc , STEP);
	alpha *= RADTODEG;
	rotm(t, yunit, -alpha);
}
