/*-=-=-=-=-=-=-=-=-=-=-=- 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:	zerograv
 *
 * USAGE:	zerograv
 *
 * DESCRIPTION: Program to put the arm into zero gravity mode. It is 
 *		assumed that the arm powered up, running the RCI monitor,
 *		and calibrated.
 * 
 * NOTES:	
 *
 * AUTHOR/DATE: John Lloyd, March, 1985
 *
 * UPDATES:
 *
 *-------------McGill-Computer-Vision-and-Robotics-Laboratory--------------*/

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

#ifdef VXWORKS_CFG
#define main zerograv_
#endif

#include <strings.h>
#include "../h/chase.h"
#include "../h/rciTask.h"
#include "../h/rciMacros.h"
#include "../h/robotClass.h"
#include "../h/robotMath.h"
#include "../h/rciOptions.h"
#include "../h/ctree.h"

#include "../h/robot_kynvar.h"
#include "../h/softStops.h"

#include "macros.h"

char key_buf[256];

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

extern int robot_grav();

int frictionCompensationOn;

checkFrictionComp()
{
	return (frictionCompensationOn);
}

main(argc, argv)
int argc;
char **argv;
{
	int numJoints;
	int simflag = 0;
	int turnPowerOff = YES;
	char *robotName = NULL;
	char *keytree;
	float fval;
	char *doFriction;
	int j;

	frictionCompensationOn = NO;

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

	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 (ARGUMENT ("-friction"))
	    { frictionCompensationOn = YES;
	    }
	   else if (!robotName)
	    { robotName = *argv;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { usageMsg (-1, "free [<robotName>] [-sim[=<simulatorAddr>]] [-on]");
	 }
	if (!robotName)
	 { robotName = (char*)getDefaultRobot();
	 }	

	keytree = tree_match_parse("					\
(	quit								\
	show 								\
	hand (open close)						\
	frictionComp ? ", checkFrictionComp, "				\
	   ([1 2 3 4 5 6 all] (on off))					\
	set ? ", checkFrictionComp, "					\
	 (frictionComp							\
	   ([1 2 3 4 5 6] (%f", &fval, ",,,\"new friction coef\")))	\
)",0);
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	td = (RCI_DESC*)createChaseTask(robotName, simflag);

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

	switch (robot_class(robotName))
	 { 
#if 0
	   case PUMA:
	    { 
	      break;
	    }
	   default:
	    { printf ("zerograv does not support this class of robot\n");
	      exit (-1);
	    }
#endif
	 }

	cb = (CHASE_CTRL*)chaseTaskMemory(td);

	cb->controlLevelChange = JOINT_LEVEL;

	cb->liveZone = 0.0;
	SET_CHECKING (rbt, CHK_MAXVEL | CHK_REQVEL | CHK_MAXTOR | CHK_REQTOR);
	if (startChaseTask(td) < 0)
	 { RCI_EXIT (-1);
	 }
	rciBlock(td);

	if (!how->calibrated)
	 { printf ("Robot reports that it is not calibrated\n");
	   rciClose();
	   exit (-1);
	 }
	if (!turnPowerOff)
	 { SET_POWER_ON_RELEASE (rbt, 1) ;
	 }
	powerUpChaseTask (td, YES);
	WAITFOR (cb->compcode == ALL_JOINTS);

#if SOFT_STOPS
	if (how->ang[5] > jls->jmax[5] || 
	    how->ang[5] < jls->jmin[5])
	 { fprintf (stderr, "\
Error -- Joint 6 is out of range.\n\
You can move it back into range using 'move' or 'free'\n");
	   rciClose();
	   exit (-1);
	 }
#endif

	doFriction = (char*)allocMem("doFriction",
				      sizeof(char)*jls->numJ,UNDEF);

	cb->jTorqueFunction = robot_grav;
	for (j=0; j<numJoints; j++)
	 { cb->mTorque[j] = 0.0;
	   cb->torqueControl[j] = TORQUE_FUNCTION;
	 }

	tree_match_catch_sig (0, sigmask(SIGALRM));

       	do
	 { tree_match (keytree, "ZEROGRAV> ", key_buf);
	   if (COMMAND ("show"))
	    { int j;
	      putchar ('\n');

	      if (frictionCompensationOn)
	       { 
	printf (" Jnt   Encoders   Angles    Friction Compensation\n");
		 for (j=0; j<numJoints; j++)
		  { printf (" %2d     %6d   %8.3f  %8.4f  %s\n", j+1, 
			    how->enc[j],
			    RADTODEG*how->ang[j],
			    kyn->gen->coulFriction[j],
			    doFriction[j] ? "ON" : "");
		  }
	       }
	      else
	       { 
	printf (" Jnt   Encoders   Angles\n");
		 for (j=0; j<numJoints; j++)
		  { printf (" %2d     %6d   %8.3f\n", j+1, 
			    how->enc[j],
			    RADTODEG*how->ang[j]);
		  }
	       }
	      putchar ('\n');
	    }
	   else if (COMMAND ("hand open"))
	    { SET_HANDPOS (rbt, 1);
	    }
	   else if (COMMAND ("hand close"))
	    { SET_HANDPOS (rbt, 0);
	    }
	   else if (COMMAND ("frictionComp all on"))
	    { for (j=0; j<jls->numJ; j++)
	       { doFriction[j] = 1;
	       }
	    }
	   else if (COMMAND ("frictionComp all off"))
	    { for (j=0; j<jls->numJ; j++)
	       { doFriction[j] = 0;
	       }
	    }
	   else if (COMMANDS ("frictionComp"))
	    { char *kp = &key_buf[strlen("frictionComp")+1];

	      j = *kp - '1';
	      kp += 2;
	      if (strcmp (kp, "on") == 0)
	       { doFriction[j] = 1;
	       }
	      else if (strcmp (kp, "off") == 0)
	       { doFriction[j] = 0;
	       }
	      else
	       { fprintf(stderr, "Internal error -- botched command\n");
		 exit (-1);
	       }
	    }
	   else if (COMMANDS ("set frictionComp"))
	    { j = key_buf[strlen("set frictionComp")+1] - '1';
	      if (fval < 0.0 || fval > 10.0)
	       { printf ("friction value must be in the range 0 to 10.0\n");
	       }
	      else
	       { printf ("setting friction %g for joint %d\n", fval, j+1);
	   	 kyn->gen->coulFriction[j] = fval;
	       }
	    }
	 }
	while (!COMMAND ("quit"));
	
	rciBlock(td);
	rciClose();
	exit (0);
}
