/*-=-=-=-=-=-=-=-=-=-=-=- 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 ~=~=~=~=
 *		
 * MODULE NAME:	display.c
 *
 * DESCRIPTION:	Routines which provide the screen display features for the
 *		simulator.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1986
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include "../h/cdefs.h"
#include "../h/robotMath.h"
#include "display.h"
#include "robotsim.h"

#define NJ 6

display (r, options)
RobotDesc *r;
register int options;
{	
	float degrees[NJ];

	if (options == 0)
	 { return;
	 }

	if (options & DISPLAY_JOINTS)
	 { scaleVf (degrees, RADTODEG, r->rs.angles, 6);
	 }

	if (options & DISPLAY_TIME)
	 { printf ("%g seconds, exchange no. %d\n",
		   r->ss.realTime / 1000., r->ss.exchangeNumber);
	 }
	
	if (options & (DISPLAY_JOINTS | DISPLAY_TORQUE))
	 { int j;

	   printf ("JNT ");
	   if (options & DISPLAY_JOINTS)
	    { printf ("ENCODER   ANGLE   ");
	    }
	   if (options & DISPLAY_TORQUE)
	    { printf (" MOTOR TORQUE ");
	    }
	   printf ("\n");
	   for (j=0; j<6; j++)
	    { 
	      printf (" %d  ", j+1);
	      if (options & DISPLAY_JOINTS)
	       { printf ("%6d   %7.2f  ", r->rs.enc[j], degrees[j]);
	       }
	      if (options & DISPLAY_TORQUE)
	       { printf ("%9.5f   ",  r->rs.mtorque[j]);
	       }
	      printf ("\n");
	    }
	 }
	putchar ('\n');
	if (options & DISPLAY_HAND)
	 { printf ("HAND: position = %d  force = %d\n", 
		   getHandPos(r), getHandForce(r));
	 }
}
