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

/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	teach
 *
 * DESCRIPTION:	The planning level front end to the RCCL teach software.
 *		The control level (teach trajectory generator) lives in
 *		teachCtrl.c. See that file for a discussion of why I use a
 *      	separate trajectiry generator for the teach software.
 *
 *		The routine rcclTeach() makes heavy use of the C-tree matcher
 *      	utility written by Mike Parker at McGill University. It is an
 *      	interactive command-completion input parser, that makes
 *      	it easier for a user to enter long command, and makes sure
 *      	that only reasonably valid input is entered. The utility is
 *      	documented in the file $RCCL/doc/CtreeMatch.doc.
 * 
 * CAVEATS:	This software works OK, but is quite old and needs to be
 *		overhauled at some point.
 *
 * AUTHOR/DATE:	John Lloyd, Winter 1985
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <signal.h>

#include <setjmp.h>

#include "../h/cdefs.h"
#include "../h/ctree.h"
#include "../h/robotMath.h"
#include "../h/robotTypes.h"
#include "../h/rciTask.h"
#include "../h/rcclDefs.h"
#include "../h/pendant.h"
#include "../h/errorCodes.h"
#include "../h/robot_kynvar.h"

#include "../rccl/teach.h"
#include "../rccl/kynMacros.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"
#include "../rccl/motionRequest.h"

#if defined(vax) || defined(sgi) || defined(VXWORKS_CFG) || defined(_AIX)
static (*oldSigusr1Handler)();
#else
static void (*oldSigusr1Handler)();
#endif
static char *keytree = NULL;

static jmp_buf env;
#if defined(linux)
static void sigusr1Handler()
#else
static sigusr1Handler()
#endif
{
	putchar ('\07');
	fflush (stdout);
	longjmp (env, 1);
}

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	initDone = 1;
	keytree = NULL;
	oldSigusr1Handler = NULL;
	requestFlagInit (&initDone);
}

#define T6	mnp->t6
#define J6	mnp->j6

#define WAITFOR(x)	while(!(x)) { delay(20.0); };

rcclTeach (mnp, prompt, tool)
MANIP *mnp;
const char *prompt;
TRSF *tool;
{
	char *fxnName = "rcclTeach()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	RCI_RBT *rbt = MANIP_ROBOT(mnp);
 	char key_buf[256];
	
	float value;
	float tx, ty, tz, phi, the, psi, roll, pitch, yaw;
 	int joint, status;
	TRSF temp, change, world_trsl, world_rot;
	TRSF t6, t6o;
	int conf;
	JNTS j6, j6o;
	JNTS j6new;
	float set_speed = 1.0;
	int savedIntegration;
	int integration;
	int handValue;

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();

	if (rbt == NULL)
	 { errorMsg (ENotSupported, rcclVirtMnpMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	if (keytree == NULL)
	 { keytree = tree_match_parse("					\
 (	record								\
	pendant								\
 	return								\
	stop								\
	show (position tool t6*tool)					\
 	[world tool]							\
 	 ([x y z rx ry rz]						\
	   (by								\
	     (%f",&value,",,,		\"distance (mm or degress)\")))	\
	joint								\
 	 ([1 2 3 4 5 6]							\
	   ([by to]							\
	      (%f",&value,",,,		\"angle value in degrees\")))	\
	goto								\
	 (joints							\
	 )								\
	set								\
	 (speed								\
	   (%f",&set_speed,",0.0,100.0,	\"speed (0.0 to 100.0)\")	\
	  integration (on off)						\
	  translation							\
	   (%f", &tx,",,,		\"x translation in mm\"		\
	   (%f", &ty,",,,		\"y translation in mm\"		\
	   (%f", &tz,",,,		\"z translation in mm\")))	\
	  rotation							\
	   (euler 							\
	     (%f", &phi,",,,		\"angle phi (degrees)\"		\
	     (%f", &the,",,,		\"angle the (degrees)\"		\
	     (%f", &psi,",,,		\"angle psi (degrees)\")))	\
	    rpy								\
	     (%f", &roll,",,,		\"angle roll (degrees)\"	\
	     (%f", &pitch,",,,		\"angle pitch (degrees)\"	\
	     (%f", &yaw,",,,		\"angle yaw (degrees)\")))	\
	    )								\
	  tool								\
	   (%f", &tx,",,,		\"x translation in mm\"		\
	   (%f", &ty,",,,		\"y translation in mm\"		\
	   (%f", &tz,",,,		\"z translation in mm\"		\
	   (%f", &roll,",,,		\"angle roll (degrees)\"	\
	   (%f", &pitch,",,,		\"angle pitch (degrees)\"	\
	   (%f", &yaw,",,,		\"angle yaw (degrees)\"))))))	\
	 )								\
	hand 								\
	 (%d", &handValue,",,,	\"hand position setting\")		\
 )", 0);
	 }
	if (keytree == NULL)
	 { printf ("%s Ctree parse error at %.30s\n",fxnName,tree_match_err_at());
	   exit(-1);
	 }
	tree_match_catch_sig (0, sigmask(SIGALRM) | sigmask(SIGUSR1));

	oldSigusr1Handler = signal(SIGUSR1, sigusr1Handler);
 	if (setjmp(env) == 1)
	 { putchar ('\n');
	   strcpy (key_buf, "record");
	   goto teach_end;
	 }

	if (!rcclActive())
	 { errorMsg (ETaskActive, rcclNotActiveMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	flushMotionQueue(mnp);
	mcb->controlModeReq = TEACH_CTRL;
	WAITFOR (mcb->controlModeReq == 0);

	savedIntegration = rbt->how->integration;
	integration = rbt->how->integration;
	
	if (tool)
 	 { writeTrans (mnp->tool, tool);
	 }

 	do
	 { tree_match (keytree, prompt, key_buf);

	   status = 0;

	   identTrsf (&change);
	   readMem (mnp->j6, &j6);
	   readTrans (mnp->t6, &t6);
	   readMem (mnp->j6o, &j6o);
	   readTrans (mnp->t6o, &t6o);
	   conf = mnp->conf;

	   if (COMMAND ("show position"))
	    { float *J6angles = j6.v;
	      float deg[MAXJNTS];
	      float dego[MAXJNTS];
	      int j;

	      printf ("\nIntegration %s  Speed %g  hand %d\n\n", 
		      integration ? "ON" : "OFF", set_speed, mnp->handPos);
	      printTrsf ("T6:  %m\n", &t6);
	      putchar ('\n');
	      printTrsf ("T6O: %m\n", &t6o);
	      putchar ('\n');
	      printf ("JNT      J6          J6O\n");
	      /*        X    XXXX.XXX    XXXX.XXX */
	      radianToDegree (deg, j6.v, mnp->jls);
	      radianToDegree (dego, j6o.v, mnp->jls);
	      for (j=0; j<6; j++)
	       { printf ("%2d    %8.3f    %8.3f\n", 
			  j+1, deg[j], dego[j]);
	       }
	      printf ("\n\n");
	    }
	   else if (COMMAND ("show t6*tool"))
	    { TRSF T6tool;
	      multTrsf (&T6tool, &t6o, mnp->tool);
	      print_trans (&T6tool, "T6 TOOL:");
	      putchar ('\n');
	    }
	   else if (COMMAND ("show tool"))
	    { print_trans (mnp->tool, "TOOL:");
	      putchar ('\n');
	    }
	   else if (COMMAND ("stop"))
	    { mcb->t_stop = YES;
	      WAITFOR (mcb->t_stop == NO);
	    }
	   else if (COMMANDS ("tool x") || COMMANDS ("world x"))
	    { multTrsfXyz (&change, value, 0., 0.);
	    }
	   else if (COMMANDS ("tool y") || COMMANDS ("world y"))
	    { multTrsfXyz (&change, 0., value, 0.);
	    }
	   else if (COMMANDS ("tool z") || COMMANDS ("world z"))
	    { multTrsfXyz (&change, 0., 0., value);
	    }
	   else if (COMMANDS ("tool rx") || COMMANDS ("world rx"))
	    { multTrsfRpy (&change, 0., 0., value);
	    }
	   else if (COMMANDS ("tool ry") || COMMANDS ("world ry"))
	    { multTrsfRpy (&change, 0., value, 0.);
	    }
	   else if (COMMANDS ("tool rz") || COMMANDS ("world rz"))
	    { multTrsfRpy (&change, value, 0., 0.);
	    }
	   else if (COMMANDS ("joint"))
	    { char *jp = &key_buf[strlen("joint ")];
	      joint = *jp - '1';
	      *jp = 'X';
	    }

	   if (COMMANDS ("tool"))
	    { multTrsf 	(&temp, &t6, mnp->tool);
	      multTrsf    (&temp, &temp, &change);
	      multRiTrsf (&temp, &temp, mnp->tool);

	      status = INV_KINEMATICS (j6new.v, &temp, conf, j6.v, NULL);
	      status &= ~(KYN_JOINT_LIMIT | KYN_SINGULAR);
	      status |= rrange_check (j6new.v, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_dest_t6 = temp;
		 mcb->t_motion_mode = TOOL_M;
	       }
	    }
	   else if (COMMANDS ("world"))
	    { 
	      multTrsf    (&temp, &t6, mnp->tool);
	      identTrsf   (&world_trsl);
	      identTrsf   (&world_rot);
	      copyTrsfXyz  (&world_trsl, &temp);
	      copyTrsfNoa   (&world_rot, &temp);
	      multTrsf    (&temp, &world_trsl, &change);
	      multTrsf    (&temp, &temp, &world_rot);
	      multRiTrsf (&temp, &temp, mnp->tool);

	      status = INV_KINEMATICS (j6new.v, &temp, conf, j6.v, NULL);
	      status &= ~(KYN_JOINT_LIMIT | KYN_SINGULAR);
	      status |= rrange_check (j6new.v, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_dest_t6 = temp;
		 mcb->t_motion_mode = WORLD_M;
	       }
	    }
	   else if (COMMAND ("set translation"))
	    { 
	      multTrsf    (&temp, &t6, mnp->tool);
	      xyzToTrsf	(&temp, tx, ty, tz);
	      multRiTrsf (&temp, &temp, mnp->tool);

	      status = INV_KINEMATICS (j6new.v, &temp, conf, j6.v, NULL);
	      status &= ~(KYN_JOINT_LIMIT | KYN_SINGULAR);
	      status |= rrange_check (j6new.v, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_dest_t6 = temp;
		 mcb->t_motion_mode = WORLD_M;
	       }
	    }
	   else if (COMMAND ("set rotation euler"))
	    {
	      multTrsf    (&temp, &t6, mnp->tool);
	      eulToTrsf	(&temp, phi, the, psi);
	      multRiTrsf (&temp, &temp, mnp->tool);

	      status = INV_KINEMATICS (j6new.v, &temp, conf, j6.v, NULL);
	      status &= ~(KYN_JOINT_LIMIT | KYN_SINGULAR);
	      status |= rrange_check (j6new.v, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_dest_t6 = temp;
		 mcb->t_motion_mode = WORLD_M;
	       }
	    }
	   else if (COMMAND ("set rotation rpy"))
	    {
	      multTrsf    (&temp, &t6, mnp->tool);
	      rpyToTrsf	(&temp, roll, pitch, yaw);
	      multRiTrsf (&temp, &temp, mnp->tool);

	      status = INV_KINEMATICS (j6new.v, &temp, conf, j6.v, NULL);
	      status &= ~(KYN_JOINT_LIMIT | KYN_SINGULAR);
	      status |= rrange_check (j6new.v, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_dest_t6 = temp;
		 mcb->t_motion_mode = WORLD_M;
	       }
	    }
	   else if (COMMAND ("set tool"))
	    { TRSF newtool;

	      xyzToTrsf (&newtool, tx, ty, tz);
	      rpyToTrsf (&newtool, roll, pitch, yaw);
	      writeTrans (mnp->tool, &newtool);
	    }
	   else if (COMMAND ("joint X to"))
	    { JNTS jnts;
	      float *jntsF = jnts.v;

	      jnts = j6;
	      if (mnp->jls->jtype[joint] == 'R')
	       { jntsF[joint] = value*DEGTORAD;
	       }
	      else
	       { jntsF[joint] = value;
	       }
	      status = rrange_check (jntsF, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_angles = jnts;
		 mcb->t_motion_mode = JOINT_M;
	       }
	    }
	   else if (COMMAND ("joint X by"))
	    { JNTS jnts;
	      float *jntsF = jnts.v;

	      jnts = j6;
	      if (mnp->jls->jtype[joint] == 'R')
	       { jntsF[joint] += value*DEGTORAD;
	       }
	      else
	       { jntsF[joint] += value;
	       }
	      status = rrange_check (jntsF, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_angles = jnts;
		 mcb->t_motion_mode = JOINT_M;
	       }
	    }
	  else if (COMMAND ("goto joints"))
	    { JNTS jnts;

	      printf ("Enter the %d angle values, in degrees >\n",
		       mnp->jls->numJ);
	      if (fscanVf (tree_match_input_stream(), jnts.v, mnp->jls->numJ) != 1)
	       { int c;
		 printf ("Error -- scanning input\n");
		 while (getchar() != '\n')
		  { /* printf ("%d\n", c);*/;
		  }
		 continue;
	       }
	      degreeToRadian (jnts.v, jnts.v, mnp->jls);
	      status = rrange_check (jnts.v, rbt->jls);
	      if (status != 0)
	       { printf ("Error -- out of range, code = 0x%03x\n", status);
		 continue;
	       }
	      else
	       { WAITFOR (mcb->t_state != CP_MOVE);
		 mcb->t_angles = jnts;
		 mcb->t_motion_mode = JOINT_M;
	       }
	    }
	   else if (COMMAND ("set integration on"))
	    { 
	      SET_INTEGRATION (rbt, ~0);
	      integration = ((1<<rbt->jls->numJ) - 1);
	    }
	   else if (COMMAND ("set integration off"))
	    { 
	      SET_INTEGRATION (rbt, 0);
	      integration = 0;
	    }
	   else if (COMMAND ("set speed"))
	    { WAITFOR (mcb->t_state != CP_MOVE)
	      mcb->t_speed = set_speed;
	    }
	   else if (COMMAND ("pendant"))
	    { printf ("0x%08x 0x%08x (OK=%d)\n",
		      rbt->how->pendantInput[0],
		      rbt->how->pendantInput[1],
		      rbt->how->pendantOK);
	    }
	   else if (COMMAND ("hand"))
	    { SET_HANDPOS (rbt, handValue);
	    }
	 }
	while (!(COMMAND ("return")) && 
	       !(COMMAND ("record")) &&
	       !(rbt->how->pendantInput[0] & RECORD) );

teach_end:
	while (rbt->how->pendantInput[0] & RECORD)
	   ;
	mcb->mcbregs[0] = 0;
	signal (SIGUSR1, oldSigusr1Handler);

	if (integration != savedIntegration)
	 { SET_INTEGRATION (rbt, savedIntegration);
	 }
	mcb->controlModeReq = SETPOINT_CTRL;
	WAITFOR (mcb->controlModeReq == 0);

	if (COMMAND ("record"))
  	 { RETURN_NORMAL (1);
	 }
 	else
  	 { RETURN_NORMAL (0);
	 }
}

print_trans (trans, name)
TRSF *trans;
char *name;
{
	float phi, the, psi, roll, pitch, yaw;

 	printf ("%s\n", name);
 	printTrsf ("%m\n", trans);
	trsfToEul(&phi, &the, &psi, trans);
        putchar ('\n');
        printf ("EULER  phi:  %7.3f   the:   %7.3f   psi: %7.3f\n", 
	      phi, the, psi);
        trsfToRpy(&roll, &pitch, &yaw, trans);
        printf ("RPY    roll: %7.3f   pitch: %7.3f   yaw: %7.3f\n",
	      roll, pitch, yaw);
}
