/*-=-=-=-=-=-=-=-=-=-=-=- 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:	rciPrintObj
 *
 * DESCRIPTION:	routines to allow the printing of transforms (type TRSF)
 *		and vector objects (arrays of floats) from the RCI control
 *		level.
 *
 *		The routines are otherwise identical to printTrsf() or
 *		printVf().
 *
 * AUTHOR/DATE:	John Lloyd, Winter 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <stdio.h>
#include <varargs.h>

#include "../h/rciTask.h"
#include "../h/robotTypes.h"

extern rciPrintf();
extern doPrintTrsf PROTO((FILE*, const char*, TRSF*, int(*)()));
	
rciPrintTrsf (fmt, tr)
const char *fmt;
TRSF *tr;
{
	doPrintTrsf ((FILE*)NULL, fmt, tr, rciPrintf);
}

/* this code was just adapted directly from the routine printVf() */

rciPrintVf (s, vf, size)
const char *s;
float *vf;
int size;
{
#define MAXF	256
	char buf[MAXF+2];
	char *format;
	register char *cp;
	register int c, i;
	
	strncpy (buf, s, MAXF);
	buf[MAXF] = '\0';

	cp = format = buf;		/* locate format specifier */
	while (*cp != '\0')
	 { if (*cp == '%')
	    { if (*(cp+1) == '%')
	       { cp++;
	       }
	      else
	       { break;
	       }
	    }
	   cp++;
	 }
	
	if (*cp == '\0')
	 { rciPrintf (format);
	 }
	else
	 { *cp = '\0';
	   rciPrintf (format);

	   *cp = '%';
	   format = cp;
	   while (*cp != '\0')
	    { c = *cp;
	      if (c == 'd' || c == 'o' || c == 'x' ||
		  c == 'f' || c == 'g' || c == 'e' ||
		  c == 'F' || c == 'G' ||
		  c == 'c' || c == 's' || c == 'u' )
	       { break;
	       }
	      cp++;
	    }
	   if (*cp != '\0')
 	    { char c1, c2;

	      c1 = cp[1];		/* massage format string to add */
	      c2 = cp[2];		/* extra <space> after number */
	      cp[1] = ' ';
	      cp[2] = '\0';
	      for (i=0; i<size; i++)
	       { rciPrintf (format, vf[i]);
	       }
	      cp[1] = c1;		/* restore format string */
	      cp[2] = c2;

	      format = cp+1;
	      rciPrintf (format);
	    }
	 }
}
