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

real when;

#define MAXACC  .015    /* mm/ms2 */

throw(v0)
VECT_PTR v0;
{
	int openat();

	real Tx = (12. * v0->x) / MAXACC;
	real Ty = (12. * v0->y) / MAXACC;
	real Tz = (12. * v0->z) / MAXACC;
	int  T = ((FABS(Tx) > (Ty))
			? ((FABS(Tx) > FABS(Tz))
				? Tx : Tz)
			: ((FABS(Ty) > FABS(Tz))
				? Ty : Tz));

	real dx, dy, dz;

	stop(0);
	setmod('c');
	dx = Tx * v0->x / 2.;
	dy = Ty * v0->y / 2.;
	dz = Tz * v0->z / 2.;

	distance("dx dy dz", -dx, -dy, -dz);
	setime(T / 2, T);
	move(there);

	when = .90;
	evalfn(openat);
	distance("dx dy dz",  2. * dx,  2. * dy,  2. * dz);
	setime(T / 2, T);
	move(there);

	setime(T / 2, T);
	move(there);
	stop(0);
	return;
}


openat()
{
	if (goalpos->scal >= when) {
		OPEN;
	}
}


pumatask()
{
	TRSF_PTR b0, grip;
	POS_PTR p0;
	VECT vel;
	int q;

	grip = gentr_trsl("GRIP", 0., 0., 170.);
	b0 = gentr_rot("B0", 400., 150., 700., yunit, 45.);

	p0 = makeposition("P0", t6, grip, EQ, b0, TL ,grip);

	QUERY(q)
	CLOSE
	setconf("d");
	setime(100, 3000);
	move(p0);
	vel.x = .0;
	vel.y = .0;
	vel.z = .6;

	throw(&vel);

	setmod('j');
	setconf("u");
	setime(100, 3000);
	move(park);
}
