/* **************************************************************
   *								*
   * (c) Copyright 1999 Mariusz Zaczek                 		*
   * ALL RIGHTS RESERVED                                        *
   *								*
   * Permission to use, copy, modify, and distribute this	*
   * software for any purpose and without fee is hereby         *
   * granted, provided that the above copyright notice          *
   * appear in all copies and that both the copyright notice	*
   * and this permission notice appear in supporting		*
   * documentation, and that the name of Mariusz Zaczek. not 	*
   * be used in advertising or publicity pertaining to		*
   * distribution of the software without specific, written	*
   * prior permission. (zaczek@uiuc.edu)			*
   *								*
   *    THE MATERIAL EMBODIED ON THIS SOFTWARE IS PROVIDED	*
   *    TO YOU "AS-IS" AND WITHOUT WARRANTY OF ANY KIND, 	*
   *    EXPRESSED, IMPLIED OR OTHERWISE, INCLUDING WITHOUT 	*
   *	LIMITATION, ANY WARRANTY OF MERCHANTABILITY OR      	*
   *    FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT		*
   *	SHALL MARIUSZ ZACZEK BE LIABLE TO YOU OR ANYONE		*
   *	ELSE FOR ANY DIRECT, SPECIAL, INCIDENTAL, INDIRECT	*
   *	OR CONSEQUENTIAL DAMAGES OF ANY KIND, OR ANY DAMAGES 	*
   *	WHATSOEVER, INCLUDING WITHOUT LIMITATION, LOSS OF 	*
   *	PROFIT, LOSS OF USE, SAVINGS OR REVENUE, OR THE 	*
   *	CLAIMS OF THIRD PARTIES, WHETHER OR NOT MARIUSZ 	*
   *	ZACZEK HAS BEEN ADVISED OF THE POSSIBILITY OF 		*
   *	SUCH LOSS, HOWEVER CAUSED AND ON ANY THEORY OF 		*
   *	LIABILITY, ARISING OUT OF OR IN CONNECTION WITH THE     *
   *    POSSESSION, USE OR PERFORMANCE OF THIS SOFTWARE.        *
   *                                                            *
   *    US Government Users Restricted Rights                   *
   *    Use, duplication, or disclosure by the Government is	*
   *	subject to restrictions set forth in 			*
   *    FAR 52.227.19(c)(2) or subparagraph (c)(1)(ii) of the 	*
   *	Rights in Technical Data and Computer Software       	*
   *    clause at DFARS 252.227-7013 and/or in similar or       *
   *	successor clauses in the FAR or the DOD or NASA FAR     *
   *    Supplement. Unpublished-- rights reserved under the     *
   *	copyright laws of the United States.                    *
   *								*
   ************************************************************** */



/* **************************************************************
   *								*
   *   SCANNER.C						*
   *								*
   *     Author: Mariusz Zaczek (zaczek@uiuc.edu)		*
   *	 Date:   May 5, 1999					*
   *     							*
   *   This program is part of the 3-D scanner project in the   *
   *   Advanced Digital Systems Lab (ADSL) in the Electrical    *
   *   Engineering Department at the University of Illinois at  *
   *   Urbana-Champaign.					*
   *   This project was started and completed by Mariusz Zaczek *
   *   and Aaron Trask, both students in the Aeronautical and   *
   *   Astronautical Engineering department.			*
   *								*
   *   The 3-D Scanner project involves the use of a X-Y table  *
   *   which allows movement of a probe along the x and y axes  *
   *   through the use of two stepper motors. A third motor     *
   *   controls the position of the probe in the vertical or Z  *
   *   axis and thus allows for the mapping of a solid object   *
   *   placed on the bottom surface of the table. The probe     *
   *   used a raster scan technique to move over the part in    *
   *   specified steps and at each step move the probe along    *
   *   the vertical axis until contact is made with the part.   *
   *   The coordinates of the probe tip are stored in a file    *
   *   as well as displayed to the screen upon contact. After   *
   *   the completion of the specified scan the entire          *
   *   surface mapping displayed on the screen is continuously  *
   *   rotated to show the three dimensional part.              *
   *   This scanning only maps the vertical characteristics of  *
   *   the part under scan and thus is most easily understood   *
   *   to be the "contour" map of the part.			*
   *   								*
   *   The control of the stepper motors is done via the  	*
   *   parallel port of a PC.					*
   *								*
   *   The code has been compiled using Borland C		*
   *								*
   *   STEPS TO BEGIN A SCAN.					*
   *	  - compile and run the code.				*
   * 	  - position the probe at the lowest ( z ) position 	*
   *		of the scanning area until an interrupt 	*
   *		indicates to stop moving further.		*
   *	  - move the probe to the maximum height position to	*
   *		set the upper scan bound....hit ENTER when 	*
   *		this position is reached.			*
   *	  - Input the maximum (x) and (y) dimensions of the 	*
   *		part in milimeters.				*
   *	  - Input the step size to be taken along the (x) 	*
   *	        and (y) directions in milimeters.		*
   *	  - when the initial graphics screen is reached hit	*
   *	      	the ENTER key to begin scanning (points will	*
   *		be displayed to the screen - no rotation	*
   *		of part yet)					*
   *	  - when scanning is complete, the part will now 	*
   *		rotate on the screen.				*
   *	  - hit ESC to exit.					*
   *								*
   *								*
   ************************************************************** */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <dos.h>
#include <conio.h>
#include <graphics.h>
#include <string.h>
#include <bios.h>

/* Define variable for reading from Parallel port. */
/* Note: 0x0378 is not the common port address on every
	 computer and thus should be checked. One can
	 do the check by going into a DOS prompt and
	 typing:

        >debug      
        -d 0040:0008 L8      
        0040:0008       78 03 78 02 00 00 00 00

        Simply reverse the first two values ( 78 03 )
        to get 0378  for LPT1...
        also ( 78 02 ) --> 0278 is for LPT2 
						*/

#define DATA 0x0378	        /* Output pins */
#define STATUS DATA+1           /* Input pins...interrupt */
#define CONTROL DATA+2

#define TRUE  1
#define FALSE 0         /* Never actually used... :) */

/* Scaling factors used for rendering of image */
#define SCALE  2.0
#define SCALE2 2.0

enum errors { NO_ERROR=0, GRAPHICS_ERROR };

/* Global variables */
float 	angle;
int 	up_pulses = 0;
int 	num_pts = 0;

/* data file used to store all binary position
   information to be sent to the printer parallel
   port. _data_ is broken up into the following
   sections:
		       7 6 5 4 3 2 1 0
       binary string   x x x x x x x x

   where bits  ( 2 1 0 ) represent the +/- Y direction
					motor
	       ( 5 4 3 ) represent the +/- X direction
					motor
	       ( 7 6 )   represent the +/- Z direction
					motor

      Note: for the X and Y motors the code shifts
	    a 0(low) bit through while all other bits
	    are 1(hi). This is done because an inverter
	    is used to buffer the signal on the actual
	    circuit and thus logic 1 will be actually
	    shifted in the circuit but logic 0 must
	    therefore be outputted from the parallel port.
 
            SEE CIRCUIT DIAGRAM TO UNDERSTAND THIS BETTER...
            INVERTERS WERE USED TO HELP PREVENT ANY BACKWASH
            OF CURRENT DIRECTLY INTO THE COMPUTER, AS A 
            RESULT ALL OUTPUT VIA THE PARALLEL PORT IS 
            THE OPPOSITE (i.e. bits are flipped) WHAT
            WOULD BE EXPECTED.
            
*/
int 	data;

/* Function declarations */
int 	getkey(void);
int 	initialize(FILE *strm1);
void 	display_data(void);
int	scan(FILE *strm1);
int     open_graphics(void);
void    render ( float xa, float ya, float za,
		 float x[], float y[], float z[],
		 float rx[], float ry[], float rz[],
		 float scrx[], float scry[]);


/* Start of Main code. */



/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++
   +							+
   +  main  						+
   +							+
   +    - main function...calls initalize() and scan()  +
   +      functions.					+
   + 							+
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
int main(void)
{
  FILE *strm1;

  strm1 = fopen("scan_out.txt","a");

  /* Initialize the DATA port (i.e. all motors to be engaged) */
  data = 0x36;       /*   00110110   */
  outportb(DATA,data);

  fprintf(strm1,"\n3D Scanner output"
	        "\n - data given as coordinate locations"
 	        "\n    of points.\n");
  fflush(strm1);

  /* Output introduction and instructions. */
  printf("\n**********************************************"
	 "\n*						*"
	 "\n*   3-D Profile Scanner                     *"
 	 "\n*						*"
	 "\n*        by Mariusz Zaczek & Aaron Trask	*"
	 "\n*						*"
	 "\n*     (ADSL) Advanced Digital Systems Lab	*"
	 "\n* 	            Spring 1999			*"
	 "\n*						*"
	 "\n**********************************************" );

  printf("\n\n\n"
	 "\n   In order to begin scanning operation, the "
	 "\n probe head must be positioned in a start    "
	 "\n position and this location to be locked in. " );

  /* Run initialization of probe location code */
  if ( initialize(strm1) )
    printf("\n Position locked in place...prepare for scanning");
  else
  {
    printf("\n Error during initialization....quitting program");
    exit(0);
  }

  /* Begin scan setup and start data taking */
  scan(strm1);

  fclose(strm1);
  return 0;
}



/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++
   +							+
   +  getkey						+
   +							+
   +	- function to determine number of key pressed.  +
   +    - parameters: none.				+
   +							+
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
int getkey(void)
{
  int i;

  switch( i = (int)getch() )
  {
    case 0xe0:
    case 0:
      return 256 + (int)getch();

    default:
      return i;
  }
}



/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++
   +							+
   +  initialize					+
   +							+
   +	- function to initialize position of probe head +
   +    - User moves probe to lowest possible position  +
   +      until interrupt is generated...this is used   +
   +      as the base z=0 location for all points.      +
   +        - Next the user then positions the probe    +
   +          at the highest point of part to           +
   +          initialize max height to be traversed by  +
   +          the probe.				+
   +    - parameters: strm1 - output file		+
   +							+
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
int initialize(FILE *strm1)
{
 int choice, z=0, status=0;

 printf("\n"
	"\n  Use the following keys to control the 	"
	"\n motors to position the probe at bottom left	"
	"\n location of the part ... \n"
	"\n   Motion direction            key"
	"\n  -------------------------------------"
	"\n     + X 			right arrow"
	"\n     - X			left arrow"
	"\n     + Y			up arrow"
	"\n     - U			down arrow"
	"\n     probe up                   +"
	"\n        FAST UP                PageUp"
	"\n     probe down                 -"
	"\n        FAST DOWN              PageDown"
	"\n     Escape                    ESC"
	"\n\n\t   hit the ENTER key to lock position"
	"\n\n\t Move motors now ...\n\n"
	"\nMove the probe down until it reaches the"
	"\nlowest possible position (interrupt should"
	"\nengage)... next move the probe to the highest"
	"\nposition and hit the Enter key to lock"
	"\nthis position in place.\n");

 while(TRUE)
 {
   choice = getkey();

   if ( ((((inportb(STATUS))^0x80 ) & 0x80 ) == 0) && status==0)
   {
     printf("\nZERO location locked...move to highest"
	    " position and press ENTER when done.\n");
     up_pulses = 0;
     status=1;
   }


   if ( choice == 328 )    /*    UP    +Y direction       */
   {
     if ( !(data & 1) )
     {
       data = data | 0x04;
       data = data & (~0x02);
       data = data | 0x01;                   /*  xxxxx101 */
     }
     else if ( !(data & 2) )
     {
       data = data & (~0x04);
       data = data | 0x02;
       data = data | 0x01;                   /*  xxxxx011 */
     }
     else if ( !(data & 4) )
     {
       data = data | 0x04;
       data = data | 0x02;
       data = data & (~0x01);                /*  xxxxx110 */
     }
   }
   else
   if ( choice == 336 )    /*   DOWN   -Y direction       */
   {
     if ( !(data & 1) )
     {
       data = data & (~0x04);
       data = data | 0x02;
       data = data | 0x01;                   /*  xxxxx011 */
     }
     else if ( !(data & 2) )
     {
       data = data | 0x04;
       data = data | 0x02;
       data = data & (~0x01);                /*  xxxxx110 */
     }
     else if ( !(data & 4) )
     {
       data = data | 0x04;
       data = data & (~0x02);
       data = data | 0x01;                   /*  xxxxx101 */
     }
   }
   else
   if ( choice == 331 )    /*   LEFT   -X direction       */
   {
     if ( !(data & 8) )
     {
       data = data | 0x20;
       data = data & (~0x10);
       data = data | 0x08;                   /*  xx101xxx */
     }
     else if ( !(data & 16) )
     {
       data = data & (~0x20);
       data = data | 0x10;
       data = data | 0x08;                   /*  xx011xxx */
     }
     else if ( !(data & 32) )
     {
       data = data | 0x20;
       data = data | 0x10;
       data = data & (~0x08);                /*  xx110xxx */
     }
   }
   else
   if ( choice == 333 )	   /*   RIGHT  +X direction       */
   {
     if ( !(data & 8) )
     {
       data = data & (~0x20);
       data = data | 0x10;
       data = data | 0x08;                   /*  xx011xxx */
     }
     else if ( !(data & 16) )
     {
       data = data | 0x20;
       data = data | 0x10;
       data = data & (~0x08);                /*  xx110xxx */
     }
     else if ( !(data & 32) )
     {
       data = data | 0x20;
       data = data & (~0x10);
       data = data | 0x08;                   /*  xx101xxx */
     }
   }
   else
   if ( choice == 43 )     /*     +    probe up        */
   {
     up_pulses++;

     /* NOTE... INVERTER USED */
     if ( !(data & 128) && !(data & 64) )    /* if ( A B ) */
     {
       data = data & (~0x80);                /* Then ( A B* ) */
       data = data | 0x40;                   /* 01xxxxxx */
     }
     else if ( !(data & 128) && (data & 64) )/* if ( A B* ) */
     {
       data = data | 0x80;                   /* Then ( A* B* ) */
       data = data | 0x40;                   /* 11xxxxxx */
     }
     else if ( (data & 128) && (data & 64) ) /* if ( A* B* ) */
     {
       data = data | 0x80;                   /* Then ( A* B ) */
       data = data & (~0x40);                /* 10xxxxxx */
     }
     else if ( (data & 128) && !(data & 64) )/* if ( A* B ) */
     {
       data = data & (~0x80);                /* Then ( A B ) */
       data = data & (~0x40);                /* 00xxxxxx */
     }
   }
   else
   if ( choice == 45 )     /*     -    probe down   */
   {
     up_pulses--;

     /* Note using INVERTER on inputs. */
     if ( !(data & 128) && !(data & 64) )    /* if ( A B ) */
     {
       data = data | 0x80;                   /* Then ( A* B ) */
       data = data & (~0x40);                /* 10xxxxxx */
     }
     else if ( (data & 128) && !(data & 64) )/* if ( A* B ) */
     {
       data = data | 0x80;                   /* Then ( A* B* ) */
       data = data | 0x40;                   /* 11xxxxxx */
     }
     else if ( (data & 128) && (data & 64) ) /* if ( A* B* ) */
     {
       data = data & (~0x80);                /* Then ( A B* ) */
       data = data | 0x40;                   /* 01xxxxxx */
     }
     else if ( !(data & 128) && (data & 64) )/* if ( A B* ) */
     {
       data = data & (~0x80);                /* Then ( A B ) */
       data = data & (~0x40);                /* 00xxxxxx */
     }
   }
   else
   if ( choice == 329 )    /*     PAGEUP  - FAST UP
					probe movement. */
   {
     up_pulses+=10;

     for ( z = 0 ; z < 10 ; z++ )
     {
       if ( !(data & 128) && !(data & 64) )  /* if ( A B ) */
       {
	 data = data & (~0x80);              /* Then ( A B* ) */
	 data = data | 0x40;                 /* 01xxxxxx */
      }
       else if ( !(data & 128) && (data & 64) )/* if ( A B* ) */
       {
	 data = data | 0x80;                 /* Then ( A* B* ) */
	 data = data | 0x40;                 /* 11xxxxxx */
       }
       else if ( (data & 128) && (data & 64) )/* if ( A* B* ) */
       {
	 data = data | 0x80;                 /* Then ( A* B ) */
	 data = data & (~0x40);              /* 10xxxxxx */
       }
       else if ( (data & 128) && !(data & 64) )/* if ( A* B ) */
       {
	 data = data & (~0x80);              /* Then ( A B ) */
	 data = data & (~0x40);              /* 00xxxxxx */
       }
       outportb(DATA,data);
       delay(10);
     }
   }
    else
   if ( choice == 337 )    /*     PAGEDOWN  - FAST DOWN
					probe movement. */
   {
     up_pulses-=10;

     for ( z = 0 ; z < 10 ; z++ )
     {
       if ( !(data & 128) && !(data & 64) )   /* if ( A B ) */
       {
	 data = data | 0x80;                  /* Then ( A* B ) */
	 data = data & (~0x40);               /* 10xxxxxx */
       }
       else if ( (data & 128) && !(data & 64) )/* if ( A* B ) */
       {
	 data = data | 0x80;                  /* Then ( A* B* ) */
	 data = data | 0x40;                  /* 11xxxxxx */
       }
       else if ( (data & 128) && (data & 64) )/* if ( A* B* ) */
       {
	 data = data & (~0x80);               /* Then ( A B* ) */
	 data = data | 0x40;                  /* 01xxxxxx */
       }
       else if ( !(data & 128) && (data & 64) )/* if ( A B* ) */
       {
	 data = data & (~0x80);               /* Then ( A B ) */
	 data = data & (~0x40);               /* 00xxxxxx */
       }
       outportb(DATA,data);
       delay(10);
     }
   }
   else
   if ( choice == 27 )     /*     ESCAPE - quit     */
     return 0;
   else
   if ( choice == 13 )     /*     ENTER - Lock location     */
   {
     fprintf(strm1,"\nHighest position equals ( %d ) pulses",
		  up_pulses);
     fflush(strm1);
     return 1;
   }

/*   display_data(); */

   outportb(DATA,data);
 }
}


/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++
   +							+
   +  display_data					+
   +							+
   +    - function to display the binary representation +
   +      of the data variable to show how pulses are   +
   +      applied to each motor.			+
   +    - WARNING: Used primarily for debugging         +
   +      purposes.                                     +
   +      IF YOU WISH TO DEBUG THEN COMMENT OUT ALL     +
   +      outportb(??) LINES AND REMOVE THE COMMENTS    +
   +      FROM THE display_data() LINES.                +
   +    - parameters: none				+
   +							+
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
void display_data(void)
{
  if ( data & 128 )
     printf("\n1");
  else
     printf("\n0");

  if ( data & 64 )
     printf("1");
  else
     printf("0");

  if ( data & 32 )
     printf("1");
  else
     printf("0");

  if ( data & 16 )
     printf("1");
  else
     printf("0");

  if ( data & 8 )
     printf("1");
  else
     printf("0");

  if ( data & 4 )
     printf("1");
  else
     printf("0");

  if ( data & 2 )
     printf("1");
  else
     printf("0");

  if ( data & 1 )
     printf("1");
  else
     printf("0");

}


/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++
   +							+
   +  scan						+
   +							+
   +    - function to perform scanning routine.         +
   +    - scanning occurs by steppingthe probe along    +
   +      along the x and y directions and at each step +
   +      bring it down to the surface until contact    +
   +      is initiated and an interrupt is generated.   +
   +    - parameters: strm1 - output file		+
   +							+
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
int scan(FILE *strm1)
{
  int 	max_X,	     /* max X location of part */
	max_Y,	     /* max Y location of part */
	stepX,	     /* Step size for probing along X
						 direction */
	stepY,       /* Step size for probing along Y
						 direction */
	numStepX,    /* The number of steps in the X
						 direction */
	numStepY,    /* The number of steps in the Y
						 direction */
	pulseX,      /* # of pulses to execute one
					step in X dir.*/
	pulseY;      /* # of pulses to execute one
					step in Y dir.*/

  int	downPulses,  /* # of pulses to reach part
					from top position */
	a,	     /* Counters */
	b,
	c,
	d,
	e;
  /* Rotation and perspective arrays. */
  float          x[2500],    y[2500],    z[2500],
		rx[2500],   ry[2500],   rz[2500],
	      scrx[2500], scry[2500];

  printf("\n\n..........SCAN SETUP.........."
	 "\n Enter the maximum part envelope...i.e. the"
	 "\n maximum X and Y dimensions of the part in "
	 "\n milimeters (mm)....."
	 "\n Enter X and Y:  ");
  scanf("%d%d",&max_X,&max_Y);

  fprintf(strm1,"\n SCAN AREA PARAMETERS:"
		"\n\t  max_X location = %d"
		"\n\t  max_Y location = %d",max_X,max_Y);
  fflush(strm1);

  printf("\n\n Enter maximum step size (mm) of probing along"
	 "\n X and Y directions....Step X and Step Y:  ");
  scanf("%d%d",&stepX,&stepY);

  /* The number of steps to be taken along X & Y directions. */
  numStepX = (int)( max_X / stepX );
  numStepY = (int)( max_Y / stepY );

  /* Redefine step sizes based on max dimensions provided. */
  stepX = max_X / numStepX;
  stepY = max_Y / numStepY;

  fprintf(strm1,"\n\t  Steps along X direction = %d"
		"\n\t  Steps along Y direction = %d",stepX,stepY);
  fprintf(strm1,"\n   x\t   y\t   z\n");
  fflush(strm1);

  /* Use a formula obtained from testing to determine how many
     pulses are required for each motor to move it one (1) step. */

  /* X stepping: 60 pulses = 5mm
	 ---> e.g. step size of 5mm = 60 pulses (12 pulses/mm)*/
  pulseX = 12 * stepX;

  /* Y stepping: */
  pulseY = 12 * stepY;

  /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     +								+
     +    Start scanning... 					+
     +								+
     ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
  /* The motion will require moving in the +Y direction followed
     by moving over a step in the +X and returning along the -Y
     direction, taking a step in the +X direction and repeating. *
       ->-   ->-
      |  |  |
      |  |  |       ... etc.     ( o = start position )
      ^  |  ^
      |  v  |
      o  |->-               				       */

  /* Initialize graphics */
  /* Check for any errors in opening a graphics window */
  if (open_graphics() == GRAPHICS_ERROR)  return(GRAPHICS_ERROR);
         
  setcolor(RED);

  /* Draw opening screen */
  outtextxy(130,200," X-Y-Z Scanner ... Zaczek, Mariusz & Trask, Aaron");

  /* Wait for keystroke...then clear screen*/
  getch();
  cleardevice();

  /* Since already at start location, lower probe down to obtain
     first point. */
  for ( a=0 ; a<numStepX ; a++ )
  {
    for ( b=0 ; b<numStepY ; b++)
    {
      /* Initialize the # of pulses, that will be counted until contact
	 is made, to zero. */
      downPulses = 0;

      /* Continue moving probe head down until it reaches contact. */
      while( ( ( (inportb(STATUS))^0x80 ) & 0x80 ) != 0 )
      {
	downPulses++;
	/* Move one pulse down since no interrupt has been received */

	if ( !(data & 128) && !(data & 64) )         /* if ( A B ) */
	{
	  data = data | 0x80;                        /* Then ( A* B ) */
	  data = data & (~0x40);                     /* 10xxxxxx */
	}
	else if ( (data & 128) && !(data & 64) )     /* if ( A* B ) */
	{
	  data = data | 0x80;                        /* Then ( A* B* ) */
	  data = data | 0x40;                        /* 11xxxxxx */
	}
	else if ( (data & 128) && (data & 64) )      /* if ( A* B* ) */
	{
	  data = data & (~0x80);                     /* Then ( A B* ) */
	  data = data | 0x40;                        /* 01xxxxxx */
	}
	else if ( !(data & 128) && (data & 64) )     /* if ( A B* ) */
	{
	  data = data & (~0x80);                     /* Then ( A B ) */
	  data = data & (~0x40);                     /* 00xxxxxx */
	}

	outportb(DATA,data);
	delay(5);
      }

      /* Interrupt has been received ... determine coordinates of stop. */
      x[num_pts] = (float)(a * stepX * SCALE - 50);
      if ( (a % 2) == 0 )
	y[num_pts] = (float)((b * stepY * SCALE) - 50);
      else if ( (a % 2) == 1 )
	y[num_pts] = (float)(((numStepY-b-1) * stepY * SCALE) - 50);

      /* 77 pulses/milimeter */
      z[num_pts] = (float)(((up_pulses - downPulses)/77.0) * SCALE2);

    /* Output data points to a file.*/
    fprintf(strm1,"%f %f %f",x[num_pts],y[num_pts],z[num_pts]);
    fprintf(strm1,"\n");
    fflush(strm1);

      num_pts++;

      /* Display point to screen...do not rotate yet --> will rotate at end. */
      render(0,0,0,x,y,z,rx,ry,rz,scrx,scry);

      /* Return probe to original position on top. */
      for ( c=0 ; c<downPulses ; c++ )
      {
	/* REVERSE MOVEMENT */

	/* Note INVERTER used */
	if ( !(data & 128) && !(data & 64) )        /* if ( A B )    */
	{
	  data = data & (~0x80);                    /* Then ( A B* ) */
	  data = data | 0x40;                       /* 10xxxxxx      */
	}
	else if ( !(data & 128) && (data & 64) )    /* if ( A B* )   */
	{
	  data = data | 0x80;                       /* Then ( A* B* )*/
	  data = data | 0x40;                       /* 00xxxxxx      */
	}
	else if ( (data & 128) && (data & 64) )     /* if ( A* B* )  */
	{
	  data = data | 0x80;                       /* Then ( A* B ) */
	  data = data & (~0x40);                    /* 01xxxxxx      */
	}
	else if ( (data & 128) && !(data & 64) )    /* if ( A* B )   */
	{
	  data = data & (~0x80);                    /* Then ( A B )  */
	  data = data & (~0x40);                    /* 11xxxxxx      */
	}

	outportb(DATA,data);
	delay(5);
      }


      /* Move in the Y direction -- Based on _pulseY_ = # of pulses. */
    if (b != numStepY-1)
      for ( d=0 ; d<pulseY ; d++)
      {
	/* Move one step in +Y direction */
	if ( (a % 2) == 0 )
	{
	  if ( !(data & 1) )
	  {
	    data = data | 0x04;
	    data = data & (~0x02);
	    data = data | 0x01;                   /*  xxxxx101 */
	  }
	  else if ( !(data & 2) )
	  {
	    data = data & (~0x04);
	    data = data | 0x02;
	    data = data | 0x01;                   /*  xxxxx011 */
	  }
	  else if ( !(data & 4) )
	  {
	    data = data | 0x04;
	    data = data | 0x02;
	    data = data & (~0x01);                /*  xxxxx110 */
	  }
	}
	else if ( (a % 2) == 1 )
	{
	  if ( !(data & 1) )
	  {
	    data = data & (~0x04);
	    data = data | 0x02;
	    data = data | 0x01;                   /*  xxxxx011 */
	  }
	  else if ( !(data & 2) )
	  {
	    data = data | 0x04;
	    data = data | 0x02;
	    data = data & (~0x01);                /*  xxxxx110 */
	  }
	  else if ( !(data & 4) )
	  {
	    data = data | 0x04;
	    data = data & (~0x02);
	    data = data | 0x01;                   /*  xxxxx101 */
	  }
	}

/* display_data(); */
	outportb(DATA,data);
	delay(50);

      }   /* for (d=0..... */
    }    /* for (b=0..... */

   if (a != numStepX-1)
    for ( e=0 ; e<pulseX ; e++)
    {
      /* Move one step over to right. (+X direction) */
      if ( !(data & 8) )
      {
	data = data & (~0x20);
	data = data | 0x10;
	data = data | 0x08;                   /*  xx011xxx */
      }
      else if ( !(data & 16) )
      {
	data = data | 0x20;
	data = data | 0x10;
	data = data & (~0x08);                /*  xx110xxx */
      }
      else if ( !(data & 32) )
      {
	data = data | 0x20;
	data = data & (~0x10);
	data = data | 0x08;                   /*  xx101xxx */
      }

/*  display_data(); */
      outportb(DATA,data);
      delay(50);
    }  /* for (e=0...... */
  }  /* for (a=0...... */


  cleardevice();


  fprintf(strm1,"\n%d",num_pts);
  fflush(strm1);

  while ( TRUE )
  {
    if ( !kbhit() )
    {
      render(angle,0,0,x,y,z,rx,ry,rz,scrx,scry);
      angle += 2.0f;
      if (angle==360)
	angle=0;

      delay(5);
      cleardevice();
    }
    else
    {
      if ( getkey() == 27 )
      {
	closegraph();
	return 0;
      }
    }
  }
} /* End of _scan()_ function. */



/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++
   +							+
   +  open_graphics					+
   +							+
   +	- function to initialize graphics to display    +
   +      rotating surface 				+
   +	- parameters: none				+
   + 							+
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
int     open_graphics(void)
{
	int   gdriver=DETECT,gmode,errorcode;
	union REGS inregs, outregs;

   /* Needed for mouse initialization ... I don't think I
      use a mouse so this is not needed, but it doesn't
      hurt */
   inregs.x.ax=0x0011;

   int86(0x10,&inregs,&outregs);

   /* Needed for general graphics driver initialization */
   initgraph(&gdriver,&gmode," ");

   /* Test if any errors occur */
   errorcode=graphresult();

   if (errorcode != grOk)
   {
      printf("Graphics error: %s\n",grapherrormsg(errorcode));
      return(GRAPHICS_ERROR);
   }

   return(NO_ERROR);
}


/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++
   +							+
   +  render						+
   +							+
   +    - function to display each point on the screen  +
   +      and rotate the final collection of points     +
   +      once scanning of part is complete.		+
   +    - parameters: xa - x point coordinate		+
   +		      ya - y point coordinate		+
   +		      za - z point coordinate		+
   +		      x[] - array of x coordinates	+
   +		      y[] - array of y coordinates      +
   + 		      z[] - array of z coordinates      +
   +		      rx[] - rotation vector x		+
   +		      ry[] - rotation vector y		+
   +		      rz[] - rotation vector z		+
   +		      scrx[] - scaling x		+
   +		      scry[] - scaling y		+
   +							+
   +  Most of the code in this function was obtained    +
   +  from the following site:				+
   +  http://www.flipcode.com/portal/  (1999)	        +
   +							+
   +    "Building a 3D Portal Engine" by 		+
   +		Jacco.Bikker@HTA.nl			+
   +							+
   ++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
void render ( float xa, float ya, float za, float x[],
	      float y[], float z[], float rx[],
	      float ry[], float rz[], float scrx[],
	      float scry[])
{
  int 	i;
  float mat[4][4];
  float xdeg=xa*3.1416f/180,
	ydeg=ya*3.1416f/180,
	zdeg=za*3.1416f/180;

  float sx=(float)sin(xdeg),
	sy=(float)sin(ydeg),
	sz=(float)sin(zdeg);

  float cx=(float)cos(xdeg),
	cy=(float)cos(ydeg),
	cz=(float)cos(zdeg);

  /* Rotation Matrix */
  mat[0][0]=cx*cz+sx*sy*sz;
  mat[1][0]=-cx*sz+cz*sx*sy;
  mat[2][0]=cy*sx;
  mat[0][1]=cy*sz;
  mat[1][1]=cy*cz;
  mat[2][1]=-sy;
  mat[0][2]=-cz*sx+cx*sy*sz;
  mat[1][2]=sx*sz+cx*cz*sy;
  mat[2][2]=cx*cy;

  /* Calculate rotation vectors. */
  for ( i=0 ; i<num_pts ; i++ )
  {
    rx[i]=x[i]*mat[0][0]+y[i]*mat[1][0]+z[i]*mat[2][0];
    ry[i]=x[i]*mat[0][1]+y[i]*mat[1][1]+z[i]*mat[2][1];
    rz[i]=x[i]*mat[0][2]+y[i]*mat[1][2]+z[i]*mat[2][2]+300;

    scrx[i]=(rx[i]*500)/rz[i]+320;
    scry[i]=(ry[i]*500)/rz[i]+240;
   }

  setcolor(WHITE);

  /* Display circles of radius 1 at point location. */
  for( i=0 ; i<num_pts ; i++ )
   circle((int)scrx[i],(int)scry[i],1);

  setcolor(RED);

  /* Draw lines connecting current and previous points. */
  for( i=0 ; i<num_pts-1 ; i++ )
    line((int)scrx[i],(int)scry[i],(int)scrx[i+1],(int)scry[i+1]);
}