/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/* code to read an ascii JLS file into a JLS data structure */

#define NEW_ENCOFFSET_FORMAT	1

#include <stdio.h>
#include <ctype.h>
#include <math.h>

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

#include "matrix.h"

int lineNo = 0;
int maybeOldFormat;

#define READVAL(s, x) \
	if (readValue (fp, (s), (void*)(x)) != 1) \
	 { return(-1); \
	 }

readValue(fp, s, v)
FILE *fp;
char *s;
void *v;
{
	if (read_conf (fp, s, v) != 1)
	 { if (maybeOldFormat)
	    { errorMsg (ESyntax,
"Line %d -- error scanning input. 'encOffset' field may be in old format:\n\
it now needs both encoder AND angle values. Compare with other .jls files.\n",
		  	lineNo);
	      return (0);
	    }
	   else
	    { errorMsg (ESyntax,
"Line %d -- error scanning input\n", lineNo);
	      return (0);
	    }
	 }
	return (1);
}

readJls (jls, fp)
JLS *jls;
FILE *fp;
{
	int i, j, k;
	int numJ, numFC, numIC;
	float fval;

	static matrix *g  = NULL;
	static matrix *gi = NULL;

	int wheelSize[MAXJNTS];
	char couplings[MAXJNTS][MAXJNTS];
	char rotational[MAXJNTS];
	float encOffsetAngles[MAXJNTS];
	int encOffsetEncoders[MAXJNTS];
	int encTmp[MAXJNTS];

	lineNo = 0;

#if 0
	READVAL ("%s", nameBuf);
	if ((jls->class = rbtGetClassByName(nameBuf)) == UNDEF)
	 { fatalMsg (-1, 
"robot class '%s' unknown; check tables in robotDataTab.c\n", nameBuf);
	 }
	READVAL ("%s", nameBuf);
	if ((jls->class = rbtGetTypeByName(nameBuf)) == UNDEF)
	 { fatalMsg (-1, 
"robot type '%s' unknown; check tables in robotDataTab.c\n", nameBuf);
	 }
#endif
	READVAL ("%d", &numJ);
	if (numJ < 0 || numJ > MAXJNTS)
	 { errorMsg (ERange, 
"Line %d -- no. of joints must be between 0 and %d\n", lineNo, MAXJNTS);
	   return (-1);
	 }
	jls->numJ = numJ;

	/* ---- determine which joints are rotationsal ---- */

	for (j=0; j<numJ; j++)
	 { char key[2];
	   READVAL ("%1s", key)
	   if (strcmp (key, "R") == 0)
	    { rotational[j] = 1;
	      jls->jtype[j] = 'R';
	    }
	   else if (strcmp (key, "P") == 0)
	    { rotational[j] = 0;
	      jls->jtype[j] = 'P';
	    }
	   else
	    { errorMsg (ERange, 
"Line %d -- joint types must be 'R' or 'P'\n", lineNo);
	      return (-1);
	    }
	 }
	jls->jtype[numJ] = '\0';

	/* ---- now set up the forward couplings ---- */

	READVAL ("%d", &numFC);
	if (numFC < 0 || numFC > MAXC)
	 { errorMsg (ERange, 
"Line %d -- no. of forward couplings must be between 0 and %d\n",lineNo,MAXC);
	   return (-1);
	 }
	jls->numFC = numFC;

	bzero (couplings, MAXJNTS*MAXJNTS);
	for (k=0; k<numFC; k++)
	 { READVAL ("%d", &i)
	   READVAL ("%d", &j);
	   i--; j--;
	   if (i < 0 || i >= numJ || j < 0 || j >= numJ)
	    { errorMsg (ERange,
"Line %d -- forward coupling indicies must be between 1 and %d",lineNo,numJ);
	      return (-1);
	    }
	   if (i == j)
	    { errorMsg (ERange,
"Line %d -- coupling element (%d, %d) lies on diagonal\n",lineNo,i+1,j+1);
	      return (-1);
	    }
	   if (couplings[i][j])
	    { errorMsg (ERange,
"Line %d -- coupling element (%d, %d) multiply defined\n",lineNo,i+1,j+1);
	      return (-1);
	    }
	   couplings[i][j] = 1;
	 }
	k = 0;
	if (numFC)
	 { for (i=0; i<numJ; i++)
	    { for (j=0; j<numJ; j++)
	       { if (couplings[i][j])
		  { jls->forCouplings[k][0] = i;
		    jls->forCouplings[k][1] = j;
		    k++;
		  }
	       }
	    }
	 }
	if (k != numFC)
	 { errorMsg (EInternal, 
"Line %d -- internal error: numFC = %d, k = %d\n", lineNo, numFC, k);
	   return (-1);
	 }

	/* ---- now set up the inverse couplings ---- */

	READVAL ("%d", &numIC);
	if (numIC < 0 || numIC > MAXC)
	 { errorMsg (ERange,
"Line %d -- no. of inverse couplings must be between 0 and %d\n",lineNo,MAXC);
	   return (-1);
	 }
	jls->numIC = numIC;

	bzero (couplings, MAXJNTS*MAXJNTS);
	for (k=0; k<numIC; k++)
	 { READVAL ("%d", &i)
	   READVAL ("%d", &j);
	   i--; j--;
	   if (i < 0 || i >= numJ || j < 0 || j >= numJ)
	    { errorMsg (ERange,
"Line %d -- inverse coupling indicies must be between 1 and %d",lineNo,numJ);
	      return (-1);
	    }
	   if (i == j)
	    { errorMsg (ERange,
"Line %d -- coupling element (%d, %d) lies on diagonal\n",lineNo,i+1,j+1);
	      return (-1);
	    }
	   if (couplings[i][j])
	    { errorMsg (ERange,
"Line %d -- coupling element (%d, %d) multiply defined\n",lineNo,i+1,j+1);
	      return (-1);
	    }
	   couplings[i][j] = 1;
	 }
	k = 0;
	if (numIC)
	 { for (i=0; i<numJ; i++)
	    { for (j=0; j<numJ; j++)
	       { if (couplings[i][j])
		  { jls->invCouplings[k][0] = i;
		    jls->invCouplings[k][1] = j;
		    k++;
		  }
	       }
	    }
	 }
	if (k != numIC)
	 { errorMsg (EInternal, 
"Line %d -- internal error: numIC = %d, k = %d\n", lineNo, numIC, k);
	   return (-1);
	 }

	/* ---- read in the forward gear ratios and couplings ---- */


	if (g)
	 { free (g);
	 }
	if (gi)
	 { free (gi);
	 }
	g  = mallocMatrix (numJ, numJ);
	gi = mallocMatrix (numJ, numJ);

	for (j=0; j<numJ; j++)
	 { float x;
	   READVAL ("%f", &x)
	   ELEMENT (g, j, j) = x;
	 }
	for (k=0; k<numFC; k++)
	 { float x;
	   READVAL ("%f", &x);
	   ELEMENT (g, jls->forCouplings[k][0],
		       jls->forCouplings[k][1]) = x;
	 }
	
	if (invertMatrix (gi, g) == NULL)
	 { errorMsg (ERange, 
"Line %d -- can't invert gear ratio matrix\n", lineNo);
	   return (-1);
	 }

#if NEW_ENCOFFSET_FORMAT
	for (j=0; j<numJ; j++)
	 { int encval;
	   float angval;

	   READVAL ("%d", &encval);
	   READVAL ("%f", &angval);
	   encOffsetEncoders[j] = encval;
	   if (jls->jtype[j] == 'R')
	    { encOffsetAngles[j] = angval*DEGTORAD;
	    }
	   else
	    { encOffsetAngles[j] = angval;
	    }
	   /* check for possible old format */
	   if (jls->jtype[j] == 'J' && ABS(encval) > 1000.0)
	    { maybeOldFormat = 1;
	    }
	   if (j > 2 && (angval == 1000 || angval == 500 || angval == 800))
	    { maybeOldFormat = 1;
	    }
	 }
#else
	for (j=0; j<numJ; j++)
	 { READVAL ("%d", &jls->encOffset[j])
	 }
#endif
	for (j=0; j<numJ; j++)
	 { int size;
	   READVAL ("%d", &size)
	   wheelSize[j] = size;
	 }

	for (j=0; j<numJ; j++)
	 { double x;
	   
	   jls->encWheelSize[j] = wheelSize[j];

	   x = ELEMENT(g, j, j);
	   jls->forGearRatios[j] = x;
	   jls->angtoenc[j] = x * wheelSize[j] / (2*PI);
	 }
	for (k=0; k<numFC; k++)
	 { double x;

	   i = jls->forCouplings[k][0];
	   j = jls->forCouplings[k][1];

	   x = ELEMENT(g, i, j);
	   jls->forGearRatios[numJ+k] = x;
	   jls->angtoenc[numJ+k] = x * wheelSize[i] / (2*PI);
	 }
	for (j=0; j<numJ; j++)
	 { double x;

	   x = ELEMENT(gi, j, j);
	   jls->invGearRatios[j] = x;
	   jls->enctoang[j] = x * (2*PI) / wheelSize[j];
	 }
	for (k=0; k<numIC; k++)
	 { double x;

	   i = jls->invCouplings[k][0];
	   j = jls->invCouplings[k][1];

	   x = ELEMENT(gi, i, j);
	   jls->invGearRatios[numJ+k] = x;
	   jls->enctoang[numJ+k] = x * (2*PI) / wheelSize[j];
	 }

#if NEW_ENCOFFSET_FORMAT
	jointToEncoder (encTmp, encOffsetAngles, jls);
	for (j=0; j<numJ; j++)
	 { jls->encOffset[j] = encOffsetEncoders[j] - encTmp[j];
	 }
#endif
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &jls->mtorqueToCurrent[j])
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &jls->currentToMtorque[j])
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%d", &jls->currentBias[j])
	 }

	for (j=0; j<numJ; j++)
	 { READVAL ("%d", &jls->encCalvec[j])
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%d", &jls->encHome[j])
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &jls->potSlope[j])
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &jls->potIntercept[j])
	 }

	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval)
	   jls->jmin[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval)
	   jls->jmax[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	   jls->jrng[j] = jls->jmax[j] - jls->jmin[j];
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval)
	   jls->maxVel[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval)
	   jls->maxAcc[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval)
	   jls->maxJerk[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }

	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &jls->maxMtorque[j])
	 }
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &jls->maxJtorque[j])
	 }

	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval);
	   jls->parkAngles[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }

	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval);
	   jls->bestAngles[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }

	fval = 0.0;
	for (j=0; j<numJ; j++)
	 { fval += jls->forGearRatios[j] * jls->forGearRatios[j];
	 }
	jls->averageGearRatio = sqrt(fval/numJ);
	
	fval = 0.0;
	for (j=0; j<numJ; j++)
	 { fval += jls->angtoenc[j] * jls->angtoenc[j];
	 }
	jls->averageAngtoenc = sqrt(fval/numJ);
	
	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval);
	   jls->nominalVel[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }

	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval);
	   jls->nominalAcc[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }

	for (j=0; j<numJ; j++)
	 { READVAL ("%f", &fval);
	   jls->nominalJerk[j] = (rotational[j]) ? fval*DEGTORAD : fval;
	 }

	fval = 0.0;
	for (j=0; j<numJ; j++)
	 { fval += jls->nominalVel[j] * jls->nominalVel[j];
	 }
	jls->averageVel = sqrt(fval/numJ);

	fval = 0.0;
	for (j=0; j<numJ; j++)
	 { fval += jls->nominalAcc[j] * jls->nominalAcc[j];
	 }
	jls->averageAcc = sqrt(fval/numJ);

	fval = 0.0;
	for (j=0; j<numJ; j++)
	 { fval += jls->nominalJerk[j] * jls->nominalJerk[j];
	 }
	jls->averageJerk = sqrt(fval/numJ);

	READVAL ("%lx", &jls->flags);
	return (1);
}

#if 0

readEncOffsets(enc, ang)
float *enc;
float *ang;
{
	int oldLineNo;

	   /* first, make sure we are in the new format */
	if (read_conf (fp, "%f", &enc[0]) != 1)
	 { goto encOffSyntaxErr;
	 }
	oldLineNo = lineNo;
	if (read_conf (fp, "%f", &ang[0]) != 1)
	 { goto encOffSyntaxErr;
	 }
	if (oldLineNo == lineNo)
	 { 
	 }
	for (j=0; j<numJ; j++)
	 { if (read_conf (fp, "%d", &encOffsetAngles[j]) != 1)
	    { errorMsg (ESyntax,
"Line %d -- error scanning encOffsetAngles\n", lineNo);
	      return (-1);
	    }
	 }
	return (1);
encOffSyntaxErr:
	errorMsg (ESyntax,
"Line %d -- error scanning encOffsetAngles\n", lineNo);
	return (-1);
}

#endif

read_conf (fp, s, x)
FILE *fp;
char *s;
void *x;
{
	int c;

	while (1)
	 { while (isspace(c = getc(fp)))
	    { if (c == '\n')
	       { lineNo++;
	       }
	    }
	   if (c == '#')
	    { while (getc (fp) != '\n')
	         ;
	      lineNo++;
	    }
	   else
	    { ungetc(c, fp);
	      return (fscanf (fp, s, x));
	    }
	 }
}

printJls (jls)
JLS *jls;
{
	register int i, j, k;

	int numJ;
	int numFC;
	int numIC;

	numJ = jls->numJ;
	numFC = jls->numFC;
	numIC = jls->numIC;

	printf ("   JNT     forGearRatio         angtoenc\n");
/*	             XX  123456789012345  123456789012345 */

	for (j=0; j<numJ; j++)
	 { printf ("    %2d  %15g  %15g\n",
		   j+1, jls->forGearRatios[j], jls->angtoenc[j]);
	 }
	putchar ('\n');

	for (k=0; k<numFC; k++)
	 { i = jls->forCouplings[k][0];
	   j = jls->forCouplings[k][1];
	   printf (" %2d %2d  %15g  %15g\n", 
		i+1, j+1, jls->forGearRatios[numJ+k], jls->angtoenc[numJ+k]);
	 }
	putchar ('\n');
	printf ("   JNT     invGearRatio         enctoang\n");

	for (j=0; j<numJ; j++)
	 { printf ("    %2d  %15g  %15g\n",
		   j+1, jls->invGearRatios[j], jls->enctoang[j]);
	 }
	putchar ('\n');
	for (k=0; k<numIC; k++)
	 { i = jls->invCouplings[k][0];
	   j = jls->invCouplings[k][1];
	   printf (" %2d %2d  %15g  %15g\n", 
		i+1, j+1, jls->invGearRatios[numJ+k], jls->enctoang[numJ+k]);
	 }
	putchar ('\n');
	printf(
     "   JNT   offset wheelSize   mtorToCur   curToMtor  curBias\n");
/*        XX  XXXXXXX   XXXXXXX  1234567890  1234567890  XXXXXXX */
	for (j=0; j<numJ; j++)
	 { printf ("    %2d  %7d   %7d  %10g  %10g  %10d\n", j+1,
		     jls->encOffset[j], jls->encWheelSize[j],
		     jls->mtorqueToCurrent[j], jls->currentToMtorque[j],
		     jls->currentBias[j]);
	 }
	putchar ('\n');
	printf ("   JNT   calvec      home    potSlope   intercept\n");
/*                   XX  XXXXXXX   XXXXXXX  1234567890  1234567890 */
	for (j=0; j<numJ; j++)
	 { printf ("    %2d  %7d   %7d  %10g  %10g\n", j+1,
		     jls->encCalvec[j], jls->encHome[j],
		     jls->potSlope[j], jls->potIntercept[j]);
	 }
	putchar ('\n');
	printf (
"   JNT      jmin      jmax    maxVel    maxAcc   maxJerk\n");
/*   XX  XXXXX.XX  XXXXX.XX  XXXXX.XX  XXXXX.XX  XXXXX.XX */
	for (j=0; j<numJ; j++)
	 { printf ("    %2d  %8.2f  %8.2f  %8.2f  %8.2f  %8.2f\n", j+1,
	    (jls->jtype[j]=='R') ? RADTODEG*jls->jmin[j] : jls->jmin[j],
	    (jls->jtype[j]=='R') ? RADTODEG*jls->jmax[j] : jls->jmax[j],
	    (jls->jtype[j]=='R') ? RADTODEG*jls->maxVel[j] : jls->maxVel[j],
	    (jls->jtype[j]=='R') ? RADTODEG*jls->maxAcc[j] : jls->maxAcc[j],
	    (jls->jtype[j]=='R') ? RADTODEG*jls->maxJerk[j]: jls->maxJerk[j]);
	 }
	putchar ('\n');
	printf (
"   JNT     maxMtor     maxJtor   parkPos   bestPos\n");
/*   XX  1234567890  1234567890  XXXXX.XX  XXXXX.XX*/
	for (j=0; j<numJ; j++)
	 { printf ("    %2d  %10g  %10g  %8.2f  %8.2f\n", j+1,
		jls->maxMtorque[j], jls->maxJtorque[j],
		(jls->jtype[j]=='R') ? RADTODEG*jls->parkAngles[j] :
					 jls->parkAngles[j],
		(jls->jtype[j]=='R') ? RADTODEG*jls->bestAngles[j] :
					 jls->bestAngles[j]);
	 }

	putchar ('\n');
	printf (
"   JNT  nominalVel  nominalAcc nominalJerk\n");
/*   XX  XXXXX.XXXX  XXXXXX.XXX  XXXXXXX.XX */
	for (j=0; j<numJ; j++)
	 { printf ("    %2d  %10.4f  %10.3f  %10.2f\n", j+1,
		(jls->jtype[j]=='R') ? RADTODEG*jls->nominalVel[j] :
					 jls->nominalVel[j],
		(jls->jtype[j]=='R') ? RADTODEG*jls->nominalAcc[j] :
					 jls->nominalAcc[j],
		(jls->jtype[j]=='R') ? RADTODEG*jls->nominalJerk[j] :
					 jls->nominalJerk[j]);
	 }

	printf ("av. gear ratio = %g\n", jls->averageGearRatio);
	printf ("av. angtoenc = %g\n", jls->averageAngtoenc);
	printf ("av. vel = %g\n", jls->averageVel);
	printf ("av. acc = %g\n", jls->averageAcc);
	printf ("av. jerk = %g\n", jls->averageJerk);
	printf ("flags = 0x%lx\n", jls->flags);
}
