/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/* This is a program to test object frames and their tracking. */

#include <stdio.h>

#ifdef VXWORKS_CFG
#define main testVm_
#endif

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

extern changeTr();
extern int transDebug;

main(argc, argv)
int argc;
char **argv;
{
	char *robotName = NULL;

	POS *vframe;
	POS *track;
	POS *start;

	TRSF *rest;
	TRSF *varb;
	TRSF *begin0;
	TRSF *begin1;
	TRSF *tool;

	TRSF tbuf;
	TRSF_DESC *varbDesc;

	MANIP *mnp;
	MANIP *obj;

	float *amount;

	int cpu0;
	int cpu1;
#define CPUMASK0 (1<<cpu0)
#define CPUMASK1 (1<<cpu1)
	int mask;

	transDebug = 0;

	argc--; argv++;
	while (argc > 0)
	 { if (robotName == NULL)
	    { robotName = *argv;
	    }
	   argc--; argv++;
	 }
	if (robotName == NULL)
	 { robotName = (char*)getDefaultRobot();
	 }
	else
	 { if (!find_robot(robotName))
	    { fatalMsg (-1, "robot %s not known to the system\n", robotName);
	    }
	 }

	rcclSetOptions(RCCL_ERROR_EXIT);

	/* try to use two CPUs if we can */

	mask = rciAllocCpus (0x3);
	if (!mask & 0x1)
	 { printf ("Can't allocate arbiter CPU!\n");
	   exit (-1);
	 }
	cpu0 = 0;
	cpu1 = (mask & 0x2) ? 1 : 0;

	amount = (float*)allocMem ("amount", sizeof(float), UNDEF);
	*amount = 0.5;

	rest = allocTrans("rest", cpu0);
	varb = allocTrans("varb", cpu0);
	begin0 = allocTrans("begin0", cpu0);
	tool = allocTrans("tool", cpu0);

	varbDesc = TRSF_GETDESC(varb);

	start = makePosition ("start", T6, EQ, begin0, TL, T6);
	vframe = makePosition ("vframe", T6, tool, EQ, varb, TL, tool);

	printf ("Creating robot on cpu %d and object on cpu %d\n", cpu1, cpu0);

	mnp = rcclCreate (robotName, CPUMASK1);
	obj = rcclCreate ("obj", CPUMASK0);

	track = makePosition ("track", T6, EQ, rest, obj->t6, TL, T6);

	rcclStart();

	printf ("Check out basic functional transform stuff:\n");

	readTrans (mnp->t6, begin0);
	readTrans (mnp->t6, varb);
	setTime (mnp, 100.0, F_UNDEF);
	transMotionEval (varb, changeTr, 1, mnp);

	move (mnp, vframe);
	printf ("Robot should be doing circle in x-y plane\n");
	confirm();

	transMotionEval (varb, changeTr, 2, mnp);
	printf ("Robot should be doing circle in y-z plane\n");
	confirm();

	transMotionEval (varb, changeTr, 3, mnp);
	printf ("Robot should be doing circle in z-x plane\n");
	confirm();

	stopCurrentMotion(mnp, 1);
	waitfor (mnp->completed);

	printf ("Now make the object move in the x-y plane:\n");

	readTrans (obj->t6, varb);
	setTime (obj, 100.0, F_UNDEF);
	move (obj, vframe);

	transMotionEval (varb, changeTr, 1, obj);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);
	delay (1000.0);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);

	printf ("Object should be moving in the x-y plane only\n");
	confirm();

	printf ("Now we will make the robot track the object\n");

	readTrans (mnp->t6, rest);
	setTime (mnp, 100.0, F_UNDEF);
	move (mnp, track);

	printf ("The robot should be tracking the object\n");
	confirm();

	printf ("Now we will test the y-z and z-x planes:\n");

	transMotionEval (varb, changeTr, 2, obj);
	delay (3000.0);
	transMotionEval (varb, changeTr, 3, obj);	

	confirm();

	printf ("Now do move in the x-y plane using joint mode:\n");

	stopCurrentMotion (mnp, 1);
	setMod (mnp, 'j');
	setTime (mnp, 100.0, F_UNDEF);
	transMotionEval (varb, changeTr, 1, obj);
	move (mnp, track);

	confirm();

	printf ("Now abort the object motion; robot should stop:\n");

	stopCurrentMotion (obj, 1);
	waitfor (obj->completed);	

	confirm();

	stopCurrentMotion (mnp, 1);
	move (mnp, start);
	waitfor (mnp->completed);
	rcclRelease (0);

	printf ("Now we put the robot on CPU %d and the object on CPU %d:\n",
		 cpu0, cpu1);

	rcclDelete (mnp);
	rcclDelete (obj);
	
	mnp = rcclCreate (robotName, CPUMASK0);
	obj = rcclCreate ("obj", CPUMASK1);

	freePosition (track);	
	track = makePosition ("track", T6, EQ, rest, obj->t6, TL, T6);
	rcclStart();

	printf ("Again, make robot move in x-y plane:\n");

	readTrans (mnp->t6, begin0);
	readTrans (mnp->t6, varb);
	setTime (mnp, 100.0, F_UNDEF);
	move (mnp, vframe);
	transMotionEval (varb, changeTr, 1, mnp);
	confirm();
	
	stopCurrentMotion(mnp, 1);
	waitfor (mnp->completed);

	printf ("Now make the object move in the x-y plane:\n");

	readTrans (obj->t6, varb);
	setTime (obj, 100.0, F_UNDEF);
	move (obj, vframe);

	transMotionEval (varb, changeTr, 1, obj);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);
	delay (1000.0);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);
	
	confirm();

	printf ("Now make the robot track the object:\n");

	readTrans (mnp->t6, rest);
	setTime (mnp, 100.0, F_UNDEF);
	move (mnp, track);
	confirm();

	stopCurrentMotion (mnp, 1);
	stopCurrentMotion (obj, 1);
	move (mnp, start);
	waitfor (mnp->completed);

	printf ("Now release the transforms and reallocate them on CPU %d\n",
		 cpu1);

	freePosition(start);
	freePosition(vframe);
	freePosition(track);

	freeTrans (rest);
	freeTrans (varb);
	freeTrans (tool);

	rest = allocTrans("rest", cpu1);
	varb = allocTrans("varb", cpu1);
	begin1 = allocTrans("begin1", cpu1);
	tool = allocTrans("tool", cpu1);
	*begin1 = *begin0;

	start = makePosition ("start", T6, EQ, begin1, TL, T6);
	vframe = makePosition ("vframe", T6, tool, EQ, varb, TL, tool);
	track = makePosition ("track", T6, EQ, rest, obj->t6, TL, T6);

	printf ("Again, make robot move in x-y plane:\n");

	readTrans (mnp->t6, begin1);
	readTrans (mnp->t6, varb);
	setTime (mnp, 100.0, F_UNDEF);
	transMotionEval (varb, changeTr, 1, mnp);
	move (mnp, vframe);
	confirm();
	
	stopCurrentMotion(mnp, 1);
	waitfor (mnp->completed);

	printf ("Now make the object move in the x-y plane:\n");

	readTrans (obj->t6, varb);
	setTime (obj, 100.0, F_UNDEF);
	move (obj, vframe);

	transMotionEval (varb, changeTr, 1, obj);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);
	delay (1000.0);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);

	confirm();

	printf ("Now make the robot track the object:\n");

	readTrans (mnp->t6, rest);
	setTime (mnp, 100.0, F_UNDEF);
	move (mnp, track);
	confirm();

	stopCurrentMotion (mnp, 1);
	stopCurrentMotion (obj, 1);
	move (mnp, start);
	waitfor (mnp->completed);

	printf ("Now we put both robot and object on CPU %d:\n", cpu1);

	rcclRelease(0);

	rcclDelete (mnp);
	mnp = rcclCreate (robotName, CPUMASK1);

	rcclStart();

	printf ("Again, make robot move in x-y plane:\n");

	readTrans (mnp->t6, begin0);
	readTrans (mnp->t6, varb);
	setTime (mnp, 100.0, F_UNDEF);
	move (mnp, vframe);
	transMotionEval (varb, changeTr, 1, mnp);
	confirm();
	
	stopCurrentMotion(mnp, 1);
	waitfor (mnp->completed);

	printf ("Now make the object move in the x-y plane:\n");

	readTrans (obj->t6, varb);
	setTime (obj, 100.0, F_UNDEF);
	move (obj, vframe);

	transMotionEval (varb, changeTr, 1, obj);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);
	delay (1000.0);
	readTrans (obj->t6, &tbuf);
	printf ("OBJ->T6:\n");
	printTrsf ("%m\n", &tbuf);
	
	confirm();

	printf ("Now make the robot track the object:\n");

	readTrans (mnp->t6, rest);
	setTime (mnp, 100.0, F_UNDEF);
	move (mnp, track);
	confirm();

	stopCurrentMotion (mnp, 1);
	stopCurrentMotion (obj, 1);
	move (mnp, start);
	waitfor (mnp->completed);

	printf ("Finally, test the maintain() primitive.\n");

#define EPSILON 1e-6

	 { TRSF *a;
	   POS *p;
	   int mid1, mid2;

	   a = allocTrans (NULL, UNDEF);
	   p = makePosition (NULL, T6, EQ, a, TL, T6);

	   a->p.x = 123456.0;

	   setTime (obj, 0.0, F_DEFAULT);
	   mid1 = maintain (obj, p);
	   waitForStart (mid1);
	   if (cmpTrsf (a, obj->t6) > EPSILON)
	    { printf ("error(1): maintain didn't go to a right away:\n");
	      printTrsf ("T6 %m\n", mnp->t6);
	      printTrsf ("A  %m\n", a);
	      exit (-1);
	    }
	   delay (3000.0);
	   if (motionStatus(mid1) != M_RUNNING)
	    { printf ("error(2): maintain stopped prematurely\n");
	      exit (-1);
	    }
	   a->p.x = -100000.0;
	   setTime (obj, 0.0, F_DEFAULT);
	   mid2 = maintain (obj, p);
	   rcclBlock();
	   rcclBlock();
	   rcclBlock();
	   if (motionStatus(mid1) == M_RUNNING)
	    { printf ("error(3): maintain did not stop when asked\n");
	      exit (-1);
	    }
	   if (cmpTrsf (a, obj->t6) > EPSILON)
	    { printf ("error(4): maintain didn't go to A right away:\n");
	      printTrsf ("T6 %m\n", obj->t6);
	      printTrsf ("A  %m\n", a);
	      exit (-1);
	    }
	 }

	printf ("\nROUTINES OK\n\n");

	rcclRelease(1);
}

confirm ()
{
	printf ("Hit <CR> to confirm:\n");
	while (getchar() != '\n')
	   ;
}
