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

#include <strings.h>
#include "../h/cdefs.h"
#include "../h/rccl.h"

main(argc, argv)
int argc;
char **argv;
{
	char *robotName = NULL;
	char *filename = NULL;
	FILE *fp = NULL;
	MANIP *mnp;
	int status;
	int count;

	argc--; argv++;
	while (argc)
	 { if (ARGUMENTS ("-sim="))
	    { char *sname;
	      if (strlen(sname=index(*argv,'=')+1) == 0)
	       { argc = -1;
		 break;
	       }
	      rcclSetSimulator(sname);
	    }
	   else if (ARGUMENT ("-sim"))
	    { rcclSetOptions (RCCL_SIMULATE);
	    }
	   else if (ARGUMENT ("-on"))
	    { rcclSetOptions (RCCL_LEAVE_POWER_ON);
	    }
	   else if (ARGUMENT ("-f"))
	    { if (filename)
	       { argc = -1;
		 break;
	       }
	      argc--; argv++;
	      filename = *argv;
	    }
	   else if (!robotName)
	    { robotName = *argv;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc == -1)
	 { usageMsg (-1, 
"teachdemo [<robotName>] [-sim[=<simulatorAddr>]] [-f <filename>] [-on]\n");
	 }
	if (!robotName)
	 { robotName = (char*)getDefaultRobot();
	   printf("robotName = %s\n", robotName);
	 }
	if (filename)
	 { if ((fp = fopen (filename, "w")) == NULL)
	    { fatalMsg (-1, "Can't open file %s\n", filename);
	    }
	 }

	if ((mnp = rcclCreate (robotName, 0)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	rcclStart();

	count = 0;
	while ((status = rcclTeach(mnp, "TEACHDEMO> ", NULL)) > 0)
	 { printf ("Recorded transform is:\n");
	   printTrsf ("%m\n", mnp->here);
	   if (filename)
	    { float deg[6];

	      fprintf (fp, "T%d:\n", count);
	      fprintTrsf (fp, "T6:  %m\n", mnp->here);
	      radianToDegree (deg, mnp->j6->v, mnp->jls);
	      fprintVf (fp, "J6:  %9.4f\n", deg, 6);
	    }
	   count++;
	 }
	if (status < 0)
	 { printErrors();
	   exit(-1);
	 }
	rcclRelease(1);
	exit (0);
}
