/*-=-=-=-=-=-=-=-=-=-=-=- 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 move_
#endif

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

#include "macros.h"

int numJoints;

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

identifier(s)
register char *s;
{
#define IDCHAR(c)	(isalnum(c) || (c) == '_' || (c) == '$')
	int c;

	if (strlen(s) >= 1 && IDCHAR(s[0]) && !(isdigit(s[0])) )
	 { for (c = *(++s); c != '\0'; c = *(++s))
	    { if (!(IDCHAR(c)))
	       { return (0);
	       }
	    }
	   return (1);
	 }
	return (0);
} 

jointSpecPart(s)
register char *s;
{
	/* returns true if its argument string is a valid component
	   of a joint spec. 
	 */

	int c;
	for (c = *(++s); c != '\0'; c = *(++s))
	 { if (!isdigit(c) && c != '.' && c != '+' && c != ',' && c != '-')
	    { return (0);
	    }
	 }
	return (1);
}

unsigned long parseJointSpec(angles, spec)
float *angles;
char *spec;
{
	int j;
	register char *cp;
	char *sp;
	unsigned long specified = 0;
	double ang;

	extern char *scanDouble();

	cp = spec;
	specified = 0;
	j = 0;

	while (*cp != '\0')
	 { if (*cp == ',')
	    { if (++j >= MAXJNTS)
	       { fatalMsg (-1,
		 "number of joints specified exceeds %d\n", MAXJNTS);
	       }
	      cp++;
	    }
	   else if (isspace (*cp))
	    { cp++;
	    }
	   else if (sp = scanDouble (&ang, cp))
	    { if (specified & (1<<j))
	       { fatalMsg (-1, 
		"angles in joint spec must be separated by commas\n");
	       }
	      else
	       { float cvt = jls->jtype[j] == 'R' ? DEGTORAD : 1.0;
		 specified |= (1<<j);
		 angles[j] = cvt * ang;
		 cp = sp;
	       }
	    }
	   else
	    { fatalMsg (-1, "can't parse joint specification\n");
	    }
	 }
	return (specified);
}

main(argc, argv)
int argc;
char **argv;
{
	char *robotName = NULL;
	char jointSpec[256];
	char *positionName = NULL;
	int scanningJointSpec = 0;
	float angles[MAXJNTS];
	float degrees[MAXJNTS];
	unsigned long moveCode;
	int turnPowerOff = YES;
	int relative = UNDEF;
	int verbose = 1;
	int testFlag = 0;
	int simflag = 0;
	enum {
		Normal,
		ExpectingPosition,
		ParsingJoints
	     } state;
	int j;

	jointSpec[0] = '\0';

	/* -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

		   Parse arguments according to the syntax:

		move [<robotName>] (to | by) (<position> | <angleSpec>)

	   -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- */

	state = Normal;

	argc--; argv++;
	while (argc > 0)
	 { switch (state)
	    { case Normal:
	       { if (ARGUMENT ("-on"))
		  { turnPowerOff = NO;
		  }
		 else if (ARGUMENT ("-t"))
		  { testFlag = YES;
		  }
		 else 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 ("to"))
		  { if (relative != UNDEF)
		     { argc = -1;
		       goto badUsage;
		     }
		    relative = NO;
		    state = ExpectingPosition;
		  }
		 else if (ARGUMENT ("by"))
		  { if (relative != UNDEF)
		     { argc = -1;
		       goto badUsage;
		     }
		    relative = YES;
		    state = ExpectingPosition;
		  }
		 else if (identifier (*argv))
		  { if (robotName == NULL && find_robot(*argv))
		     { robotName = *argv;
		     }
		    else
		     { argc = -1;
		       goto badUsage;
		     }
		  }
		 else
		  { goto badUsage;
		  }
		 argc--; argv++;
		 break;
	       }
	      case ParsingJoints:
	       { 
		 if (jointSpecPart (*argv))
		  { strncat (jointSpec, " ", 256);
		    strncat (jointSpec, *argv, 256);
		    argc--; argv++;
		  }
		 else
		  { state = Normal;
		  }
		 break;
	       }
	      case ExpectingPosition:
	       { if (identifier (*argv))
		  { positionName = *argv;
		    state = Normal;
		  }
		 else if (jointSpecPart (*argv))
		  { strncpy (jointSpec, *argv, 256);
		    state = ParsingJoints;
		  }
		 else
		  { argc = -1;
		    goto badUsage;
		  }
		 argc--; argv++;
		 break;
	       }
	      default:
	       { argc = -1;
		 goto badUsage;
		 break;
	       }
	    }
	 }
badUsage:
	if (argc == -1)
	 { usageMsg (-1, 
"move <robotName> [to|by] [<positionName>|<jointSpec>]\n\
[-sim[=<simulatorAddr>]] [-on]\n");
	 }
	if (relative == UNDEF)
	 { fatalMsg (-1,
	"must specify ``to'' or ``by'' in motion request\n");
	 }
	if (!robotName)
	 { robotName = getDefaultRobot();
	 }

	jls = getJls(robotName);
	numJoints = jls->numJ;

	if (jointSpec[0] != '\0')
	 { moveCode = parseJointSpec (angles, jointSpec);
	 }
	else
	 { int status;
	   if (relative == YES)
	    { fatalMsg (-1,
	"can't do a relative motion to a defined position\n");
	    }
	   if ((status = 
		getRobotPosition(angles, positionName, robotName)) < 0)
	    { printErrors();
	      exit (-1);
	    }
	   else if (status == 0)
	    { fatalMsg (-1, "position '%s' not defined for robot\n", 
		     positionName);
	    }
	   moveCode = ~0;
	 }

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

	rbt = TASKROBOT(td, 0);
	how = rbt->how;
	moveCode &= ((1<<numJoints)-1);

	cb = (CHASE_CTRL*)chaseTaskMemory(td);

	cb->controlLevelChange = JOINT_LEVEL;

/* MS
	SET_CHECKING (rbt, CHK_MAXVEL | CHK_MAXTOR | CHK_REQTOR | CHK_REQVEL);
*/
	SET_CHECKING (rbt, 0);

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

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

	if (!simflag && !how->calibrated)
  	 { rciClose ();
	   fatalMsg (-1, "arm is not calibrated\n");
	 }


/* MS
	SET_CHECKING (rbt, CHK_MAXVEL | CHK_MAXTOR | CHK_REQTOR | CHK_REQVEL);
*/
	SET_CHECKING (rbt, 0);
	if (!turnPowerOff)
	 { SET_POWER_ON_RELEASE (rbt, 1) ;
	 }
if(!simflag)
{
powerUpChaseTask (td, YES);
WAITFOR (cb->compcode == ALL_JOINTS);
}
#if 0
	if (robot_class(robotName) == PUMA && rbt->jls->flags == PUMA_260)
	 { 
#define J2_NEST_LIMIT	-15.0*DEGTORAD

	   if (how->ang[1] < J2_NEST_LIMIT && (moveCode & 0x2))
	    { printf ("dealing with joint2 first\n");
	      cb->angSpeed[1] /= 2.0;
	      cb->angTarget[1] = J2_NEST_LIMIT;
	      MOVE (0x2);
	      printf ("done with joint 2\n");
	      cb->angSpeed[1] *= 2.0;
	    }
	 }
#endif

	for (j=0; j<numJoints; j++)
	 { if (moveCode & (1<<j))
	    { float angle;

	      if (relative)
	       { angle = cb->angTarget[j] + angles[j];
	       }
	      else
	       { angle = angles[j];
	       }
	      if (jls->jmin[j] >= angle || angle >= jls->jmax[j])
	       { float cvt = jls->jtype[j] == 'R' ? RADTODEG : 1.0;
		 rciClose();
		 fatalMsg (-1, "%g would drive joint %d out of range\n",
			       cvt * angles[j], j+1);
	       }
	      cb->angTarget[j] = angle;
	    }
	 }

	radianToDegree (degrees, cb->angTarget, jls);

	if (testFlag)
	 { printf ("Would move to joint values:\n");
	   for (j=0; j<numJoints; j++)
	    { printf ("%2d %10.4f %s\n", j+1,
		      degrees[j], (moveCode & (1<<j)) ? "" : "(DEFAULT)");
	    }		
	 }
	else
	 { 
	   printf ("Moving to joint values:\n");
	   printVf ("%8.3f\n", degrees, numJoints);
	   MOVE (moveCode);
	   SET_INTEGRATION (rbt, ~0);
	   nap (1.0);	
	 }
	rciClose ();
	exit (0);
}

