/*-=-=-=-=-=-=-=-=-=-=-=- 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:	puma
 *
 * DESCRIPTION:	functions for computing the kinematics for the PUMA
 *		robot.
 * 
 * AUTHOR/DATE:	John Lloyd, Winter 1988
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

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

#include "../h/robot.h"
#include "../h/robot_jls.h"
#include "../h/robot_kynvar.h"

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

setupRobot (func, name)
char *func, *name;
{
	if ((jls = getJls (name)) == NULL)
	 { error (0, "%s() -- robot '%s' not supported\n",
		     func, name);
	   return (-1);
	 }
	if ((kyn = getKyn (name, NULL)) == NULL)
	 { error (0, "%s() -- robot '%s' not supported\n",
		     func, name);
	   return (-1);
	 }
	var = (void*)malloc(getVarSize(name));
	return (1);
}

Jacobian (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *angleVec;
	matrix *jacob;
	float ang[MAXJNTS];
	float unitVec[MAXJNTS];
	float jCol[6];

	int i, j;

	angleVec = a[1].val.m;

	if ((angleVec->row != 1 || angleVec->col != 6) &&
	    (angleVec->row != 6 || angleVec->col != 1))
	 { error (0, "%s() -- 2nd argument must be 6 vector of angles\n", 
	   r->name);
	   return (-1);
	 }
	if (setupRobot (r->name, a[0].val.s) < 0)
	 { return (-1);
	 }
	if ((jacob = talloc_matrix (6, jls->numJ)) == NULL)
	 { return (-1);
	 }
	for (i=0; i<6; i++)
	 { for (j=0; j<jls->numJ; j++)
	    { ELEMENT (jacob, i, j) = 0.0;
	    }
	 }

	for (j=0; j<jls->numJ; j++)
	 { float cvt = (jls->jtype[j] == 'R' ? DEGTORAD : 1.0);
	   ang[j] = cvt * ELEMENT(angleVec,0,j);
	   unitVec[j] = 0.0;
	 }
	updateVar (var, ang, kyn);

	/* now go through and compute each column of the Jacobian */

	for (j=0; j<jls->numJ; j++)
	 { if (j > 0)
	    { unitVec[j-1] = 0.0;
	    }
	   unitVec[j] = 1.0;
	   fwdJacobVar (jCol, unitVec, var, kyn);
	   for (i=0; i<6; i++)
	    { ELEMENT (jacob, i, j) = jCol[i];
	    }
	 }
	r->val.m = jacob;
	return (1);
}

fwdKinematics (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *angleVec;
	matrix *t6;
	float ang[MAXJNTS];
	TRSF trsf;
	char *confstr;
	unsigned long conf;
	int j;

	angleVec = a[1].val.m;

	if ((angleVec->row != 1 || angleVec->col != 6) &&
	    (angleVec->row != 6 || angleVec->col != 1))
	 { error (0, "%s() -- 2nd argument must be 6 vector of angles\n", 
	   r->name);
	   return (-1);
	 }
	if (setupRobot (r->name, a[0].val.s) < 0)
	 { return (-1);
	 }
	if ((t6 = (matrix*)talloc_homo()) == NULL)
	 { return (-1);
	 }
	if ((confstr = (char*)talloc(4)) == NULL)
	 { return (-1);
	 }
	for (j=0; j<jls->numJ; j++)
	 { float cvt = (jls->jtype[j] == 'R' ? DEGTORAD : 1.0);
	   ang[j] = cvt * ELEMENT(angleVec,0,j);
	 }
	fwdKinematics (&trsf, &conf, ang, kyn, NULL);

	ELEMENT(t6, 0, 0) = trsf.n.x;
	ELEMENT(t6, 1, 0) = trsf.n.y;
	ELEMENT(t6, 2, 0) = trsf.n.z;

	ELEMENT(t6, 0, 1) = trsf.o.x;
	ELEMENT(t6, 1, 1) = trsf.o.y;
	ELEMENT(t6, 2, 1) = trsf.o.z;

	ELEMENT(t6, 0, 2) = trsf.a.x;
	ELEMENT(t6, 1, 2) = trsf.a.y;
	ELEMENT(t6, 2, 2) = trsf.a.z;

	ELEMENT(t6, 0, 3) = trsf.p.x;
	ELEMENT(t6, 1, 3) = trsf.p.y;
	ELEMENT(t6, 2, 3) = trsf.p.z;

	pumaConfigToStr (confstr, conf);

	r[0].val.m = t6;
	r[1].val.s = confstr;
}

invKinematics (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *angleVec;
	matrix *t6;
	float refj[MAXJNTS];
	float ang[MAXJNTS];
	TRSF trsf;
	char *confstr;
	unsigned long conf;
	int j, status;

	if (n != 3 && n != 4)
	 { error (0, "%s() needs either 3 or 4 arguments\n", r->name);
	   return (-1);
	 }
	if (a[0].type != STRING)
	 { error (0, "%s() -- first argument must be robot name\n", r->name);
	   return (-1);
	 }
	if (setupRobot (r->name, a[0].val.s) < 0)
	 { return (-1);
	 }
	t6 = a[1].val.m;
	if (a[1].type != MATRIX ||
	    (t6->row != 4 || t6->col != 4))
	  { error (0, "%s() -- 2nd argument must be 4 X 4 transform\n", 
		   r->name);
	   return (-1);
	 }
	if (a[2].type != STRING)
	 { error (0, "%s() -- 3rd argument must be configuration string\n",
		  r->name);
	   return (-1);
	 }
	if (strlen (a[2].val.s) != 3 || 
	    (pumaStrToConfig (&conf, a[2].val.s) == -1))
	 { error (0, "%s() -- invalid configuration string\n", r->name);
	   return (-1);
	 }
	if (n == 4)
	 { angleVec = a[3].val.m;
	   if (a[3].type != MATRIX ||
	       (angleVec->row != 1 || angleVec->col != 6) &&
	       (angleVec->row != 6 || angleVec->col != 1))
	    { error (0, 
	"%s() -- optional 4th argument must be 6 vector of angles\n",r->name);
	      return (-1);
	    }
	   for (j=0; j<jls->numJ; j++)
	    { float cvt = (jls->jtype[j] == 'R' ? DEGTORAD : 1.0);
	      refj[j] = cvt*ELEMENT(angleVec, 0, j);
	    }
	 }
	else
	 { for (j=0; j<jls->numJ; j++)
	    { refj[j] = (jls->jmin[j] + jls->jmax[j])/2.0;
	    }
	 }
	if ((angleVec = talloc_matrix(jls->numJ, 1)) == NULL)
	 { return (-1);
	 }

	trsf.n.x = ELEMENT(t6, 0, 0);
	trsf.n.y = ELEMENT(t6, 1, 0);
	trsf.n.z = ELEMENT(t6, 2, 0);

	trsf.o.x = ELEMENT(t6, 0, 1);
	trsf.o.y = ELEMENT(t6, 1, 1);
	trsf.o.z = ELEMENT(t6, 2, 1);

	trsf.a.x = ELEMENT(t6, 0, 2);
	trsf.a.y = ELEMENT(t6, 1, 2);
	trsf.a.z = ELEMENT(t6, 2, 2);

	trsf.p.x = ELEMENT(t6, 0, 3);
	trsf.p.y = ELEMENT(t6, 1, 3);
	trsf.p.z = ELEMENT(t6, 2, 3);

	status = invKinematics (ang, &trsf, conf, refj, kyn, jls, NULL);

	if (status & KYN_CANT_REACH)
	 { error (0, "%s() -- position is out of reach\n", r->name);
	   return (-1);
	 }
	if (status & KYN_JOINT_LIMIT)
	 { error (0, "%s() -- position is out of range of joints 0x%x\n",
		  r->name, status & KYN_JOINT_LIMIT);
	   return (-1);
	 }
	if (status & KYN_SINGULAR)
	 { printf ("warning: %s() -- robot is at a singularity, code 0x%x\n",
		  r->name, status & KYN_SINGULAR);
	 }
	for (j=0; j<jls->numJ; j++)
	 { float cvt = (jls->jtype[j] == 'R' ? RADTODEG : 1.0);
	   ELEMENT (angleVec, 0, j) = cvt*ang[j];
	 }
	r[0].val.m = angleVec;
}

getPosition (r, n, a)
GTYPE *r;
int n;
GTYPE *a;
{
	matrix *angleVec;
	float ang[MAXJNTS];
	int j;

	if ((angleVec = talloc_matrix(jls->numJ, 1)) == NULL)
	 { return (-1);
	 }
	if (getRobotPosition (ang, a[1].val.s, a[0].val.s) < 0)
	 { error (0, "%s() -- position not found\n", r->name);
	   return (-1);
	 }
	for (j=0; j<jls->numJ; j++)
	 { float cvt = (jls->jtype[j] == 'R' ? RADTODEG : 1.0);
	   ELEMENT (angleVec, 0, j) = cvt*ang[j];
	 }
	r[0].val.m = angleVec;
	return (1);
}
