/*-=-=-=-=-=-=-=-=-=-=-=- 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 ~=~=~=~=
 *		
 * MODULE NAME:	test-subs.c
 *
 * DESCRIPTION:	Subroutines used to generate a large set of testing angles
 *		to be used in inspecting kinematic routines.
 *
 *		testAnglesInit (angle_spec) specifies at set of angles
 *		   to be returned through subsequent calls to get_angles().
 *		   The set of angles is defined by an array of
 *		   floats called 'angle_spec'. This array describes a
 *		   list of angle values for each joint, starting at
 *		   joint 1. The delimiter between lists any float less
 *		   than or equal to -1.0e30 (hack).
 *
 *		   The set of angles is taken to be the cartesian product
 *		   of all angle values specified (which can get quite large
 *		   if one is not carefull). Subsequent calls to get_angles() 
 *		   will step incrementally through this set.
 *
 *		    The routine returns the total size of the set specified.
 *
 *		testAnglesGet (angles) takes the next member of the specified
 *		   angle set, converts it into radians, copies it into 
 *		   'angles' (an array of 6 doubles), and returns the value
 *		   of 'angles'. If no set was specified, or we have run 
 *		   though all the angles in the set, the function copies 
 *		   nothing and returns NULL.
 *		
 * AUTHOR/DATE:	John Lloyd, Spring 87
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include "../h/robotMath.h"

#define MAX	1024

#define END -1.0e30+1.0e-6

static struct {
	int i, size; } set[6];

static float setvalues [1024];
static int totalsize = 0;
static int totalcount = 0;

testAnglesInit (spec)
float *spec;
{
	float value;
	int i, j, k, size;

	i = k = 0;
	for (j = 0; j < 6; j++)
	 { size = 0;
	   while ((value = spec[i++]) > END)
	    { if (k == MAX)
	       { 
printf ("ERROR in testAnglesInit() -- more than %d angles specified\n", MAX);
		 return(0);
	       }
	      setvalues[k++] = value;
	      size++;
	    }
	   if (size == 0)
	    {
printf ("ERROR in testAnglesInit() -- zero size list for joint %d\n", j);
	      return (0);
	    }
	   set[j].size = size;
	 }

	totalsize = 1;
	for (j=0; j<6; j++)
	 { totalsize *= set[j].size;
	   set[j].i = 0;
	 }
	totalcount = 0;

	return (totalsize);
}

float *testAnglesGet (angles)
float *angles;
{
	int j, base;

	if (totalcount >= totalsize)
	 { return (NULL);
	 }
	else
	 { for (j=0; j<6; j++)
	    { if (set[j].i+1 == set[j].size)
	       { set[j].i = 0;
	       }
	      else
	       { set[j].i++;
		 break;
	       }
	    }
	   base = 0;
	   for (j=0; j<6; j++)
	    { if (j!=2)
	       { angles[j] = DEGTORAD * setvalues [base + set[j].i];
	       }
	      else
	       { angles[j] = setvalues [base + set[j].i];
	       }
	      base += set[j].size;
	    }
           totalcount++;
	   return (angles);
	 }
}

#ifdef TEST

float a[] = { 45.0, 90.0, END, 
	      10.0, END,
	      11.0, END,
	      12.0, END,
	      1.0, 2.0, 3.0, END,
	      13.0, END };

main()
{
	float angles[6];

	printf ("init angles returns %d\n", testAnglesInit (a));

	while (testAnglesGet (angles))
	 { printVf ("ANGLES: %8.3f\n", angles, 6);
	 }
}

#endif
