/*----------McGill-Computer-Vision-and-Robotics-Laboratory--C-File-----------
 *
 * NAME:	speed
 *
 * USAGE:	speed <numIterations>
 *
 * DESCRIPTION: A program to measure how much time critical functions take
 *		to execute. The functions are executed repeatedly in a loop,
 *		which iterates <numIterations> times. 
 *
 * AUTHOR:	Vincent Hayward, Purdue University, 1983.
 *		CVaRL installation and modification by John Lloyd.
 *
 * NOTES:	these time critical procedures are currently:
 *			tr_to_jns_n( .. )
 *			jns_to_tr_n( .. )
 *			jacobT_n( .. )
 *			invertTrsf( .. )
 *			multTrsf( .. )
 *			SINCOS( .. )
 *
 *-------------McGill-Computer-Vision-and-Robotics-Laboratory--------------*/

#include "rccl.h"
#include "manip.h"
#include "macros.h"
#include "useful.h"

#ifndef FASTMATH
#define SINCOS(a, s, c) { *(s) = sin(a); *(c) = cos(a); }
#endif

JNTS j = {"run",1., 1., 1., 1., 1., 1.},
     oj= {"run",0., 0., 0., 0., 0., 0.};
FORCE f = {1., 1., 1., 1., 1., 1.};
SNCS sncs;

main(argc,argv)
char **argv;
{
	int i, n = 1;
	double d = 1.;
	double d1, d2;
	TRSF t, u, v;
	double t1, t2, lt, cputime();
	double fact = 1000000.;

	if (argc != 2)
	 { fprintf (stderr, "Usage: %s <iterationCount>\n", *argv);
	   exit(-1);
	 }

	jns_to_tr(&t, &j, 0);	    /* Make a transform to solve */

	n = atoi(*(argv+1));		    /* Determine no. of iterations */
	if (n <= 0)
	 { fprintf (stderr, "%d iterations specified\n", n);
	   exit (-1);
	 }

	i = n;				    /* Time 'tr_to_jns_n' ... 	*/
	t1 = cputime (User);		    /*   CPU time before 	*/
	while (i--)			    /*   execute n times    	*/
		tr_to_jns(&j, &t, &oj, &sncs);   /*   CPU time after	   	*/
	t2 = cputime (User);

	lt = ((t2 - t1) * fact) / n;
	printf("solutions time = %f\n", lt);

	i = n;
	t1 = cputime (User);			    /* Time 'jns_to_tr_n' ...	*/
	while (i--)
		jns_to_tr(&t, &j, &sncs);
	t2 = cputime (User);

	lt = ((t2 - t1) * fact) / n;
	printf("handpos time = %f\n", lt);

	n *= 10;
	update_jacobian_terms (&sncs);

	i = n;				    /* Time 'Jacobian' ...	*/
	t1 = cputime (User);
	while (i--) {

		jacobT(&j, &f, &sncs);
	}
	t2 = cputime (User);

	lt = ((t2 - t1) * fact) / n;
	printf("Jacobian time = %f\n", lt);

	i = n;				    /* Time 'invert' ...	*/
	t1 = cputime (User);
	while (i--)
		invertTrsf(&u, &t);
	t2 = cputime (User);

	lt = ((t2 - t1) * fact) / n;
	printf("invertTrsf time = %f\n", lt);


	i = n;				    /* Time 'multTrsf' ...	*/
	t1 = cputime (User);
	while (i--)
		multTrsf(&v, &t, &u);
	t2 = cputime (User);

	lt = ((t2 - t1) * fact) / n;
	printf("multTrsf time = %f\n", lt);


	i = n;				    /* Time SINCOS ...		*/
	t1 = cputime (User);
	while (i--)
		SINCOS(3.34567, &d1, &d2);
	t2 = cputime (User);

	lt = ((t2 - t1) * fact) / n;
	printf("sin+cos time = %f\n", lt);
}
