/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * NAME:	testKinematics <robotName>
 *
 * USAGE:	testKinematics <robotName> [-i]
 *
 * DESCRIPTION: performs a test of the forward and inverse kinematic 
 *		functions of the arm. It is done simply by playing them
 *		off against each other ... the program runs through a 
 *		sequence of joint angles, computes the configuration,
 *		the forward kinematics, and then computes the inverse
 *		kinematics.
 *
 *		The program performs about 2000 tests. These include 
 *		cases away from and close to singularities. The results
 *		printed out describe the maximum observed norm of the
 *		difference (in degrees) between the original angles and the
 *		ones recomputed by the inverse kinematics.
 *
 * ARGUMENTS:	The -i option allows the user to interactively run tests
 *		on specific sets of joints angles.
 *
 * FILES:
 *	
 * NOTES:	I wrote this because I was always playing with the kinematic
 *		functions, but was never sure I hadn't screwed them up in the
 *		process.
 *
 * AUTHOR/DATE: John Lloyd, Spring 87
 *
 * UPDATES:
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include <math.h>

#ifdef VXWORKS_CFG
#define main testKinematics_
#endif

#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"
#include "../h/elbow_kynvar.h"
#include "../h/vector.h"
#include "../h/robotTypes.h"
#include "../h/cputime.h"

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf [256];

KYN *kyn;
void *var;
JLS *jls;

/* List of singularities to test for */

unsigned long singSet[] = { 
	ELBOW_ALIGN_DEG,
	ELBOW_ELBOW_DEG,
	ELBOW_WRIST_DEG,
};
int numSingCombos = (1<<ARRAY_SIZE(singSet));

unsigned long getSingCode(i)
int i;
{
	unsigned long code = 0;
	unsigned int mask;
	int k;

	for (mask=i,k=0; mask; mask>>=1,k++)
	 { if (mask & 0x1)
	    { code |= singSet[k];
	    }
	 }
	return (code);	
}

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

	argc--; argv++;
	while (argc)
	 { if (ARGUMENT ("-i"))
	    { interactive = 1;
	    }
	   else if (robotName == NULL)
	    { robotName = *argv;
	    }
	   else
	    { robotName = NULL;
	      break;
	    }
	   argc--; argv++;
	 }
	if (robotName == NULL)
	 { usageMsg (-1, "testKinematics <robotName> [-i]\n");
	 }

	if ((jls = getJls(robotName)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	if ((kyn = getKyn(robotName, NULL)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	var = (void*)malloc(getVarSize(robotName));

	if (interactive)
	 { char *keytree, *tree_match_parse(), *tree_match_err_at();
	   float angles[ELBOWJ];
	   float result[ELBOWJ];
	   TRSF T6;

	   keytree = tree_match_parse("					\
(	   quit								\
	   set 								\
	    (angles)							\
	   solve							\
	   speed							\
	    (%d", &icount, ",,,	\"iteration count\")			\
)",0);

	   if (keytree == 0)
	    { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	      exit (-1);
	    }
	   do
	    { int status;

	      tree_match (keytree, "TEST KINEMATICS> ", key_buf);

	      if (COMMAND ("set angles"))
	       { 
	         printf ("Enter j6 in degrees> ");
		 if (scanVf (angles, 6) != 1)
	          { fprintf (stderr, "ERROR/ cannot read values\n");
		    continue;
	          }
		 scaleVf (angles, DEGTORAD, angles, 6);
	       }
	      else if (COMMAND ("solve"))
	       { unsigned long conf, testConf;
		 char cbuf[4];

		 elbowFwdKinematics (&T6, &conf, angles, kyn, NULL);
		 elbowSolveConf (&testConf, angles, kyn);
		 status = elbowInvKinematics (result, &T6, conf, angles, 
							   kyn, jls, NULL);
		 printf ("FwdKin conf is '%s' (0x%x)\n", 
			 configToStr(cbuf, conf, kyn), conf);
		 printf ("SolveConf conf is '%s' (0x%x)\n", 
			 configToStr(cbuf, testConf, kyn), testConf);
		 printf ("IK status = 0x%x\n", status);
		 scaleVf (result, RADTODEG, result, 6);
		 printTrsf ("T6:    %m\n", &T6);
		 printVf ("Result: %10.4f\n", result, 6);
	       }
	      else if (COMMAND ("speed"))
	       { float loopTime;
		 float time1;
		 float time2;
		 unsigned long conf;
		 int i;

		 time1 = cputime(User);
		 for (i=0; i<icount; i++)
		    ;
		 time2 = cputime(User);
		 loopTime = time2-time1;

		 time1 = cputime(User);
		 for (i=0; i<icount; i++)
		  { elbowFwdKinematics (&T6, &conf, jls->bestAngles, kyn, var);
		  }
		 time2 = cputime(User);
		 
		 printf ("forward kinematics: %g usec.\n",
			  (time2-time1-loopTime)*1e6/icount);

		 time1 = cputime(User);
		 for (i=0; i<icount; i++)
		  { elbowInvKinematics (result, &T6, conf, jls->bestAngles, 
					       kyn, jls, var);
		  }
		 time2 = cputime(User);
		 
		 printf ("inverse kinematics: %g usec.\n",
			  (time2-time1-loopTime)*1e6/icount);

		 time1 = cputime(User);
		 for (i=0; i<icount; i++)
		  { elbowInvKinematics (result, &T6, conf, jls->bestAngles, 
					       kyn, jls, 0);
		  }
		 time2 = cputime(User);
		 
		 printf ("inverse kinematics (no var update): %g usec.\n",
			  (time2-time1-loopTime)*1e6/icount);
	       }
	    }
	   while (!COMMAND ("quit"));
	 }
	else
	 {
	   int numtests, singular, numsing;
	   float angles1[6];
	   float angles2[6];
	   float norm, worstNorm;
	   float worstAngles[6];
	   float *testset1;
	   float *testset2;

#define END		-1.0e30

	   static float newtestset1[] = 
	    {  0.0, -145.0, 120.0, END,
	     -30.0,   60.0, 120.0, END,
	     -40.0,   80.0, 125.0, 0.0, END,
	       0.0,  120.0, END,
	       0.0,   45.0, 145.0, END,
	     120.0, -120.0,   0.0, END };

	   static float newtestset2[] =
	     {  0.0, 120.0, END,
	      -30.0,  60.0, END,
	      -40.0, 125.0, END,
	       20.0, 120.0, END,
	       10.0,  45.0, END,
	      120.0,-120.0, END };

	   testset1 = newtestset1;
	   testset2 = newtestset2;

	   /* Testing away from singularities */

	   numtests = testAnglesInit (testset1);
	   printf (
"\nTesting with %d non-singular positions ...\n\n", numtests);
	   worstNorm = -1.0;
	   numsing = 0;
	   while (testAnglesGet (angles1))
	    { 
	      if (singular = elbowSingularityDetect (angles1,5.0*DEGTORAD,kyn))
	       { elbowSingularitySet (angles1, angles1, 5.0*DEGTORAD, 
						       singular, kyn, jls);
	       }
	      if (test_solut (&norm, angles1))
	       { numsing++;
	       }
	      if (norm > worstNorm)
	       { worstNorm = norm;
		 copyVf (worstAngles, angles1, 6);
	       }
	    }
/*	   printf ("%d singular positions detected\n", numsing);*/
	   printf ("Worst average error found (degrees) = %g at angles\n", 
		    worstNorm*RADTODEG);
	   printa (" ", worstAngles);

	   /* Test near singularities */

	   numtests = testAnglesInit (testset2);
	   printf (
"\nTesting with %d positions 1 degree from singular ... \n\n", 
	           numtests*numSingCombos*2);
	   worstNorm = -1.0;
	   numsing = 0;
	   while (testAnglesGet (angles1))
	    { int i;
	      float distance;

	      for (i=0; i<numSingCombos*2; i++)
	       { 
		 distance = (i & 0x1) ? -1.0*DEGTORAD : 1.0*DEGTORAD;
		 elbowSingularitySet (angles2, angles1, distance, 
					      getSingCode(i>>1), kyn, jls);

		 if (test_solut (&norm, angles2))
		  { numsing++;
		  }
		 if (norm > worstNorm)
		  { worstNorm = norm;
		    copyVf (worstAngles, angles2, 6);
		  }
   	       }
	    }
/*	   printf ("%d singular positions detected\n", numsing);*/
	   printf ("Worst average error found (degrees) = %g at angles\n", 
		    worstNorm*RADTODEG);
	   printa (" ", worstAngles);

	   /* test VERY near singularities */

	   numtests = testAnglesInit (testset2);
	   printf (
"\nTesting with %d positions .1 degrees from singular ...\n\n", 
		   numtests*numSingCombos*2);
	   worstNorm = -1.0;
	   numsing = 0;
	   while (testAnglesGet (angles1))
	    { int i;
	      float distance;

	      for (i=0; i<numSingCombos*2; i++)
	       { 
		 distance = (i & 0x1) ? -0.1*DEGTORAD : 0.1*DEGTORAD;
		 elbowSingularitySet (angles2, angles1, distance, 
					      getSingCode(i>>1), kyn, jls);

		 if (test_solut (&norm, angles2))
		  { numsing++;
		  }
		 if (norm > worstNorm)
		  { worstNorm = norm;
		    copyVf (worstAngles, angles2, 6);
		  }
   	       }
	    }
/*	   printf ("%d singular positions detected\n", numsing);*/
	   printf ("Worst average error found (degrees) = %g at angles\n", 
		    worstNorm*RADTODEG);
	   printa (" ", worstAngles);
	 }
}

test_solut (norm, angles1)
float *norm;
float *angles1;
{	
	TRSF T6;
	float j60[ELBOWJ];
	float j61[ELBOWJ];
	float j62[ELBOWJ];
	float angles2[ELBOWJ];
	int code = 0;
	unsigned long conf1;
	unsigned long conf2;

	elbowFwdKinematics (&T6, &conf1, angles1, kyn, NULL);
	elbowSolveConf (&conf2, angles1, kyn);
	if (conf1 != conf2)
	 { printf (
"solveConf() and fwdKinematics() compute configuartions 0x%x and 0x%x\n",
	           conf2, conf1);
	   printa ("at angles ", angles1);
	 }
	code = elbowInvKinematics (angles2, &T6, conf1, angles1, 
							kyn, jls, NULL);
	*norm = normVf (subtractVf (angles2, angles2, angles1, 6), 6);
	if (code & KYN_CANT_REACH)
	 { float deg[6];
	   scaleVf (deg, RADTODEG, angles1, 6);
	   printVf ("cant reach %8.3f\n", deg, 6);
	 }
	return (code);
}

printa (s, fp)
char *s;
float *fp;
{
	char buf[256];
	float a[6];

	strcpy (buf, s);
	strcat (buf, " %10.5lf\n");
	scaleVf (a, RADTODEG, fp, 6);
	printVf (buf, a, 6);
}
