/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	printTrsf
 *
 * DESCRIPTION:	This module contains functions for printing out the value
 *		of a 4X4 transform.
 * 
 * CAVEATS:	The format string is not as compatible with the notions
 *		of printf() as I would like. I will leave it to the reader
 *		to decide who is more at fault.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <stdio.h>
#include <ctype.h>
#include <strings.h>

#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/robotMath.h"

#if defined(sun) || defined(vax)	/* stdio defines this otherwise */
extern fprintf();
#endif

#define DEFAULT_WIDTH		8
#define MINIMUM_WIDTH		5
#define DEFAULT_TPRECISION	2

#define MARGIN_FEATURE		0

enum component { 
	Matrix,
	Euler,
	RollPitchYaw,
	Translation,
	Quaternion,
	Undef
};

typedef struct printControlBlock {
	int fieldWidth;
	int precision1;
	int precision2;
	int precision3;
	char *margin;
	int marginSet;
	int doLabel;
	int (*printFxn)();
	FILE *fp;
} PCB;

static setMargin ();
static char *parse ();
static char *nextSpec ();
static printMatrix ();
static printRollPitchYaw ();
static printEuler ();
static printQuaternion ();
static printTranslation ();

printTrsf (fmt, tr)
const char *fmt;
TRSF *tr;
{
	doPrintTrsf (stdout, fmt, tr, fprintf);
}

fprintTrsf (fp, fmt, tr)
FILE *fp;
const char *fmt;
TRSF *tr;
{
	doPrintTrsf (fp, fmt, tr, fprintf);
}

doPrintTrsf (fp, fmt, tr, fxn)
FILE *fp;
const char *fmt;
TRSF *tr;
int (*fxn)();
{
	char fcpy[256];
	char *cp1 = strcpy(fcpy,fmt);
	char *cp2;
	char margin[256];
	enum component component;
	PCB pcb;

	pcb.margin = margin;
	pcb.marginSet = NO;
	pcb.printFxn = fxn;
	pcb.fp = fp;

	while (cp2 = nextSpec(cp1, &pcb))
	 { 	   
	   /* parse the spec */

	   cp2++;
	   if (*cp2 == '%')
	    { if (pcb.fp)
	       { (*pcb.printFxn) (fp, "%");
	       }
	      else
	       { (*pcb.printFxn) ("%");
	       }
	      cp1 = cp2+1;
	    }
	   else
	    { 
	      cp1 = parse (&component, &pcb, cp2);
   
	      switch (component)
	       { case Matrix:
		  { printMatrix (&pcb, tr);
		    break;
		  }
		 case Euler:
		  { printEuler (&pcb, tr);
   
		    break;
		  }
		 case RollPitchYaw:
		  { printRollPitchYaw (&pcb, tr);
		    break;
		  }
		 case Quaternion:
		  { printQuaternion (&pcb, tr);
		    break;
		  }
		 case Translation:
		  { printTranslation (&pcb, tr);
		    break;
		  }
		 default:
		  { 
		    break;
		  }
	       }
	    }
	 }
}

static char *nextSpec (cp1, pcb)
char *cp1;
PCB *pcb;
{
	char *cp2;
	char *cp;
	
	if (cp2 = (char*)index (cp1, '%'))
	 { *cp2 = '\0';
	 }

	if (pcb->fp)
	 { (*pcb->printFxn) (pcb->fp, cp1);
	 }
	else
	 { (*pcb->printFxn) (cp1);
	 }
	if (!cp2)
	 { return (NULL);
	 }
	else
	 { setMargin (pcb->margin, cp1);
	   pcb->marginSet = 1;
	   *cp2 = '%';
	   return (cp2);
	 }
}

#define TABSIZE	8

static setMargin (margin, cp1)
char *margin, *cp1;
{
	char *cp, *mp;
	int maxLength = 0;
	int length = 0;
	int k;

	for (cp = cp1; *cp; cp++)
	 { if (*cp == '\t')
	    { length = length - (length%TABSIZE) + TABSIZE;
	    }
	   else if (*cp == '\n' || *cp == '\f')
	    { length = 0;
	    }
	   else if (isprint (*cp))
	    { length++;
	    }
	   if (length > maxLength)
	    { maxLength = length;
	    }
	 }
	mp = margin;
	for (k=0; k<maxLength/TABSIZE; k++)
	 { *mp++ = '\t';
	 }
	for (k=0; k<maxLength%TABSIZE; k++)
	 { *mp++ = ' ';
	 }		
	*mp = '\0';
}

static char *parse (comp, pcb, cp2)
enum component *comp;
PCB *pcb;
char *cp2;
{
	PCB ncb;
	char *cp = cp2;

	ncb = *pcb;

	ncb.fieldWidth = DEFAULT_WIDTH;
	ncb.precision1 = UNDEF;
	ncb.precision2 = UNDEF;
	ncb.precision3 = UNDEF;

	if (isdigit(*cp))
	 { int width = atoi(cp);
	   while (isdigit(*cp))
	    { cp++;
	    }
	   ncb.fieldWidth = MAX (MINIMUM_WIDTH, width);
	 }
	if (*cp == '.')
	 { cp++;
	   if (isdigit(*cp))
	    { ncb.precision1 = atoi(cp);
	      while (isdigit(*cp))
	       { cp++;
	       }
	    }
	 }
	if (*cp == '.')
	 { cp++;
	   if (isdigit(*cp))
	    { ncb.precision2 = atoi(cp);
	      while (isdigit(*cp))
	       { cp++;
	       }
	    }
	 }
	if (*cp == '.')
	 { cp++;
	   if (isdigit(*cp))
	    { ncb.precision3 = atoi(cp);
	      while (isdigit(*cp))
	       { cp++;
	       }
	    }
	 }

	/* -- check for qualifiers -- */

	ncb.doLabel = 0;	
	if (*cp == 'L')
	 { cp++;
	   ncb.doLabel = 1;
	 }

	/* -- now determine the component -- */

	switch (*cp)
	 { case 'm':
	    { *comp = Matrix;
	      break;
	    }
	   case 'p':
	    { *comp = Translation;
	      break;
	    }
	   case 'q':
	    { *comp = Quaternion;
	      break;
	    }
	   case 'r':
	    { *comp = RollPitchYaw;
	      break;
	    }
	   case 'e':
	    { *comp = Euler;
	      break;
	    }
	   default:
	    { *comp = Undef;
	      break;
	    }	   
	 }
	cp++;
	*pcb = ncb;
	return (cp);
}

newline (pcb)
PCB *pcb;
{
	if (pcb->marginSet)
	 { if (pcb->fp)
	    { (*pcb->printFxn) (pcb->fp, "\n%s", pcb->margin);
	    }
	   else
	    { (*pcb->printFxn) ("\n%s", pcb->margin);
	    }
	 }
	else
	 { if (pcb->fp)
	    { (*pcb->printFxn) (pcb->fp, "\n");
	    }
	   else
	    { (*pcb->printFxn) ("\n");
	    }
	 }
}

static printMatrix (pcb, tr)
PCB *pcb;
TRSF *tr;
{
	char fmt[256];
	int w = pcb->fieldWidth;
	int tp = pcb->precision1;
	int rp = pcb->precision2;
	int tw = pcb->precision3;

	if (tp == UNDEF)
	 { tp = DEFAULT_TPRECISION;
	 }
	if (rp == UNDEF)
	 { rp = w-3;
	 }
	if (tw == UNDEF)
	 { tw = w;
	 }

	sprintf (fmt, "%%%d.%df %%%d.%df %%%d.%df %%%d.%df",
		      w, rp, w, rp, w, rp, tw, tp);
	if (pcb->fp)
	 { (*pcb->printFxn)(pcb->fp, fmt, tr->n.x, tr->o.x, tr->a.x, tr->p.x);
	 }
	else
	 { (*pcb->printFxn)(fmt, tr->n.x, tr->o.x, tr->a.x, tr->p.x);
	 }
	newline (pcb);
	if (pcb->fp)
	 { (*pcb->printFxn)(pcb->fp, fmt, tr->n.y, tr->o.y, tr->a.y, tr->p.y);
	 }
	else
	 { (*pcb->printFxn)(fmt, tr->n.y, tr->o.y, tr->a.y, tr->p.y);
	 }
	newline (pcb);
	if (pcb->fp)
	 { (*pcb->printFxn)(pcb->fp, fmt, tr->n.z, tr->o.z, tr->a.z, tr->p.z);
	 }
	else
	 { (*pcb->printFxn)(fmt, tr->n.z, tr->o.z, tr->a.z, tr->p.z);
	 }
}

static printRollPitchYaw (pcb, tr)
PCB *pcb;
TRSF *tr;
{
	char fmt[256];
	int w = pcb->fieldWidth;
	int rp = pcb->precision1;
	float roll, pitch, yaw;

	if (rp == UNDEF)
	 { rp = w-5;
	 }

	trsfToRpy (&roll, &pitch, &yaw, tr);

	if (pcb->doLabel)
	 { sprintf (fmt, "%%%ds %%%d.%df %%%d.%df %%%d.%df",
		      w, w, rp, w, rp, w, rp);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, "RPY:", roll, pitch, yaw);
	    }
	   else
	    { (*pcb->printFxn)(fmt, "RPY:", roll, pitch, yaw);
	    }
	 }
	else
	 { sprintf (fmt, "%%%d.%df %%%d.%df %%%d.%df",
		      w, rp, w, rp, w, rp);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, roll, pitch, yaw);
	    }
	   else
	    { (*pcb->printFxn)(fmt, roll, pitch, yaw);
	    }
	 }
}

static printEuler (pcb, tr)
PCB *pcb;
TRSF *tr;
{
	char fmt[256];
	int w = pcb->fieldWidth;
	int rp = pcb->precision1;
	float phi, theta, psi;

	if (rp == UNDEF)
	 { rp = w-5;
	 }

	trsfToEul (&phi, &theta, &psi, tr);

	if (pcb->doLabel)
	 { sprintf (fmt, "%%%ds %%%d.%df %%%d.%df %%%d.%df",
		      w, w, rp, w, rp, w, rp);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, "EUL:", phi, theta, psi);
	    }
	   else
	    { (*pcb->printFxn)(fmt, "EUL:", phi, theta, psi);
	    }
	 }
	else
	 { sprintf (fmt, "%%%d.%df %%%d.%df %%%d.%df",
		      w, rp, w, rp, w, rp);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, phi, theta, psi);
	    }
	   else
	    { (*pcb->printFxn)(fmt, phi, theta, psi);
	    }
	 }
}

static printQuaternion (pcb, tr)
PCB *pcb;
TRSF *tr;
{
	char fmt[256];
	int w = pcb->fieldWidth;
	int rp = pcb->precision1;
	QTRN q;

	if (rp == UNDEF)
	 { rp = w-3;
	 }

	trsfToQtrn (&q, tr);

	if (pcb->doLabel)
	 { sprintf (fmt, "%%%ds %%%d.%df %%%d.%df %%%d.%df %%%d.%df",
		      w, w, rp, w, rp, w, rp, w, rp);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, "QTRN:", q.c, q.v.x, q.v.y, q.v.z);
	    }
	   else
	    { (*pcb->printFxn)(fmt, "QTRN:", q.c, q.v.x, q.v.y, q.v.z);
	    }
	 }
	else
	 { sprintf (fmt, "%%%d.%df %%%d.%df %%%d.%df %%%d.%df",
		      w, rp, w, rp, w, rp, w, rp);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, q.c, q.v.x, q.v.y, q.v.z);
	    }
	   else
	    { (*pcb->printFxn)(fmt, q.c, q.v.x, q.v.y, q.v.z);
	    }
	 }
}

static printTranslation (pcb, tr)
PCB *pcb;
TRSF *tr;
{
	char fmt[256];
	int w = pcb->fieldWidth;
	int p = pcb->precision1;

	if (p == UNDEF)
	 { p = DEFAULT_TPRECISION;
	 }
	if (pcb->doLabel)
	 { sprintf (fmt, "%%%ds %%%d.%df %%%d.%df %%%d.%df",
		      w, w, p, w, p, w, p);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, "XYZ:", tr->p.x, tr->p.y, tr->p.z);
	    }
	   else
	    { (*pcb->printFxn)(fmt, "XYZ:", tr->p.x, tr->p.y, tr->p.z);
	    }
	 }
	else
	 { sprintf (fmt, "%%%d.%df %%%d.%df %%%d.%df",
		      w, p, w, p, w, p);
	   if (pcb->fp)
	    { (*pcb->printFxn)(pcb->fp, fmt, tr->p.x, tr->p.y, tr->p.z);
	    }
	   else
	    { (*pcb->printFxn)(fmt, tr->p.x, tr->p.y, tr->p.z);
	    }
	 }
}

#if TEST

main()
{
	static TRSF t = { 1.0,  0.0,  0.0,
		   0.0,  0.0,  1.0,
		   0.0, -1.0,  0.0,
		  10.0, 20.0, 30.0 };
	char ibuf[256];
	char fmt[256];

	while (1)
	 { if (isatty (fileno(stdin)))
	    { printf ("Format> ");
	    }
	   if (!gets (ibuf))
	    { break;
	    }
	   makeformat (fmt, ibuf);
	   printTrsf (fmt, &t);
	 }
}


makeformat (s1, s2)
char *s1, *s2;
{
	while (*s2)
	 { if (*s2 == '\\')
	    { s2++;
	      switch (*s2)
	       { case 'n':
		  { *s1++ = '\n';
		    break;
		  }
		 case 't':
		  { *s1++ = '\t';
		    break;
		  }
		 case 'f':
		  { *s1++ = '\f';
		    break;
		  }
		 default:
		  { *s1++ = *s2;
		  }
	       }
	      s2++;
	    }
	   else
	    { *s1++ = *s2++;
	    }
	 }
	*s1 = '\0';
}

#endif
