#include "../h/cdefs.h"
#include "../h/robotMath.h"
#include "../h/robot_jls.h"

float ang[MAXJNTS];
int enc[MAXJNTS];

#if VXWORKS_CFG
#define main angtoencDemo_
#endif

char **argvec;

main(argc, argv)
int argc;
char *argv[];
{
	int j;
	JLS *jls;

	argvec = argv;
	argc--; argv++;
	if (argc != 1)
	 { usageMsg (-1, "%s <robot-name>\n", argvec);
	 }
	
	if ((jls = getJls (*argv)) == NULL)
	 { printErrors ();
	   exit (-1);
	 }
	while (1)
	 { for (j=0; j<jls->numJ; j++)
	    { if (scanf ("%f", &ang[j]) != 1)
	       { exit (0);
	       }
	    }
	   degreeToRadian (ang, ang, jls);
	   (*jls->angleToEncoder)(enc, ang, jls);
	   for (j=0; j<jls->numJ; j++)
	    { printf ("%6d ", enc[j]);
	    }
	   putchar ('\n');
	 }
}
