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

#include <stdio.h>

#ifdef VXWORKS_CFG
#define main pumacal_
#endif

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

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

typedef enum {
	UNSPECIFIED,
	NEST,
	PARK,
	POTS
} CALIB_METHOD;

extern char *getDefaultRobot();
extern potvalMonitor();
extern char *jointDescStr();

#define J2NEST_OFFSET 	010000		/* For PUMA 260 */
#define NUMPOTSAMPLES 	16

int numJoints;
RCI_DESC *td;
RCI_RBT *rbt;
CHASE_CTRL *cb;
JLS *jls;
HOW *how;
POTCAL *pc;

char jstrbuf[64];

int argcnt;
char **argvec;

main (argc, argv)
int argc;
char **argv;
{
	int firstPotReadings [MAXJNTS];
	int secondPotReadings [MAXJNTS];
	int encFirst [MAXJNTS];
	int encEstimate [MAXJNTS];
	int encActual [MAXJNTS];

	int verbose = NO;
	int moveToPark = UNDEF;	
	CALIB_METHOD method = UNSPECIFIED;
	char *robotName = NULL;
	int testflag = 0;
	int simflag = 0;
	int turnPowerOff = YES;
	
	int j;

	/* ---- start by parsing the argument list ---- */

	argcnt = argc;
	argvec = argv;

	argc--;	argv++;
	while (argc)
	 { 
	   if (ARGUMENT("-v"))
	    { verbose = YES;
	    }
	   else if (ARGUMENTS ("-sim="))
	    { char *sname;
	      if (strlen(sname=index(*argv,'=')+1) == 0)
	       { argc = -1;
		 break;
	       }
	      setSimulator(sname);
	      simflag = 1;
	    }
	   else if (ARGUMENT("-sim"))
	    { simflag = 1;
	    }
	   else if (ARGUMENT("-park"))
	    { if (moveToPark != UNDEF)
	       { fatalMsg (-1, "more than one park option specified\n");
	       }
	      moveToPark = YES;
	    }
	   else if (ARGUMENT("-nopark"))
	    { if (moveToPark != UNDEF)
	       { fatalMsg (-1, "more than one park option specified\n");
	       }
	      moveToPark = NO;
	    }
	   else if (ARGUMENT("-atpark"))
	    { if (method != UNSPECIFIED)
	       { fatalMsg (-1, "more than one calibration mode specified\n");
	       }
	      method = PARK;
	    }
	   else if (ARGUMENT("-pots"))
	    { if (method != UNSPECIFIED)
	       { fatalMsg (-1, "more than one calibration mode specified\n");
	       }
	      else
	       { method = POTS;
	       }
	    }
	   else if (ARGUMENT("-nest"))
	    { if (method != UNSPECIFIED)
	       { fatalMsg (-1, "more than one calibration mode specified\n");
	       }
	      else
	       { method = NEST;
	       }
	    }
	   else if (ARGUMENT("-test"))
	    { testflag = 1;
	    }
	   else if (ARGUMENT("-on"))
	    { turnPowerOff = NO;
	    }
	   else if (*argv[0] != '-')
	    { if (robotName == NULL)
	       { robotName = *argv;
	       }
	      else
	       { fatalMsg (-1, "more than one robot name specified\n");
	       }
	    }
	   else
	    { usageMsg (-1, 
"%s [<robot>] [-v] [-pots] [-nest] [-park]\n\
[-sim[=<simulatorAddr>]] [-test] [-on]\n", argvec[0]);
	    }
	   argc--; argv++;
	 }

	if (robotName == NULL)
	 { robotName = getDefaultRobot();
	 }
	if (!find_robot(robotName))
	 { fatalMsg (-1, "robot '%s' not known to the system\n", robotName);
	 }
	if (robot_class(robotName) != PUMA)
	 { fatalMsg (-1, "robot '%s' is not a PUMA\n", robotName);
	 }

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

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

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

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

	/* ---- now instantiate any unspecified options ---- */

	if (method == UNSPECIFIED)
	 { method = jls->flags & PUMA_260 ? NEST : POTS;
	 }
	if (moveToPark == UNDEF)
	 { moveToPark = (method == NEST) ? YES : NO;
	 }

	cb->controlLevelChange = MOTOR_LEVEL;

	/* set up very small encoder level speeds */

	for (j=0; j<numJoints; j++)
	 { cb->encSpeed[j] = 500;
	   cb->encAccel[j] = 5000;
	 }

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

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

	if (!(how->options & POTS_AVAIL) && (method == POTS))
	 { printf ("ERROR -- this robot does not have potentiometers\n");
	   rciClose();
	   exit (-1);
	 }

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

	/* ----- Check to see if we are already calibrated ----- */

	if ((how->calibrated) && (method == NEST || method == PARK))
	 { char reply[2];
	   printf ("The robot reports that it is already calibrated.\n");
	   printf ("Do not recalibrate unless the robot is in the %s\n",
		    (method == NEST) ? "nest" : "park position" );
	   printf ("Proceed (Y/N)? ");
	   scanf ("%1s", reply);
	   if (reply[0] != 'y' && reply[0] != 'Y')
	    { RCI_EXIT(0);
	    }
	 }

	/* ----- Record the initial positions, for posterity ----- */

	if (method == POTS)
 	 { read_pots (firstPotReadings, NUMPOTSAMPLES);
	 }
	else
  	 { for (j=0; j<numJoints; j++)
	    { encFirst[j] = how->enc[j];
	    }
	 }
	
	/* ----- Move out of the nest, if necessary ----- */

	if (method == NEST)
 	 { cb->encTarget[1] += J2NEST_OFFSET;
	   MOVE (0x2);
	 }
	
	/*  Back up joint 1 from it's limit stop, if necessary. The 260  */
	/*  park and nest positions are very close to the limit stop for */
	/*  joint 1.							 */

	if (jls->flags & PUMA_260)
	 { cb->encTarget[0] -= 1.5*jls->encWheelSize[0];
	   MOVE (0x1);
	 }

	/* --- Now go into calibration mode and move to the zero index --- */

	if (method == POTS)
	 { int deltaEnc[MAXJNTS];
	   int estTargetEnc[MAXJNTS];
	   float estTargetAng[MAXJNTS];

	   for (j=0; j<numJoints; j++)
	    { deltaEnc[j] = 1.5*jls->encWheelSize[j] + 
			    4*jls->potSlope[j];
	      estTargetEnc[j] = how->enc[j] + deltaEnc[j] +
	      			jls->encWheelSize[j];
	    }
	   (*jls->encoderToAngle)(estTargetAng, estTargetEnc, jls);

	   for (j=0; j<numJoints; j++)
	    { 
#if 0
	      if (estTargetAng[j] < jls->jmin[j] || 
		  estTargetAng[j] > jls->jmax[j])
	       { fatalMsg (-1, "joint %d would be driven out of range\n", j+1);
	       }
#endif
	    }
	   
	   for (j=0; j<numJoints; j++)
	    { if (estTargetEnc[j] < 32768 /* half way point */)
	       { cb->encTarget[j] += deltaEnc[j];
		 /* the 100 allows for acceleration time */
		 pc->sampleStart[j] = how->enc[j] + 100;
		 pc->sampleStop[j] = how->enc[j] + 4*jls->potSlope[j];
	       }
	      else
	       { cb->encTarget[j] -= deltaEnc[j];
		 pc->sampleStart[j] = how->enc[j] - 100;
		 pc->sampleStop[j] = how->enc[j] - 4*jls->potSlope[j];
	       }
	    }
	   pc->jntSelect = ALL_JOINTS;
	   pc->initialize = 1;
	   pc->setCalibMode = 1;
	   cb->monitorArg = (int)pc;
	   cb->monitorFxn = potvalMonitor;
	   MOVE (ALL_JOINTS);
 
	   if (how->markFound != ALL_JOINTS)
	    { rciClose();
	      fatalMsg (-1, "%s did not find zero index\n",
		     jointDescStr(jstrbuf, (~how->markFound)&ALL_JOINTS) );
	    }
	   nap (0.5);		/* let things settle down ... */

	   /* estimate the correct encoder count for the position */

	   if (verbose)
	    { printf ("ESTIMATION:\n");
	      printf (
" Jnt   start    stop  1stEnc  lstEnc           m          b  howEnc\n");
/*XX   XXXXXX  XXXXXX  XXXXXX  XXXXXX  X.XXXXXXXX  XXX.XXXXX  XXXXXX */
	    }
	   for (j=0; j<numJoints; j++)
	    { double m, b, potval;
	      int encMidpoint;

	      if (pc->data[j].n <= 2)
	       { fatalMsg (-1, "Only %d sample points taken for joint %d\n",j+1);
	       }
	      computeLine (&m, &b, &pc->data[j]);
	      encMidpoint = (pc->data[j].firstEnc+pc->data[j].lastEnc) / 2;
	      potval = m * encMidpoint + b;
	      encEstimate[j] = potval*jls->potSlope[j] + jls->potIntercept[j]
			     + (how->enc[j] - encMidpoint);
	      if (verbose)
	       { printf (" %2d   %6d  %6d  %6d  %6d  %10.8f  %9.5f  %6d\n",
			  j+1,
			  pc->sampleStart[j], pc->sampleStop[j],
			  pc->data[j].firstEnc, pc->data[j].lastEnc,
			  m, b, how->enc[j]);
	       }
	    }
	 }
	else /* method == NEST or method == PARK */
	 {
	   SET_CALIBMODE (rbt, ~0);
	
	   for (j=0; j<numJoints; j++)
	    { /* the factor 1.5 is just to be sure! */
	      cb->encTarget[j] += 1.5*jls->encWheelSize[j];
	    }
	   MOVE (ALL_JOINTS);
 
	   if (how->markFound != ALL_JOINTS)
	    { rciClose();
	      fatalMsg (-1, "%s did not find zero index\n",
		     jointDescStr(jstrbuf,(~how->markFound)&ALL_JOINTS));
	    }
	   nap (0.5);		/* let things settle down ... */

	   /* estimate the correct encoder values for current position */

	   if (method == PARK)
	    { int encPark[MAXJNTS];
	      (*jls->angleToEncoder)(encPark, jls->parkAngles, jls);
	      for (j=0; j<numJoints; j++)
	       { encEstimate[j] = encPark[j] + 
				  (int)(how->enc[j] - encFirst[j]);
	       }
	    }
	   else if (method == NEST)
	    { for (j=0; j<numJoints; j++)
	       { encEstimate[j] = jls->encHome[j] +
				  (int)(how->enc[j] - encFirst[j]);
	       }
	    }
	 }

	/* ----- set up control level for changing encoder values ---- */

	cb->block = YES;

	rciBlock(td);

	/* -- Compute the actual encoder value from where we are and the -- */
	/*    estimated encoder values, and then set robot encoders to      */
	/* -- these values. Print out diagnostic info if desired	 -- */

	if (verbose)
	 { printf ("\n Jnt  Original  Estimate  Delta   N  Computed\n");
/*		     XX    XXXXXX    XXXXXX   XXXXX  XXX  XXXXXX */
	 }
	for (j=0; j<numJoints; j++)
	 { double a;
	   int n;

	   a = (encEstimate[j]-jls->encCalvec[j]) / 
	       (double)jls->encWheelSize[j];
	   n = (a < 0.0) ? a - 0.5 : a + 0.5;	/* round to nearest integer */
	   encActual[j] = n * jls->encWheelSize[j] + jls->encCalvec[j];
	   if (verbose)
	    { printf (" %2d    %6d    %6d   %5d  %3d  %6d\n", 
		      j+1, how->enc[j], encEstimate[j], 
		      encEstimate[j]-how->enc[j], n, encActual[j]);
	    }
	   if (!testflag)
	    { SET_ENCODER (rbt, encActual[j], j);
	    }
	 }
	if (verbose)
	 { printf ("\n");
	 }
	nap (0.5);		/* Wait for encoders to settle */

	if (testflag)
	 { rciClose();
	   exit(0);
	 }

	SET_CALIBFLAG (rbt, 1);
	SET_CHECKING (rbt, (CHK_RBTALL));
	cb->halt = YES;
	rciBlock(td);

	cb->block = NO;

#define ETOLERENCE	5

	for (j=0; j<6; j++)
	 { if (ABS(how->enc[j]-encActual[j]) > ETOLERENCE)
	    { printf ("WARNING: joint %d encoder reads %6d vs. %6d\n",
		      j+1, how->enc[j], encActual[j]);
	    }
	 }

	if (moveToPark)
 	 { int encPark[MAXJNTS];
	   (*jls->angleToEncoder) (encPark, jls->parkAngles, jls);

	   scaleVf(cb->encSpeed, jls->averageAngtoenc,
				 jls->nominalVel, numJoints);
	   scaleVf(cb->encAccel, jls->averageAngtoenc,
				 jls->nominalAcc, numJoints);

	   for (j=0; j<numJoints; j++)
 	    { cb->encTarget[j] = encPark[j];
	    }
	   MOVE (ALL_JOINTS);
	 }
	rciClose();
	exit (0);
}

printVi (fp, fmt, v, size)
FILE *fp;
char *fmt;
int *v;
int size;
{
	int i;
	fprintf (fp, fmt);
	for (i=0; i<size; i++)
	 { fprintf (fp, "%9d", v[i]);
	 }
	fputc ('\n', fp);
}

read_pots (values, n)
int values[];
int n;
{
	double temp[6];
	int i, j, ex;

	for (j=0; j<6; j++)
	 { temp[j] = 0.0;
	 }
	for (i=0; i<n; i++)
	 { for (j=0; j<6; j++)
	    { temp[j] += how->potInput[j];
	    }
	   rciBlock(td);
	 }	
	for (j=0; j<6; j++)
 	 { values[j] = (temp[j]/(double)n) + 0.5;
	 }
}

pot_encoder_estimate (enc, potReadings)
int *enc;
int *potReadings;
{
	register int j;

	for (j=0; j<numJoints; j++)
	 { enc[j] = jls->potSlope[j]*potReadings[j] + jls->potIntercept[j];

	   if (enc[j] < 0 || enc[j] > 0x10000)
	    { fprintf (stderr, 
		      "WARNING: bogus estimated pot value (%d) on joint %d\n",
		      enc[j], j+1);
	    }
	 }
}
