/*-=-=-=-=-=-=-=-=-=-=-=- 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:	function
 *
 * DESCRIPTION:	Main driver function for all the calculator operations.
 *		
 *		The routine function takes an array 'r' of GTYPE for
 *		storing return values, an integer 'n' specifying the number 
 *		of arguments, and an array 'a' of GTYPES containing the
 *		input arguments. The name of the function is 
 *		stored in r[0].name, and the anticipated number of return
 *		values is stored in r[0].val.i. The function is then looked
 *		up the function definition table.
 *
 *		The function definition table contains, for each function,
 *		its
 *			1. name.
 *			2. the function which implements it.
 *			3. a string whose characters denote the return values.
 *			4. a string whose characters denote the expected
 *			   arguments.
 *
 *		The characters 'i', 'd', 'm', and 's' denote integer, double,
 *		matrix, and string, respectively. These are the same as the
 *		values defined in "types.h" for INTEGER, DOUBLE, MATRIX, and
 *		STRING. The character 'x' is reserved for "don't care".
 *
 *		In finding the function associated with a given name, the
 *		driver routines checks the table entries against the following
 *		rules:
 *
 *		1. name match.
 *
 *		2. the number of return arguments is not less than the number
 *		   of arguments anticipated.
 *
 *		3. the number and types of the arguments in 'a' correspond to
 *		   the expected arguments (the string argdesc). An 'x' 
 *		   character in argdesc accepts any type. If argdesc is
 *		   null, then the argument list is accepted unconditionally 
 *		   and it is up to the specific function to do whatever
 *		   type checking is required.
 *		
 *		If no match can be found, function checks to see if a match
 *		can be produced by converting data types. The following
 *		conversions are permitted:
 *
 *			integer to double
 *			double to integer
 *			integer to matrix
 *			double to matrix
 *
 *		Conversion is performed automatically. In some cases, however,
 *		it may not be desirable. Conversion is supressed if the data
 *		type character in argdesc is upper case instead of lower case.
 *
 *		Once function() finds the required entry point, it calls
 *		that entry point with the same 'r', 'n', 'a' arguments that
 *		it was called with. When the function returns, the types
 *		of the arguments are set according to the specification
 *		in the the return type string 'retdesc'.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1987
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include "cdefs.h"
#include "types.h"

typedef struct _funtab {
	char *retdesc;
	char *name;
	int (*function)();
	char *argdesc;
} FUNTAB;

extern sinM();
extern cosM();
extern tanM();
extern logM();
extern log10M();

extern expM();
extern sinhM();
extern coshM();
extern tanhM();
extern atanM();
extern asinM();
extern acosM();

extern sqrtM();
extern powM();
extern atan2M();
extern hypotM();

extern addDouble();
extern subtractDouble();
extern multiplyDouble();
extern divideDouble();
extern modDouble();
extern powerDouble();

extern addInteger();
extern subtractInteger();
extern multiplyInteger();
extern divideInteger();
extern modInteger();
extern powerInteger();

extern addMatrix();
extern subtractMatrix();
extern multiplyMatrix();

extern negateInteger();
extern negateDouble();
extern negateMatrix();
extern transposeMatrix();

extern scaleMatrix();
extern divideMatrix();
extern invertMatrix();
extern euclidianNorm();

extern trslTrans();
extern rotTrans();
extern rpyTrans();
extern eulTrans();
extern quaternionTrans();
extern vaoTrans();
extern forceTrans();
extern velocityTrans();
extern multInvMatrixLeft();
extern multInvMatrixRight();

extern identityMatrix();
extern zeroMatrix();
extern diagonalMatrix();
extern crossProduct();
extern transToRot();
extern transToRot2();
extern transToRpy();
extern transToEul();
extern transToQuaternion();

extern driveToTrans();
extern transToDrive();

extern setInput();
extern setOutput();
extern saveVariables();
extern loadVariables();
extern openLogFile();
extern closeLogFile();

int lastFxn()
{
}

/* test routine for a function returning two arguments */

futz (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
#define SIZE 6
	matrix *m1, *m2;
	int i;

	m1 = (matrix *)talloc_matrix (1,SIZE);
	m2 = (matrix *)talloc_matrix (SIZE,1);
	for (i=0; i<SIZE; i++)
 	 { m1->buf[i] = m2->buf[i] = i;
	 }
	r[0].val.m = m1;
	r[1].val.m = m2;
	return (1);
}

/* robot specific functions */

extern Jacobian();
extern forwardKinematics();
extern inverseKinematics();
extern getPosition();

FUNTAB funTable[] = { 

   "i",     "+", 	addInteger,	"II",
   "d",     "+",	addDouble,	"dd",
   "m",     "+", 	addMatrix,	"MM",

   "i",     "-", 	negateInteger,	"I",
   "i",     "-", 	subtractInteger,"II",
   "d",     "-",	subtractDouble, "dd",
   "d",     "-",	negateDouble,   "d",
   "m",     "-",	negateMatrix,   "M",
   "m",     "-", 	subtractMatrix, "MM",

   "i",     "*", 	multiplyInteger,"II",
   "d",     "*",	multiplyDouble, "dd",
   "m",     "*", 	multiplyMatrix, "MM",
   "m",     "*", 	scaleMatrix,	"dM",
   "m",     "*", 	scaleMatrix,	"Md",

   "i",     "/", 	divideInteger,	"II",
   "d",     "/",	divideDouble,   "dd",
   "m",     "/",	multInvMatrixRight,	"MM",
   "m",     "/",	divideMatrix,		"Md",
   "m",     "\\",	multInvMatrixLeft,	"MM",

   "i",     "%", 	modInteger,	"II",
   "d",     "%", 	modDouble,	"dd",

   "i",     "^", 	powerInteger,	"II",
   "d",     "^",	powerDouble,	"dd",

   "m",     "'",	transposeMatrix,"m",

   "mm",    "futz",	futz,		"i",

   "d",     "sin",	sinM, 		"d",
   "d",     "cos",      cosM, 		"d",
   "d",     "tan",      tanM, 		"d",
   "d",     "log",      logM, 		"d",
   "d",     "log10",    log10M, 	"d",
   "d",     "exp",      expM, 		"d",
   "d",     "sinh",     sinhM,		"d",
   "d",     "cosh",     coshM,		"d",
   "d",     "tanh",     tanhM,		"d",
   "d",     "atan",	atanM, 		"d",
   "d",     "asin",     asinM,		"d",
   "d",     "acos",	acosM,		"d",
   "d",     "sqrt",	sqrtM,		"d",
   "d",     "pow",	powM,		"dd",
   "d",     "atan2",	atan2M,		"dd",
   "d",     "hypot",	hypotM,		"dd",

   "m",	    "zero",     zeroMatrix,	"ii",
   "m",	    "diag",	diagonalMatrix, "m",
   "m",	    "ident",    identityMatrix, "i",
   "d",	    "enorm",    euclidianNorm,  "m",
   "m",	    "inv",	invertMatrix,	"m",
   "m",	    "cross",    crossProduct,   "mm",

   "m",     "rot",	rotTrans,	"dm",
   "m",     "rot",	rotTrans,	"m",
   "m",     "rpy",	rpyTrans,	"m",
   "m",     "eul",	eulTrans,	"m",
   "m",     "q",        quaternionTrans, "m",
   "m",	    "vao",	vaoTrans,	"mm",
   "m",     "trsl",	trslTrans,	"ddd",

   "m",     "trsfToDrive", transToDrive, "m",
   "m",     "driveToTrsf", driveToTrans, "m",

   "m",	    "ftrans",   forceTrans,	"m",
   "m",	    "vtrans",   velocityTrans,	"m",
   "m",	    "getrot",   transToRot,	"m",
   "m",	    "getrot2",   transToRot2,	"m",
   "m",	    "getrpy",   transToRpy,	"m",
   "m",	    "geteul",   transToEul,	"m",
   "m",     "getq",     transToQuaternion, "m",
   
   "i",     "input",    setInput,	"x",
   "i",     "output",   setOutput,	"x",
   "i",     "save",     saveVariables,  NULL,
   "i",     "load",     loadVariables,  "s",
   "i",     "log",	openLogFile,  "s",
   "i",	    "log",      closeLogFile, "",
#ifdef ROBOTS
   "m",	    "Jacobian", Jacobian, 	"sm",
   "ms",    "fwdK",     forwardKinematics,  "sm",
   "m",	    "invK",     inverseKinematics,  NULL,
   "m",	    "getpos",   getPosition,	"ss",
#endif
#if 0
/* Section containg RCCL equivalent names */
   "m",     "rotToTrsf",	rotTrans,	"dm",
   "m",     "rotToTrsf",	rotTrans,	"m",
   "m",     "rpy",		rpyTrans,	"m",
   "m",     "eul",		eulTrans,	"m",
   "m",     "q",        	quaternionTrans, "m",
   "m",	    "vao",		vaoTrans,	"mm",
   "m",     "trsl",		trslTrans,	"ddd",

   "m",     "trsfToDrive", 	transToDrive, "m",
   "m",     "driveToTrsf", 	driveToTrans, "m",

   "m",	    "ftrans",   	forceTrans,	"m",
   "m",	    "vtrans",   	velocityTrans,	"m",
   "m",	    "getrot",   	transToRot,	"m",
   "m",	    "getrot2",   	transToRot2,	"m",
   "m",	    "getrpy",   	transToRpy,	"m",
   "m",	    "geteul",   	transToEul,	"m",
   "m",     "getq",     	transToQuaternion, "m",
#endif
   "x",	    "x",	 	lastFxn,	"x",
};

#define MATCH		2
#define CONVERTIBLE 	1
#define NOMATCH 	0

function (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	char *name = r->name;
	int functionFound = 0;
	int returnValuesOk = 0;
	int numArgsOK = 0;
	int convertible = -1;
	int argStatus;

	int numReturnValues = r->val.i;	
	int i;
	
	for (i=0; funTable[i].function != lastFxn; i++)
	 { if (funTable[i].name[0] == name[0])	/* check 1st char for speed */
	    { if (strcmp (funTable[i].name, name) == 0)
	       { functionFound = i;
		
		 /* name has been found. Check number of return values */

		 if (strlen (funTable[i].retdesc) >= numReturnValues)
		  { returnValuesOk = i;
		
		    /* now check argument types */

		    argStatus = 0;
		    if (funTable[i].argdesc == NULL)
		     { argStatus = MATCH;
		       numArgsOK = i;
		     }
		    else 
		     { if (strlen (funTable[i].argdesc) == n)
		        { numArgsOK = i;
			  argStatus = argcheck (n, a, funTable[i].argdesc);
			}
		     }
		    /* if match, all the function; otherwise, if we can
		       convert the arguments, defer this untill we have
		       gone through the whole list */

		    switch (argStatus)
		     { case MATCH:
			{ if ((*funTable[i].function)(r, n, a) == -1)
			   { return (-1);
			   }
			  else
			   { char *retdesc;
			     int k;

			     retdesc = funTable[i].retdesc;

			     /* set return value types here, and change
				1X1 matrices into doubles */
			    
			     for (k=0; k<numReturnValues; k++)
			      { if (retdesc[k] == MATRIX &&
				    r[k].val.m->row == 1 &&
				    r[k].val.m->col == 1)
				 { r[k].val.d = r->val.m->buf[k];
				   r[k].type = DOUBLE;
				 }
				else
				 { r[k].type = retdesc[k];
				 }
			      }
			     return (1);
			   }
			  break;
			}
		       case CONVERTIBLE:
			{ if (convertible == -1)
			   { convertible = i;
			   }
			  break;
			}
		       default:
			{ break;
			}
		     }
		  }
	       }
	    }
	 }
	if (funTable[i].function == lastFxn)
	 { if (convertible != -1)
	    { 
	      /* if the argument list is conevrtible, do the conversion
		 and call the function here. */

	      convert (n, a, funTable[convertible].argdesc);
	      if ((*funTable[convertible].function)(r, n, a) == -1)
	       { return (-1);
	       }
	      else
	       { char *retdesc;
		 int k;

		 /* set return value types here, and change
		    1X1 matrices into doubles */

		 retdesc = funTable[convertible].retdesc;
		
	         for (k=0; k<numReturnValues; k++)
		  { if (retdesc[k] == MATRIX &&
		        r[k].val.m->row == 1 &&
		        r[k].val.m->col == 1)
		     { r[k].val.d = r->val.m->buf[k];
		       r[k].type = DOUBLE;
		     }
		    else
		     { r[k].type = retdesc[k];
		     }
		  }
	         return (1);
	       }
	    }
	   else if (numArgsOK)
	    { error (0, "illegal type(s) specified for function %s\n", name);
	      return (-1);
	    }
	   else if (returnValuesOk)
	    { error (0, "illegal number of arguments for function %s\n", 
		 	name);
	      return (-1);
	    }
	   else if (functionFound)
	    { error (0, "function %s does not return %d value%s\n",
			name, numReturnValues, (numReturnValues != 1) ? "s" : "");
	      return (-1);
	    }
	   else
	    { error (0, "function %s not defined\n", name);
	      return (-1);
	    }
	 }
}

/* checks the argument list 'a' (length 'n') for a function to see if it 
   matches the types given in the argument specification string 'argdesc' */

argcheck (n, a, argdesc)
int n;
GTYPE *a;
char *argdesc;
{
	int i, result;

	if (n == strlen (argdesc))
	 { result = MATCH;
	   for (i=0; i<n; i++)
	    { if (argdesc[i] == 'x' || argdesc[i] == a[i].type ||
		  argdesc[i] == a[i].type + ('A'-'a'))
	       { continue;
	       }
	      else if ((argdesc[i] == INTEGER && a[i].type == DOUBLE) ||
		       (argdesc[i] == DOUBLE  && a[i].type == INTEGER) ||
		       (argdesc[i] == MATRIX  && a[i].type == DOUBLE) ||
		       (argdesc[i] == MATRIX  && a[i].type == INTEGER))
	       { result = CONVERTIBLE;
	       }
	      else
	       { return (NOMATCH);
	       }
	    }
	   return (result);
	 }
	return (NOMATCH);
}

matrix *tdouble_to_matrix();

/* converts the argument list 'a' (length 'n') for a function into 
   something that matches the types given in the argument specification
   string 'argdesc' */

convert (n, a, argdesc)
int n;
GTYPE *a;
char *argdesc;
{
	int i;

	for (i=0; i<n; i++)
	 { if (argdesc[i] == INTEGER && a[i].type == DOUBLE)
	    { a[i].type = INTEGER;
	      a[i].val.i = a[i].val.d;
	    }
	   else if (argdesc[i] == DOUBLE && a[i].type == INTEGER)
	    { a[i].type = DOUBLE;
	      a[i].val.d = a[i].val.i;
	    }
	   else if (argdesc[i] == MATRIX && a[i].type == DOUBLE)
	    { a[i].type = MATRIX;
	      a[i].val.m = tdouble_to_matrix (a[i].val.d);
	    }
	   else if (argdesc[i] == MATRIX && a[i].type == INTEGER)
	    { a[i].type = MATRIX;
	      a[i].val.m = tdouble_to_matrix ((double)a[i].val.i);
	    }
	 }
}
