/*-=-=-=-=-=-=-=-=-=-=-=- 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 ~=~=~=~=
 *		
 * NAME:	potcal
 *
 * USAGE:	potcal
 *
 * DESCRIPTION: An interactive program used for determining the relationship
 *		between joint potentiometer readings and encoder values on
 *		the PUMA 500 and 700 series robots.
 * 
 * 	        Check the RCI Reference manual for a description of how
 *		to operate the program.
 *
 * AUTHOR/DATE: John Lloyd, Fall 1986
 *
 * UPDATES:	John Lloyd, July 30, 1990
 *		Added control level code to make the sampling operation
 *		continuous. Also modified the interface to make things 
 *		a little easier to use.
 *		Code used before the change is blocked off by '#if OLD'
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include <signal.h>

#ifdef VXWORKS_CFG
#define main potcal_
#endif

#include <strings.h>
#include "../h/cdefs.h"
#include "../h/rciTask.h"
#include "../h/rciMacros.h"
#include "../h/chase.h"
#include "../h/robotMath.h"
#include "../h/rciOptions.h"
#include "../h/allocMem.h"

#include "macros.h"
#include "potcal.h"

#if OLD
#define NUM_STOPS	16
#define MAX_STOPS	256
static float potReadings[MAXJNTS][MAX_STOPS];
static float potEncoders[MAXJNTS][MAX_STOPS];
#endif

int numJoints;

RCI_DESC *td;
RCI_RBT *rbt;
JLS *jls;
HOW *how;
CHASE_CTRL *cb;
POTCAL *pc;
FILE *fp;

static char *outputFile = "potcal.out";

extern float speedSet();
extern potvalMonitor();
extern char *jointDescStr();

char *getDefaultRobot();

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf[256];
char jstrbuf[64];

#define MARGIN	(5.0*DEGTORAD)

int wristJoints[] = {4, 5, 6};	/* defines the wrist joints */
int testMode = NO;
float travelSpeed;
float sampleSpeed; 

main(argc, argv)
int argc;
char **argv;
{       
	char *robotName = NULL;

	int simflag = NO;
	char *keytree, *tree_match_parse(), *tree_match_err_at();
	int ival, jnt, j, k;
	float fval;
	float sweepAngle;
	int sweepAngleSet;
	unsigned long calibrateRequest;
	int turnPowerOff = YES;

	float startAngles[MAXJNTS];
	float stopAngles[MAXJNTS];
	float deltaAngles[MAXJNTS];

#if OLD
	int numStops = NUM_STOPS;
#endif

	/* ---- first, parse the command line ---- */

	argc--; argv++;
	while (argc)
	 { 
	   if (robotName == NULL)
	    { robotName = *argv;
	    }
	   else if (ARGUMENTS ("-sim="))
	    { char *sname;
	      if (strlen(sname=index(*argv,'=')+1) == 0)
	       { argc = -1;
		 break;
	       }
	      setSimulator(sname);
	      simflag = YES;
	    }
	   else if (ARGUMENT ("-sim"))
	    { simflag = YES;
	    }
	   else if (ARGUMENT("-on"))
	    { turnPowerOff = NO;
	    }	   
	   else if (ARGUMENT ("-t"))
	    { testMode = YES;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { usageMsg (-1, 
"potcal [<robotName>] [-sim[=<simulatorAddr>]] [-on]\n");
	 }
	if (!robotName)
	 { robotName = getDefaultRobot();
	 }
	travelSpeed = 15.0*DEGTORAD;
	sampleSpeed =  2.5*DEGTORAD;
	sweepAngleSet = NO;

	keytree = tree_match_parse("					\
(	help								\
	calibrate							\
	 ([1 2 3 4 5 6]							\
	  wrist)							\
	show								\
	angle								\
	 ([1 2 3 4 5 6]							\
	   ([to by]							\
	     (%f", &fval, ",,,\"angle (degrees)\")))			\
	move								\
	 (to								\
	   (midpoint))							\
	stop								\
	set								\
	 ([travel sample]						\
	   (speed							\
	     (%f", &fval, ",,,	\"degrees/sec.\"))			\
	  sweepAngle							\
	   (%f", &sweepAngle, ",0,, \"sweep angle (degrees)\")		\
	 )								\
	unset								\
	 (sweepAngle)							\
	quit								\
)",0);
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	if ((fp = fopen (outputFile, "w")) == NULL)
	 { printf ("ERROR: can't open output file %s\n", outputFile);
	   exit (-1);
	 }
	printf ("\ncomputed values will be written into the file %s\n\n",
		outputFile);	

	/* ---- set up the RCI control task and memory ---- */

	td = (RCI_DESC*)createChaseTask(robotName, simflag);

	rbt = (RCI_RBT *)TASKROBOT(td, 0);
	how = rbt->how;
	jls = rbt->jls;
	numJoints = jls->numJ;

	cb = (CHASE_CTRL*)chaseTaskMemory(td);
	pc = ALLOC_MEM (NULL, POTCAL, UNDEF);

	cb->controlLevelChange = JOINT_LEVEL;

	copyVf (cb->angAccel, jls->nominalAcc, numJoints);
	scaleVf(cb->encAccel, jls->averageAngtoenc,jls->nominalAcc,numJoints);
	speedSet (cb, jls, travelSpeed);

	SET_CHECKING (rbt, CHK_MAXVEL | CHK_MAXTOR | CHK_REQTOR | CHK_REQVEL);

	if (startChaseTask(td) < 0)
	 { RCI_EXIT (-1);
	 }
	rciBlock(td);

	if (!(how->options & POTS_AVAIL) && !testMode)
	 { printf ("ERROR -- this robot does not have potentiometers\n");
	   rciClose();
	   exit (-1);
	 }
	if (!how->calibrated)
	 { printf ("ERROR -- robot reports that it is not calibrated\n");
	   rciClose();
	   exit (-1);
	 }

/*	SET_CHECKING (rbt, CHK_RBTALL); */

	if (!testMode)
	 { SET_POT_READ (rbt, ~0);
	 }
	if (!turnPowerOff)
	 { SET_POWER_ON_RELEASE (rbt, 1) ;
	 }
	powerUpChaseTask (td, YES);
	WAITFOR (cb->compcode == ALL_JOINTS);
	SET_INTEGRATION (rbt, ~0);

	tree_match_catch_sig (0, sigmask(SIGALRM));

	do
	 { calibrateRequest = 0;

	   tree_match (keytree, "POTCAL> ", key_buf);

	   if (COMMANDS ("angle"))
	    { 
	      jnt = key_buf[strlen ("angle ")] - '1';
	      key_buf[strlen ("angle ")] = 'X';

	      fval *= DEGTORAD;
	      if (COMMAND ("angle X by"))
	       { fval += cb->angTarget[jnt];
	       }
	      if (fval < jls->jmin[jnt] || fval > jls->jmax[jnt])
	       { printf ("ERROR -- angle value is out of range\n");
		 continue;
	       }
	      cb->angTarget[jnt] = fval;
	      cb->movecode = (1<<jnt);
	      rciBlock(td);
	    }
	   else if (COMMAND ("move to midpoint"))
	    { for (j=0; j<numJoints; j++)
	       { cb->angTarget[j] = (jls->jmin[j] + jls->jmax[j])/2.0;
	       }
	      cb->movecode = ALL_JOINTS;
	      rciBlock(td);
	    }
	   else if (COMMAND ("stop"))
	    { cb->stop = YES;
	      rciBlock(td);
	    }
	   else if (COMMAND ("set travel speed"))
	    { if (FABS(fval) > 360)
	       { printf ("Aw, get real!\n");
		 continue;
	       }
	      travelSpeed = FABS(fval)*DEGTORAD;
	      speedSet (cb, jls, travelSpeed);
	    }
	   else if (COMMAND ("set sample speed"))
	    { if (FABS(fval) > 90)
	       { printf ("Aw, get real!\n");
		 continue;
	       }
	      sampleSpeed = FABS(fval)*DEGTORAD;
	    }
	   else if (COMMAND ("set sweepAngle"))
	    { sweepAngle = DEGTORAD*FABS(sweepAngle);
	      sweepAngleSet = YES;
	    }
	   else if (COMMAND ("unset sweepAngle"))
	    { sweepAngleSet = NO;
	    }
	   else if (COMMAND ("show"))
	    { int j;

	      putchar ('\n');
	      if (testMode)
	       { printf ("TESTMODE: Pot values are simulated.\n");
	       }
	      printf (
"travel speed=%g (deg./sec.)  sample speed=%g\n",
			RADTODEG*travelSpeed, RADTODEG*sampleSpeed);
	      if (sweepAngleSet)
	       { printf ("sweep angle set to %g degrees\n", 
			 RADTODEG*sweepAngle);
	       }
	      putchar ('\n');

printf ("JNT     Angle  Encoders      Angle Range       Pot value\n");
/*       XX   -XXX.XXX   XXXXXXX  -XXX.XXX to -XXX.XXX    XXXXXXX	*/
	      for (j=0; j<6; j++)
	       { printf ("%2d   %8.3f   %7d  %8.3f to %8.3f    %7d\n", j+1,
		 how->ang[j]*RADTODEG,
		 how->enc[j],
		 jls->jmin[j]*RADTODEG,
		 jls->jmax[j]*RADTODEG,
		 testMode ? fakePotValue(j) : how->potInput[j]);
	       }
	      printf ("\n");
	    }
#if OLD
	   else if (COMMAND ("calibrate wrist"))
	    { STOP();

	      for (j=0; j<numJoints; j++)
	       { startAngles[j] = cb->angTarget[j];
		 deltaAngles[j] = 0.0;
	       }
	      for (k=0; k<ARRAY_SIZE(wristJoints); k++)
	       { j = wristJoints[k]-1;
		 startAngles[j] = jls->jmin[j]+MARGIN;
		 deltaAngles[j] = (jls->jmax[j]-MARGIN-startAngles[j]) /
				  (numStops-1);
		 calibrateRequest |= (1<<j);
	       }
	    }
	   else if (COMMANDS ("calibrate"))
	    { jnt = key_buf[strlen("calibrate ")] - '1';
	      sweepAngle = FABS(sweepAngle)*DEGTORAD;

	      STOP();

	      for (j=0; j<numJoints; j++)
	       { startAngles[j] = cb->angTarget[j];
		 deltaAngles[j] = 0.0;
	       }
	      startAngles[jnt] = (jls->jmin[jnt]+jls->jmax[jnt])/2.0 - sweepAngle;
	      deltaAngles[jnt] = 2*sweepAngle/(numStops-1);
	      if (startAngles[jnt] < jls->jmin[jnt]+MARGIN ||
		  startAngles[jnt] + 2*sweepAngle > jls->jmax[jnt]-MARGIN)
	       { printf ("ERROR -- sweep would take joint out of range\n");
		 continue;
	       }
	      calibrateRequest = (1<<jnt);
	    }
#endif
	   else if (COMMAND ("calibrate wrist"))
	    { STOP();

	      for (j=0; j<numJoints; j++)
	       { startAngles[j] = cb->angTarget[j];
		 stopAngles[j] =  cb->angTarget[j];
	       }
	      for (k=0; k<ARRAY_SIZE(wristJoints); k++)
	       { j = wristJoints[k]-1;
		 startAngles[j] = jls->jmin[j]+MARGIN;
		 stopAngles[j] =  jls->jmax[j]-MARGIN;
		 calibrateRequest |= (1<<j);
	       }
	    }
	   else if (COMMANDS ("calibrate"))
	    { jnt = key_buf[strlen("calibrate ")] - '1';

	      STOP();

	      for (j=0; j<numJoints; j++)
	       { startAngles[j] = cb->angTarget[j];
		 stopAngles[j] =  cb->angTarget[j];
	       }
	      if (sweepAngleSet)
	       { startAngles[jnt] = (jls->jmin[jnt]+jls->jmax[jnt])/2.0 
			 	  - sweepAngle;
		 stopAngles[jnt]  = (jls->jmin[jnt]+jls->jmax[jnt])/2.0 
			 	  + sweepAngle;
		 if (startAngles[jnt] < jls->jmin[jnt]+MARGIN ||
		     stopAngles[jnt]  > jls->jmax[jnt]-MARGIN)
		  { printf ("ERROR -- sweep would take joint out of range\n");
		    continue;
		  }
		 sweepAngleSet = NO;
	       }
	      else
	       { startAngles[jnt] = jls->jmin[jnt]+MARGIN;
		 stopAngles[jnt]  = jls->jmax[jnt]-MARGIN;
	       }
	      calibrateRequest = (1<<jnt);
	    }
	   else if (COMMAND ("help"))
	    { printf ("\n");
printf (" A description of the program and its commands can be found in \n");
printf (" the RCI reference manual page \"potcal(1)\".\n");
	      printf ("\n");
	    }

#if OLD
	   if (calibrateRequest)
	    { float slope, intercept;
	      float oldSpeed;
	      unsigned long mask;
	      int aborted;
	
	      printf ("Hit <CR> to abort.\n");
	      putchar ('\n');
	      printf ("Going to the start position ... \n");
	      putchar ('\n');

	      for (j=0; j<numJoints; j++)
	       { cb->angTarget[j] = startAngles[j];
	       }
	      cb->movecode = ALL_JOINTS;
	      rciBlock(td);

	      if (waitOrAbort() == 0)
	       { STOP();
		 continue;
	       }

	      printf ("Taking samples ...\n");
	      putchar ('\n');

	      displayHeader (calibrateRequest);

	      aborted = 0;
	      for (k=0; k<numStops; k++)
	       { 
		 displayAndReadPots (calibrateRequest, k);
		
		 if (k==numStops-1)
		  { break;
		  }
		 for (j=0; j<numJoints; j++)
		  { cb->angTarget[j] += deltaAngles[j];
		  }
		 cb->movecode = ALL_JOINTS;
		 rciBlock(td);
		 if (waitOrAbort() == 0)
		  { STOP();
		    aborted = 1;
		    break;		    
		  }
	       }
	      if (aborted)
	       { continue;
	       }
	      putchar ('\n');
	      j = 0;
	      for (mask = calibrateRequest; mask; mask >>= 1)
	       { if (mask & 1)
		  { linearFit (&slope,
			       &intercept,
			       &potReadings[j][0],
			       &potEncoders[j][0],
			       numStops);
		    printf ("Joint %2d: encoder = %g potreading + %g\n",
			     j+1, slope, intercept);
		  }
		 j++;
	       }
	    }
#endif
	   if (calibrateRequest)
	    { float oldSpeed;
	      unsigned long mask;
	      int aborted;
	      int forwardReq;
	      int reverseReq;

	      computeDirections (&forwardReq, &reverseReq, calibrateRequest,
				 startAngles, stopAngles, jls);
	
	      printf ("Hit <CR> to abort.\n\n");

	      if (forwardReq)
	       { printf ("Sampling for %s ...\n\n",
			  jointDescStr (jstrbuf, forwardReq));
		 if (!collectSamples (forwardReq, startAngles, stopAngles))
		  { continue;
		  }
	       }
	      if (reverseReq)
	       { printf ("Sampling for %s ...\n\n",
			  jointDescStr (jstrbuf, reverseReq));
		 if (!collectSamples (reverseReq, stopAngles, startAngles))
		  { continue;
		  }
	       }
	    }
	 }
	while (!(COMMAND("quit")));

/* MS Turn off pot reading before we leave */
        SET_POT_READ (rbt, 0);
	rciBlock(td);
	rciClose();
	exit (0);
}

collectSamples (mask, beginAng, endAng)
int mask;
float *beginAng, *endAng;
{
	float oldSpeed;
	int j;

	for (j=0; j<numJoints; j++)
	 { cb->angTarget[j] = beginAng[j];
	 }
	cb->movecode = ALL_JOINTS;
	rciBlock(td);

	if (waitOrAbort() == 0)
	 { STOP();
	   return(0);
	 }

	for (j=0; j<numJoints; j++)
	 { cb->angTarget[j] = endAng[j];
	 }
	computeSampleInterval (pc, beginAng, endAng, jls);

	oldSpeed = speedSet (cb, jls, sampleSpeed);
	pc->jntSelect = mask;
	pc->testMode = testMode;
	pc->initialize = 1;
	cb->monitorArg = (int)pc;
	cb->monitorFxn = potvalMonitor;
	cb->movecode = ALL_JOINTS;

	rciBlock(td);
	if (waitOrAbort() == 0)
	 { STOP();
	   speedSet (cb, jls, oldSpeed);
	   return(0);
	 }
	cb->monitorFxn = NULLF;
	speedSet (cb, jls, oldSpeed);

	if (pc->complete != mask)
	 { printf ("Error: %s did not complete sampling\n",
		    jointDescStr(jstrbuf, mask&~pc->complete));
	 }
	for (j=0; j<jls->numJ; j++)
	 { if (mask & (1<<j))
	    { double m, b;
	      
	      if (pc->data[j].n < 2)
	       { printf ("Joint %2d: %d points inadequate for estimation\n", 
			  pc->data[j].n);
	       }
	      else
	       { computeLine (&m, &b, &pc->data[j]);
		 printf ("Joint %2d: slope=%g intercept=%g\n",
			  j+1, 1/m, -b/m);
		 fprintf (fp, "%2d  %10.4f %10.1f\n", 
			  j+1, 1/m, -b/m);
		 fflush (fp);
	       }
	    }
	 }
	return (1);
}

waitOrAbort ()
{
	cb->compcode = 0;
	while (cb->compcode != ALL_JOINTS)
	 { if (testchar(0.0))
	    { while (getchar() != '\n')
		 ;
	      printf ("Aborted.\n");
	      return (0);
	    }
	   nap (0.1);
	 }
	rciBlock(td);
	rciBlock(td);
	return (1);
}

#if OLD

#define NUM_SAMPLES	10

displayAndReadPots (jntMask, stopNum)
unsigned long jntMask;
int stopNum;
{
	float potValues[MAXJNTS];
	unsigned long mask;
	int i, j;

	for (j=0; j<numJoints; j++)
	 { potValues[j] = 0.0;
	 }
	for (i=0; i<NUM_SAMPLES; i++)
	 { for (j=0; j<numJoints; j++)
	    { potValues[j] += testMode ? fakePotValue(j) : how->potInput[j];
	    }
	   rciBlock (td);
	 }
	for (j=0; j<numJoints; j++)
	 { potValues[j] /= (float) NUM_SAMPLES;
	 }

	printf (" %3d   ", stopNum);
	j = 0;
	for (mask = jntMask; mask; mask >>= 1)
	 { if (mask & 1)
	    { printf ("%7d  %7.1f   ", how->enc[j], potValues[j]);
	    }
	   j++;
	 }
	putchar ('\n');
	for (j=0; j<numJoints; j++)
	 { potReadings [j][stopNum] = potValues[j];
	   potEncoders [j][stopNum] = how->enc[j];
	 }
}

displayHeader (jntMask)
unsigned long jntMask;
{
	unsigned long mask;
	int j;

	printf ("stop   ");
/*		 Stop   encoder/potval j   encoder/potval j
 		  XXX   XXXXXXX  XXXXX.X   XXXXXXX  XXXXX.X ...		*/
	j = 0;
	for (mask = jntMask; mask; mask >>= 1)
	 { if (mask & 1)
	    { printf ("encoder/potval %-2d  ", j+1);
	    }
	   j++;
	 }
	putchar ('\n');
}
	
linearFit (slope, intercept, Xvector, Yvector, size)
float *slope, *intercept;
float Xvector[], Yvector[];
int size;
{
	int i;

	float sumX, sumY;
	float sumXX, sumXY;
	float denom;

	sumX = 0.0;
	sumY = 0.0;
	sumXX = 0.0;
	sumXY = 0.0;

	for (i=0; i<size; i++)
	 { sumX += Xvector[i];
	   sumY += Yvector[i];
	   sumXX += Xvector[i] * Xvector[i];
	   sumXY += Xvector[i] * Yvector[i];
	 }

	denom = size * sumXX - sumX * sumX;

	if (denom == 0.0)
	 { return (-1);
	 }

	*slope = (size * sumXY - sumX * sumY) / denom;
	*intercept = (sumXX * sumY - sumX * sumXY) / denom;
}

#endif

computeSampleInterval (pc, startAngles, stopAngles, jls)
POTCAL *pc;
float *startAngles;
float *stopAngles;
JLS *jls;
{        
	/* computes the encoder values between which the pot data should
	   be sampled for the joints in question. */

#define ENC_MARGIN	100

	int startEnc[MAXJNTS];
	int stopEnc[MAXJNTS];
	register int j;

	(*jls->angleToEncoder)(startEnc, startAngles, jls);
	(*jls->angleToEncoder)(stopEnc, stopAngles, jls);

	for (j=0; j<jls->numJ; j++)
	 { if ((startEnc[j] <= stopEnc[j]) &&
	       (startEnc[j]+2*ENC_MARGIN <= stopEnc[j]))
	    { pc->sampleStart[j] = startEnc[j]+ENC_MARGIN;
	      pc->sampleStop[j] = stopEnc[j]-ENC_MARGIN;
	    }
	   else if ((startEnc[j] > stopEnc[j]) &&
		    (startEnc[j]-2*ENC_MARGIN > stopEnc[j]))
	    { pc->sampleStart[j] = startEnc[j]-ENC_MARGIN;
	      pc->sampleStop[j] = stopEnc[j]+ENC_MARGIN;
	    }
	   else
	    { pc->sampleStart[j] = (startEnc[j]+stopEnc[j])/2;
	      pc->sampleStop[j] = (startEnc[j]+stopEnc[j])/2;
	    }
	 }
}

computeDirections (forward, reverse, mask, startAng, stopAng, jls)
int *forward, *reverse;
int mask;
float *startAng, *stopAng;
JLS *jls;
{
	/* For each of the joints indicated in 'mask', determine
	   which increase their encoder values by travelling from
	   startAng to stopAng; indicate these in 'forward'. 
	   Joints whose encoder values increase by travelling in
	   the other direction are indicated in 'reverse'.
	 */

	int startEnc[MAXJNTS];
	int stopEnc[MAXJNTS];
	register int j;

	(*jls->angleToEncoder)(startEnc, startAng, jls);
	(*jls->angleToEncoder)(stopEnc, stopAng, jls);

	*forward = 0;
	for (j=0; j<jls->numJ; j++)
	 { if ((mask & (1<<j)) && (stopEnc[j] > startEnc[j]))
	    { *forward |= (1<<j);
	    }
	 }
	*reverse = (mask & ~(*forward));
}
