#include "../h/rccl.h"

static int t0;

pumatask()
{
	TRSF_PTR z, e , b, pa1, pa2, table;
	POS_PTR  p0, pt1, pt2;
	int tablefn();
	int i;

	table = newtrans("TABLE", tablefn);
	z = gentr_trsl("Z",  0.,  0., 864.);
	e = gentr_trsl("E" , 0. , 0. , 170.);
	b = gentr_rot("B", 600. ,  300., 700., yunit, 180.);
	pa1 = gentr_rpy("PA1"  , 0., 0., 0., 0., 0., 10.);
	pa2 = gentr_rpy("PA2"  , 0., 0., 0., 0., 0., -10.);

	p0 = makeposition("P0" , z, t6, e, EQ, b, TL, e);
	pt1 = makeposition("PT1", z, t6, e, EQ, b, table, pa1, TL, e);
	pt2 = makeposition("PT2", z, t6, e, EQ, b, table, pa2, TL, e);

	setvel(300, 20);
	setmod('c');
	setime(200, 0);
	move(p0);
	waitfor(completed);
	t0 = rtime;
	for (i = 0; i < 6; ++i) {
		move(pt1);
		move(pt2);
	}
	setvel(400, 100);
	setmod('j');
	move(park);
}

tablefn(t)
TRSF_PTR t;
{
	real rps = .03;
	real alpha = rps * (t0 - rtime) * .001;

	t->p.x = 100. * cos(alpha * pit2_m);
	t->p.y = 100. * sin(alpha * pit2_m);
	Rot(t, zunit, alpha * 360.);
}
