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

extern struct how how;
int sensor;

real Y = 0.;

pumatask()
{
	TRSF_PTR z, e, circ, tilt, or, fl, b1, conv1;
	POS_PTR  pm3, pm4, pf, p0, p1;
	int conv1fn();
	int circfn(), tiltfn();
	int q;

	circ = newtrans("CONV", circfn);
	tilt = newtrans("TILT", tiltfn);
	conv1 = newtrans("CONV1",conv1fn);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);
	or = gentr_eul("OR", 300. ,  600., 600.,    0., 0., 90.);
	fl = gentr_rot("FL", 0. , 0., 0., yunit, 180.);
	b1 = gentr_rot("B1", 600. , -300., 500., yunit, 180.);

	pm3 = makeposition("PM1" , z, t6, e, EQ, tilt, or, fl, TL, e);
	pm4 = makeposition("PM2" , z, t6, e, EQ, circ, or, fl, TL, e);
	pf = makeposition("PF" , z, t6, e, EQ, or, fl, TL, e);
	p0 = makeposition("P0" , z, t6, e, EQ, b1, TL, e);
	p1 = makeposition("P1", z, t6, e, EQ, conv1, b1, TL, e);

	sensor = adcopen(7);
	for (; ; ) {
		movecart(p0, 300, 1500);
		movejnts(p1, 300, 3000);
		movecart(pf, 300, 1500);
		movecart(pm4, 300, 3000);
		movecart(pm3, 300, 3000);
		movecart(park, 300, 1500);
		++completed;
		while (completed) {
			nap(10);
			nextmove = how.adcr[sensor] > 5;
		}
		Y = 0.;
		QUERY(q); if (q == 'n') break;
	}
}



circfn(t)
TRSF_PTR t;
{
	double radius = 50.;
	t->p.y = radius * sin(2. * goalpos->scal * PIT2);
	t->p.z = radius * cos(2. * goalpos->scal * PIT2);
}


tiltfn(t)
TRSF_PTR t;
{
	VECT k;

	k.x = sin(2. * goalpos->scal * PIT2);
	k.y = cos(2. * goalpos->scal * PIT2);
	k.z = 0.;
	Rot(t, &k, 5.);
}


conv1fn(t)
TRSF_PTR t;
{
	t->p.y = (Y += 6.);
}
