/*-=-=-=-=-=-=-=-=-=-=-=- 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 free_
#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/pendant.h"
#include "macros.h"

int numJoints;

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

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

char *getDefaultRobot();

main(argc, argv)
int argc;
char **argv;
{
	char *keytree, *tree_match_parse(), *tree_match_err_at();

	int j;
	int warned = 0;
	char *robotName = NULL;
	int simflag = NO;
	int turnPowerOff = YES;

	argc--; argv++;
	while (argc)
	 {
	   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)
	    { robotName = *argv;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { usageMsg (-1, 
"free [<robotName>] [-sim[=<simulatorAddr>]] [-on]\n");
	 }
	if (!robotName)
	 { robotName = getDefaultRobot();
	 }	
	keytree = tree_match_parse("					\
(	quit								\
	[free lock]							\
	 (all 1 2 3 4 5 6)						\
	show 								\
)",0);
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }

	rciSetOptions (EXIT_ON_ERROR);
	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 = MOTOR_LEVEL;

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

	cb->liveZone = 0.0;
	if (startChaseTask(td) < 0)
	 { RCI_EXIT (-1);
	 }
	if (!turnPowerOff)
	 { SET_POWER_ON_RELEASE (rbt, 1) ;
	 }
	powerUpChaseTask (td, YES);
	WAITFOR (cb->compcode == ALL_JOINTS);

	tree_match_catch_sig (0, sigmask(SIGALRM));

	printf ("\nType '?' for commands\n");
	do
 	 { 
	   tree_match (keytree, "FREE> ", key_buf);

	   if (COMMAND("free all"))
	    { 
	      if (!warned)
	       { 
printf ("Make sure the arm is supported before freeing it.\n");
printf ("Type <CR>, or hit the 'FREE' button on the pendant, to confirm:\n");
		 warned = 1;
	       }
	      else
	       { printf ("<CR> or 'FREE' to confirm:\n");
	       }
	      while (1)
	       { if (testchar (0.0))
		  { if (getchar() == '\n')
		     { break;
		     }
		  }
		 if (how->pendantInput[0] & FREE_BUTTON)
		  { break;
		  }
	       }

	      for (j=0; j<numJoints; j++)
 	       { cb->mTorque[j] = 0.0;
		 cb->torqueControl[j] = TORQUE_ZERO;
	       }
	    }
	   else if (COMMANDS ("free"))
	    { j = key_buf[strlen("free ")] - '1';

	      if (j == 1 || j == 2)
	       {
	         if (!warned)
	          { 
printf ("Make sure the link is supported before freeing it.\n");
printf ("Type <CR>, or hit the 'FREE' button on the pendant, to confirm:\n");
		    warned = 1;
	          }
	         else
	          { printf ("<CR> or 'FREE' to confirm:\n");
	          }
		 while (1)
		  { if (testchar (0.))
		     { if (getchar() == '\n')
			{ break;
			}
		     }
		    if (how->pendantInput[0] & FREE_BUTTON)
		     { break;
		     }
		  }
	       }
	      cb->mTorque[j] = 0.0;
	      cb->torqueControl[j] = TORQUE_ZERO;
	    }
	   else if (COMMAND ("lock all"))
	    { 
	      for (j=0; j<numJoints; j++)
	       { cb->torqueControl[j] = 0;
	       }
	    }
	   else if (COMMANDS ("lock"))
	    { j = key_buf[strlen("lock ")] - '1';
	      cb->torqueControl[j] = 0;
	    }
	   else if (COMMAND ("show"))
	    { 
	      putchar ('\n');

	      if (how->calibrated)
	       { printf (" JNT   ENCODERS   ANGLES\n");
	         for (j=0; j<numJoints; j++)
	          { printf (" %2d     %6d   %8.3f  %s\n", j+1, how->enc[j],
			    RADTODEG*how->ang[j],
			    cb->torqueControl[j] ? "(FREE)" : "");
	          }
		 putchar ('\n');
	       }
	      else 
	       { printf ("Robot reports it is not calibrated\n");
		 putchar ('\n');
		 printf (" JNT   ENCODERS\n");
	         for (j=0; j<numJoints; j++)
	          { printf (" %2d     %6d  %s\n", j+1, how->enc[j],
		    cb->torqueControl[j] ? "(FREE)" : "");
	          }
	         printf ("\n");
	       }
	    }
	 }
	while (!(COMMAND("quit")));

	rciClose();
	printf ("\n");
	exit (0);
}
