/*-=-=-=-=-=-=-=-=-=-=-=- 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>
#include <signal.h>

#ifdef VXWORKS_CFG
#define main primecal_
#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 "macros.h"

#define ENCODER_MARGIN	  1024
#define MAX_ENCODER     0xffff

int numJoints;

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

static char *outputFile = "primecal.out";

char *getDefaultRobot();
float averageEncoderRatio();

motorLevelControl()
{
	return (cb->controlLevel == MOTOR_LEVEL ? 1 : 0);
}

jointLevelControl()
{
	return (cb->controlLevel == JOINT_LEVEL ? 1 : 0);
}

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

main(argc, argv)
int argc;
char **argv;
{       
	char *robotName = NULL;
	int simflag = NO;
	int turnPowerOff = YES;

	char *keytree, *tree_match_parse(), *tree_match_err_at();
	int ival, jnt, j, k;
	float fval;

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

	argc--; argv++;
	while (argc)
	 { 
#if 0
	   if (ARGUMENT ("-v"))
	    { verbose = YES;
	    }
#endif	 
	   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 (robotName == NULL)
	    { robotName = *argv;
	    }
	   else 
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { usageMsg (-1, 
"primecal [<robotName>] [-sim[=<simulatorAddr>]] [-on]\n");
	 }
	if (!robotName)
	 { robotName = getDefaultRobot();
	 }

	keytree = tree_match_parse("					\
(	primecal							\
	show								\
	encoder			? ", motorLevelControl, "		\
	 ([1 2 3 4 5 6]							\
	   ([to by]							\
	     (%d", &ival, ",,,	\"encoder count (decimal)\")))		\
	angle			? ", jointLevelControl, "		\
	 ([1 2 3 4 5 6]							\
	   ([to by]							\
	     (%f", &fval, ",,,	\"angle (degrees)\")))			\
	move								\
	 (to								\
	   (encoders		? ", motorLevelControl, "		\
	    angles		? ", jointLevelControl, "))		\
	stop								\
	set								\
	 (speed								\
	   (%f", &fval, ",,,	\"degrees/sec.\")			\
	  [angle encoder]						\
	   (mode)							\
	 )								\
	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);
	 }

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

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

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

	cb = (CHASE_CTRL*)chaseTaskMemory(td);

	cb->controlLevelChange = JOINT_LEVEL;

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

/* MS
SET_CHECKING (rbt, CHK_MAXVEL | CHK_MAXTOR | CHK_REQTOR | CHK_REQVEL);
*/
SET_CHECKING (rbt, 0);
	if (startChaseTask(td) < 0)
	 { RCI_EXIT (-1);
	 }

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

	/* ---- Allow the user to move the arm to the desired position ---- */

	tree_match_catch_sig (0, sigmask(SIGALRM));

	do
 	 { tree_match (keytree, "PRIMECAL> ", key_buf);

	   if (COMMAND ("set angle mode"))
	    { if (cb->controlLevel != JOINT_LEVEL)
	       { STOP();
		 cb->controlLevelChange = JOINT_LEVEL;
	       }
	    }
	   else 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])
	       { if (!userVerify())
		  { continue;
		  }
	       }
	      cb->angTarget[jnt] = fval;
	      cb->movecode = (1<<jnt);
	      rciBlock(td);
	    }
	   else if (COMMAND ("set encoder mode"))
	    { if (cb->controlLevel != MOTOR_LEVEL)
	       { STOP();
		 cb->controlLevelChange = MOTOR_LEVEL;
	       }
	    }
	   else if (COMMANDS ("encoder"))
	    { int newEncTarget[MAXJNTS];
       
	      jnt = key_buf[strlen ("encoder ")] - '1';
	      key_buf[strlen ("encoder ")] = 'X';

	      for (k=0; k<numJoints; k++)
	       { newEncTarget[k] = cb->encTarget[k];
	       }
	      if (COMMAND ("encoder X by"))
	       { ival += cb->encTarget[jnt];
	       }
	      if (ival < ENCODER_MARGIN ||
		  ival > MAX_ENCODER - ENCODER_MARGIN)
	       { printf ("encoder counts must be between %d and %d\n",
			 ENCODER_MARGIN, MAX_ENCODER - ENCODER_MARGIN);
		 continue;
	       }
	      newEncTarget[jnt] = ival;

	      if (rangeCheck (newEncTarget, jls) == 0)
	       { if (!userVerify())
		  { continue;
		  }
	       }
	      cb->encTarget[jnt] = ival;
	      cb->movecode = (1<<jnt);
	      rciBlock(td);
	    }
	   else if (COMMAND ("move to encoders"))
	    { int newEncTarget[MAXJNTS];
	      int error = 0;

	      printf ("enter %d encoder values:\n", numJoints);
	      for (j=0; j<numJoints; j++)
	       { if (scanf ("%d", &newEncTarget[j]) != 1)
		  { printf ("ERROR readings values\n");
		    error = 1;
		    break;
		  }
		 if (newEncTarget[j] < ENCODER_MARGIN ||
		     newEncTarget[j] > MAX_ENCODER - ENCODER_MARGIN)
		  { printf ("encoder counts must be between %d and %d\n",
			    ENCODER_MARGIN, MAX_ENCODER - ENCODER_MARGIN);
		    error = 1;
		  }
	       }
	      if (error)
	       { continue;
	       }
	      if (rangeCheck (newEncTarget, jls) == 0)
	       { if (!userVerify())
		  { continue;
		  }
	       }
	      for (j=0; j<numJoints; j++)
	       { cb->encTarget[j] = newEncTarget[j];
	       }
	      cb->movecode = ALL_JOINTS;
	    }
	   else if (COMMAND ("move to angles"))
	    { float angles[MAXJNTS];
	      int error = 0;
	      int outOfRange = 0;

	      printf ("enter %d angle values (degrees):\n", numJoints);
	      for (j=0; j<numJoints; j++)
	       { if (scanf ("%f", &angles[j]) != 1)
		  { printf ("ERROR readings values\n");
		    error = 1;
		    break;
		  }
		 angles[j] *= DEGTORAD;
		 if (angles[j] < jls->jmin[j] || angles[j] > jls->jmax[j])
		  { outOfRange |= (1<<j);
		  }
	       }
	      if (error)
	       { continue;
	       }
	      if (outOfRange)
	       { if (!userVerify())
		  { continue;
		  }
	       }
	      for (j=0; j<numJoints; j++)
	       { cb->angTarget[j] = angles[j];
	       }
	      cb->movecode = ALL_JOINTS;
	    }
	   else if (COMMAND ("stop"))
	    { cb->stop = YES;
	      rciBlock(td);
	    }
	   else if (COMMAND ("set speed"))
	    { if (FABS(fval) > 360)
	       { printf ("Aw, get real!\n");
		 continue;
	       }
	      for (j=0; j<numJoints; j++)
	       { cb->angSpeed[j] = fval*DEGTORAD;
		 cb->encSpeed[j] = fval*DEGTORAD * jls->averageAngtoenc;
	       }
	    }
	   else if (COMMAND ("show"))
	    { int j;
	      printf ("\n");
	      printf ("%s mode, speed = %g deg/sec.\n", 
		      cb->controlLevel == JOINT_LEVEL ? "ANGLE" : "ENCODER",
		      RADTODEG*cb->angSpeed[0]);
	      printf ("\n");
	      printf (" JNT    ENCODER     ANGLE%s\n",
		       how->calibrated ? "" : "*");
	      /*         X      XXXXXX  -XXX.XXX */
	      for (j=0; j<6; j++)
	       { printf (" %2d      %6d  %8.3f\n", j+1,
			 how->enc[j], how->ang[j]*RADTODEG);
	       }
	      printf ("\n");
	      if (!how->calibrated)
	       { printf ("* -- Robot reports it is not calibrated\n");
	       }
	    }
	   else if (COMMAND ("quit"))
	    { rciClose();
	      exit (0);
	    }
	 }
	while (!(COMMAND("primecal")));

	STOP();

	 { int j;
	   float angles[MAXJNTS];
	   int realEncoders1[MAXJNTS];
	   int realEncoders2[MAXJNTS];
	   int firstEncoders[MAXJNTS];
	   int calvec[MAXJNTS];
	   FILE *tree_match_input_stream();

	   printf ("Enter the present robot angles (degrees):\n");

	   for (j=0; j<numJoints; j++)
	    { if (fscanf (tree_match_input_stream(), "%f", &angles[j]) != 1)
	       { printf ("ERROR reading values\n");
		 rciClose();
		 exit (-1);
	       }
	      angles[j] *= DEGTORAD;
	    }
	   (*jls->angleToEncoder)(realEncoders1, angles, jls);

	   for (j=0; j<numJoints; j++)
	    { firstEncoders[j] = how->enc[j];
	    }

	   SET_CHECKING (rbt, CHK_MAXTOR | CHK_REQTOR);
	   SET_CALIBMODE (rbt, ~0);

	   cb->controlLevelChange = MOTOR_LEVEL;
	   rciBlock (td);

	   for (j=0; j<numJoints; j++)
	    { cb->encSpeed[j] = 500;
	      cb->encAccel[j] = 5000;
	      cb->encTarget[j] += jls->encWheelSize[j]*1.5;
				  /* the 1.5 is just to be sure */
	    }
	   MOVE (ALL_JOINTS);

	   if (how->markFound != ALL_JOINTS)
	    { printf ("ERROR -- joints 0x%x did not find their index mark\n",
	      ~how->markFound & ALL_JOINTS);
	      rciClose();
	      exit (-1);
	    }

	   nap(0.5);		/* wait position to stabilize exactly */

	   cb->block = YES;

	   for (j=0; j<numJoints; j++)
	    { realEncoders2[j] = realEncoders1[j]
			       + how->enc[j] - firstEncoders[j];

	      SET_ENCODER (rbt, realEncoders2[j], j);
	    }
	   rciBlock (td);
	   rciBlock (td);
	   rciBlock (td);

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

	   cb->block = NO;

	   SET_CHECKING (rbt, CHK_MAXVEL | CHK_REQVEL |
			      CHK_MAXTOR | CHK_REQTOR);
	  
	   for (j=0; j<numJoints; j++)
 	    { int delta;

	      delta = how->enc[j] - realEncoders2[j];
	      if (ABS (delta) > 2)
	       { fprintf (stderr, "WARNING -- joint %d set to %d vs. %d\n", 
			  j+1, how->enc[j], realEncoders2[j]);
	       }
	    }

#define REFERENCE	0100000

	   for (j=0; j<numJoints; j++)
	    { int delta = REFERENCE - realEncoders2[j];
	      int wheelSize = jls->encWheelSize[j];
	
	      if (delta < 0)
	       { calvec[j] = REFERENCE + ((-delta) % wheelSize);
	       }
	      else
	       { calvec[j] = REFERENCE + wheelSize - (delta % wheelSize);
	       }
	    }	   

	   printf (
"encCalvec is the first encoder index point past reference %d\n", REFERENCE);
	   printf (" JNT     angle  encoders  encCalvec\n");
/*		      X   -XXX.XXX    XXXXXX     XXXXXX */
	   for (j=0; j<numJoints; j++)
	    { printf (" %2d   %8.3f    %6d     %6d\n", j+1,
	      		angles[j]*RADTODEG, realEncoders1[j], calvec[j]);
	    }
	   for (j=0; j<numJoints; j++)
	    { fprintf (fp, "%d\n", calvec[j]);
	    }
	   fprintf (fp, "\n");
	   printf ("\nencCalvec written into file %s\n\n", outputFile);
	 }

	rciClose();
	exit (0);
}

float averageEncoderRatio (jls)
JLS *jls;
{
	float sum;
	int j;
	
	sum = 0.0;
	for (j=0; j<jls->numJ; j++)
	 { sum += jls->angtoenc[j];
	 }
	return (sum/jls->numJ);
}

rangeCheck (enc, jls)
int enc[];
JLS *jls;
{
	float ang[MAXJNTS];
	int j;

	(*jls->encoderToAngle)(ang, enc, jls);
	for (j=0; j<jls->numJ; j++)
	 { if (ang[j] < jls->jmin[j] || ang[j] > jls->jmax[j])
	    { return (0);
	    }
	 }
	return (1);
}

userVerify()
{
	char cmd[2];

	printf (
"WARNING: this will take the robot out of its nominal range.\n");
	printf ("proceed (Y/N) ? ");
	scanf ("%1s", cmd);
	if (cmd[0] != 'y' && cmd[0] != 'Y')
	 { return (0);
	 }
	else
	 { return (1);
	 }
}
