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

TRSF_PTR spire;

pumatask()
{
	int spirefn();

       TRSF_PTR
	base = gentr_rot("BASE", -200., 300., 200., yunit, 0.),
	tool = gentr_rot("TOOL", 0., 0., 180., yunit, 0.),
	washer = gentr_rot("WASHER", 600., 100., 0., zunit, 0.),
	washer_appro = gentr_rot("WASH_APP", 0., 0., 10., yunit, 180.),
	washer_grasp = gentr_rot("WASH_GRASP", 0., 0., 0., yunit, 180.),
	pin = gentr_rot("PIN", 400. , 300., 0., zunit, 0.),
	pinbase = gentr_rot("PINBASE", 0. ,0. , 0., yunit, 180.),
	pintip = gentr_rot("PINTIP", 0., 0., 50., yunit, 180.),
	pin_appro = gentr_rot("PIN_APP", 0., 0., 55., yunit, 180.),
	pin_away = gentr_eul("PIN_AWAY", 0., 10., 20., 20., -20., 0.);

	POS_PTR
	approach = makeposition("APPROACH",
		base, t6, tool, EQ, washer, washer_appro, TL, tool),
	getwasher = makeposition("GETWASH",
		base, t6, tool, EQ, washer, washer_grasp, TL, tool),
	above = makeposition("ABOVE",
		base, t6, tool, EQ, pin, pin_appro, TL, tool),
	touch = makeposition("TOUCH",
		base, t6, tool, EQ, pin, pintip, TL, tool),
	down = makeposition("DOWN",
		base, t6, tool, EQ, pin, pinbase, TL, tool),
	away = makeposition("AWAY",
		base, t6, tool, EQ, pin, pinbase, pin_away, TL, tool),
	spiral;


	spire = newtrans("SPIRE", spirefn);
	spiral = makeposition("SPIRAL",
		base, t6, tool, EQ, spire, pin, pintip, TL, tool);

	movejnts(approach, 200, 2000);
	movecart(getwasher, 150, 500);
	movejnts(above, 150, 1000);
	movecart(touch, 150, 500);
	waitfor(completed);
	rtime = 0;
	movecart(spiral, 150, 5500);
	movejnts(down, 200, 1500);
	movecart(away, 200, 1500);
	movejnts(park, 200, 2000);
}

/*
 * generate a spiraling motion in the x y plan
 */

spirefn()
{
	extern int rtime;
	double time,
	       omega = 1.,
	       growth = 5.0;

	time = (double)rtime / 1000.;
	spire->p.x = time * growth * cos(omega * time * PIT2);
	spire->p.y = time * growth * sin(omega * time * PIT2);
}
