/** desk_scan.c -- 
 *
 * Paul Blaer <psb15@cs.columbia.edu>
 * Lenny Volchok <lv71@columbia.edu>
 * Naveed Hassan <naveed@cs.columbia.edu>
 * Corey Tripp <crt17@columbia.edu>
 *
 * Last updated:  5/1/2001
 *
 * Usage:  desk_scan <light_cal_file> <camera_cal_file> <contrast> <desk_plane_epsilon> <top-ref-row> <bottom-ref-row> <output-file> <scan_files...>
 *
 * Compilation: use make.
 *
 **/


/* Includes: */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "pgm.h"


/* Various defines: */

/* A random buffer length. */
#define FILENAMELENGTH 128

/* Our camera gives us a small black edge along either side of the
   image.  We use the values to exclude those edges. */
#define SIDECUTOFFRIGHT 30
#define SIDECUTOFFLEFT 30

/* Boolean, because 0 and 1 just ain't good enough. */
#define FALSE 0
#define TRUE 1


/* Various useful structures: */

/* We use this structure to store the individual pixel values. */
struct pixel {
  double row, col;
};
typedef struct pixel *pixel;

/* We use this structure to hold the final 3D points. */
struct point {
  double x, y, z;
};
typedef struct point *point;

/* The camera calibration constants:

   Normal: r3, r6, r9
   Distance: Tz/f
   Focal length: f
   Distortion: Kappa1

*/
typedef struct {
  double    f;                /* [mm]          */
  double    kappa1;           /* [1/mm^2]      */
  double    p1;               /* [1/mm]        */
  double    p2;               /* [1/mm]        */
  double    Tx;               /* [mm]          */
  double    Ty;               /* [mm]          */
  double    Tz;               /* [mm]          */
  double    Rx;               /* [rad]         */
  double    Ry;               /* [rad]         */
  double    Rz;               /* [rad]         */
  double    r1;               /* []            */
  double    r2;               /* []            */
  double    r3;               /* []            */
  double    r4;               /* []            */
  double    r5;               /* []            */
  double    r6;               /* []            */
  double    r7;               /* []            */
  double    r8;               /* []            */
  double    r9;               /* []            */
} calibration_constants;


/* More camera calibration parameters. */
typedef struct {
  double Ncx; /* [sel]     Number of sensor elements in camera's x direction */
  double Nfx; /* [pix]     Number of pixels in frame grabber's x direction   */
  double dx;  /* [mm/sel]  X dimension of camera's sensor element (in mm)    */
  double dy;  /* [mm/sel]  Y dimension of camera's sensor element (in mm)    */
  double dpx; /* [mm/pix]  Effective X dimension of pixel in frame grabber   */
  double dpy; /* [mm/pix]  Effective Y dimension of pixel in frame grabber   */
  double Cx;  /* [pix]     Z axis intercept of camera coordinate system      */
  double Cy;  /* [pix]     Z axis intercept of camera coordinate system      */
  double sx;  /* []        Scale factor to compensate for any error in dpx   */
} camera_parameters;


/* Print the usage. */
void usage(char *progName) {
  fprintf(stderr,"Usage:  %s <light_cal_file> <camera_cal_file> <contrast> <desk_plane_epsilon> <top-ref-row> <bottom-ref-row> <output-file> <scan_files...>\n",progName);
}

/* Reads in the camera calibration constants into an existing
   structure. */
int readParams( char *fname, camera_parameters *cp, calibration_constants *cc );

/* Reads in the light calibration and returns it as a 3D point. */
point readLightParams(char *fname);

/* Functions to perform the temporal and spatial analysis of the image
   data: */

/* This function computes, for each pixel in the image (less the black
   stripes at the edges), the time that the shadow edge touches that
   pixel.  The time is actually the frame in which this occurs.
   However, we do a sub-frame sampling using interpolation, hence the
   shadow times are in fact double values.  In addition it also
   computes the two reference points for each image which we will use
   later as two of the pieces of the shadow plane.

   Variables:

     pixel *shadowPlanesTop -- the array of top reference points
     pixel *shadowPlanesBottom -- the array of bottom reference points
     int refRowTop -- the row number of the top reference row
     int refRowBottom -- the row number of the bottom reference row
     char *files -- a list of the names of the image files that
                    contain the image data.  
     int numImages -- the number of files in the previous list
     int contrast -- the contrast with which to threshold

   Return values -- It returns a two dimensional (1 slot for each
   pixel) that contains the sub-frame sampled shadow times.
  
*/
double **findShadowTimes(pixel *shadowPlanesTop, pixel *shadowPlanesBottom, 
			 int refRowTop, int refRowBottom, char *files[FILENAMELENGTH], 
			 int numImages, int contrast);

/* This function takes a single, already opened image and computes the
   reference point along the specified reference row and returns it in
   a pixel structure.

   Variables:

     struct image *im -- the image structure.
     refRow -- the reference line.
     contrast -- the contrast with which to threshold

   Return value -- returns a single pixel structure that represents
   the pixel at which the shadow edge is along the reference row.

 */
pixel findReferencePoint(struct image *im, int refRow, int contrast);


/* Functions to compute the final 3D-points: */

/* Function pixelTo3D
 ********************
 * Fills x, y, and z with coordinates of the 3D point,
 * which corresponds to the (px, py) pixel,
 * where:
 * sh_a, sh_b, sh_c, sh_d are the shadow parameters,
 * which define the shadow plane of the form:
 * sh_a*x + sh_b*y + sh_c*z = sh_d
 * f is the focal lenght of the camera
 * (px, py) are the image coordinates (pixel)
 */
void pixelTo3D (double *x, double *y, double *z,
		double sh_a, double sh_b, double sh_c, double sh_d,
		double px, double py, double f);

/* Function: findShadowPlane
 ***************************
 * This function finds the shadow plane of the form:
 * ax + by + cz = d
 * when given the following parameters:
 * (nx, ny, nz) is the normal vector from the camera to the desktop plane,
 * dist is the distance from the camera to the desktop plane
 * f is the focal lenght of the camera
 * (light_x, light_y, light_z) are the 3D coordinates of the light
 * (top_x, top_y) are the image coordinates of the top point
 * in the shadow edge
 * (bot_x, bot_y) are the image coordinates of the bottom point
 * in the shadow edge
 */
void findShadowPlane (double *a, double *b, double *c, double *d,
		      double nx, double ny, double nz,
		      double dist, double f,
		      double light_x, double light_y, double light_z,
		      double top_x, double top_y,
		      double bot_x, double bot_y);


/* Helper functions for the 3D point computation: */
void vectorPlaneIntersect (double *x, double *y, double *z,
			   double a, double b, double c, double d,
			   double vx, double vy, double vz);

void pointsToPlane (double *pa, double *pb, double *pc, double *pd,
		    double x1, double y1, double z1,
		    double x2, double y2, double z2,
		    double x3, double y3, double z3);

double determinant (double n00, double n01, double n02,
		    double n10, double n11, double n12,
		    double n20, double n21, double n22);

int orderEqs (double *x1, double *y1, double *z1,
	      double *x2, double *y2, double *z2,
	      double *x3, double *y3, double *z3);

void swap3Doubles (double *x1, double *y1, double *z1,
		   double *x2, double *y2, double *z2);

double distToPlane(double x, double y, double z,
		   double a, double b, double c, double d);

/* Implementations: */

/* Reads in the camera calibration constants into an existing
   structure. */
int readParams(char *fname, camera_parameters *cp, calibration_constants *cc) {

  FILE *fp;
  double sa, ca, sb, cb, sg, cg;

  if( !fname || (fp = fopen(fname, "r")) == 0 ) return FALSE;

  fscanf(fp, "%lf", &(cp->Ncx));
  fscanf(fp, "%lf", &(cp->Nfx));
  fscanf(fp, "%lf", &(cp->dx));
  fscanf(fp, "%lf", &(cp->dy));
  fscanf(fp, "%lf", &(cp->dpx));
  fscanf(fp, "%lf", &(cp->dpy));
  fscanf(fp, "%lf", &(cp->Cx));
  fscanf(fp, "%lf", &(cp->Cy));
  fscanf(fp, "%lf", &(cp->sx));
  fscanf(fp, "%lf", &(cc->f));
  fscanf(fp, "%lf", &(cc->kappa1));
  fscanf(fp, "%lf", &(cc->Tx));
  fscanf(fp, "%lf", &(cc->Ty));
  fscanf(fp, "%lf", &(cc->Tz));
  fscanf(fp, "%lf", &(cc->Rx));
  fscanf(fp, "%lf", &(cc->Ry));
  fscanf(fp, "%lf", &(cc->Rz));

  sa = sin(cc->Rx);
  ca = cos(cc->Rx);
  sb = sin(cc->Ry);
  cb = cos(cc->Ry);
  sg = sin(cc->Rz);
  cg = cos(cc->Rz);

  cc->r1 = cb * cg;
  cc->r2 = cg * sa * sb - ca * sg;
  cc->r3 = sa * sg + ca * cg * sb;
  cc->r4 = cb * sg;
  cc->r5 = sa * sb * sg + ca * cg;
  cc->r6 = ca * sb * sg - cg * sa;
  cc->r7 = -sb;
  cc->r8 = cb * sa;
  cc->r9 = ca * cb;

  fscanf(fp, "%lf", &(cc->p1));
  fscanf(fp, "%lf", &(cc->p2));

  fclose(fp);

  return TRUE;

} /*end readParams()*/


/* Reads in the light calibration and returns it as a 3D point. */
point readLightParams(char *fname) {

  FILE *fp;
  point l;
  
  /* Open the file. */
  if( !fname || (fp = fopen(fname, "r")) == 0 ) return NULL;

  /* Create the structure. */
  l = (point) calloc(1, sizeof(struct point));

  /* Read in the points. */
  fscanf(fp,"%lf %lf %lf",&(l->x),&(l->y),&(l->z));

  return l;

}


/* Computes the shadow times. */
double **findShadowTimes(pixel *shadowPlanesTop, pixel *shadowPlanesBottom, 
			 int refRowTop, int refRowBottom, char *files[FILENAMELENGTH], 
			 int numImages, int contrast) {
  
  int row, col, maxRows, maxCols, image, x;
  int **imin, **imax, **icontrast, **prev;
  double **ishadow, **t;
  struct image *im;
  
  /* Initialize the image. */
  im = (struct image *)malloc(sizeof(struct image));
  
  /* Make sure that we have at least 2 images, otherwise we can't even
     try to do this. */
  if(numImages<2) {
    fprintf(stderr,"You need more than one image to compute anything.\n");
    return NULL;
  }

  /* We're going to need to process the first image to initialize everything. */
  if (readImage(im, files[0])) {
    fprintf(stderr,"Error Reading %s!\n", files[0]);
    return NULL;
  }
  
  /* Get the dimensions. */
  maxRows = getNRows(im);
  maxCols = getNCols(im);
  
  /* Now that we have the dimensions we can create our arrays. */
  imin = (int **) calloc(maxRows, sizeof(int *));
  imax = (int **) calloc(maxRows, sizeof(int *));
  icontrast = (int **) calloc(maxRows, sizeof(int *));
  prev = (int **) calloc(maxRows, sizeof(int *));
  ishadow = (double **) calloc(maxRows, sizeof(double *));
  t = (double **) calloc(maxRows, sizeof(double *));
  
  for(row=0;row<maxRows;row++) {
    imin[row] = (int *) calloc(maxCols, sizeof(int));
    imax[row] = (int *) calloc(maxCols, sizeof(int));
    icontrast[row] = (int *) calloc(maxCols, sizeof(int));
    prev[row] = (int *) calloc(maxCols, sizeof(int));
    ishadow[row] = (double *) calloc(maxCols, sizeof(double));
    t[row] = (double *) calloc(maxCols, sizeof(double));
  }
  
  /* Now we find the initial maximum. */
  for(row=0;row<maxRows;row++) {
    for(col=SIDECUTOFFLEFT;col<maxCols-SIDECUTOFFRIGHT;col++) {
      prev[row][col] = imin[row][col] = imax[row][col] = getPixel(im,row,col);
      icontrast[row][col] = 0;
      ishadow[row][col] = imax[row][col];
      t[row][col] = -1;
    }
  }

  /* Compute the shadow plane for the image that we already have
     open. */
  if((shadowPlanesTop[0] = findReferencePoint(im,refRowTop,contrast))==NULL)
    fprintf(stderr,"Unable to find top reference point in: %s.\n",files[0]);
  if((shadowPlanesBottom[0] = findReferencePoint(im,refRowBottom,contrast))==NULL)
    fprintf(stderr,"Unable to find bottom reference point in: %s.\n",files[0]);

  /* Finally, we have to go through each image and do the temporal
     analysis. */
  for(image=1;image<numImages;image++) {

    /* Read in the new file. */
    if (readImage(im, files[image])) {
      fprintf(stderr,"Error Reading %s!\n", files[image]);
      return NULL;
    }
    
    /* Make sure it was the right size. */
    if(maxRows != getNRows(im) || maxCols != getNCols(im)) {
      fprintf(stderr,"Error, file %s is the wrong size.\n",files[image]);
      return NULL;
    }
    
    /* Before we do anything we compute the shadow planes with for
       this image. */
    if((shadowPlanesTop[image] = findReferencePoint(im,refRowTop,contrast))==NULL)
      fprintf(stderr,"Unable to find top reference point in: %s.\n",files[image]);
    if((shadowPlanesBottom[image] = findReferencePoint(im,refRowBottom,contrast))==NULL)
      fprintf(stderr,"Unable to find bottom reference point in: %s.\n",files[image]);

    /* Loop through all of the rows. */
    for(row=0;row<maxRows;row++) {
      
      /* Loop through all the columns. */
      for(col=SIDECUTOFFLEFT;col<maxCols-SIDECUTOFFRIGHT;col++) {
      
        /* Check to make sure we haven't already set this pixel. */
        if(t[row][col]!=-1)
	  continue;

	/* Get the pixel. */
	x = getPixel(im,row,col);
	
	/* Update the variables as needed. */
	if(x>imax[row][col])
	  imax[row][col] = x;
	if(x<imin[row][col])
	  imin[row][col] = x;
	icontrast[row][col] = imax[row][col] - imin[row][col];
	ishadow[row][col] = ((double)imax[row][col] + (double)imin[row][col])/2.0;
	
	/* Check to see if we have a high enough contrast. */
	if(icontrast[row][col] > contrast) {
	  
	  /* See if we've crossed the shadow line going up. */
	  if(prev[row][col]<ishadow[row][col] && x>ishadow[row][col]) {

	    t[row][col] = (1.0-(ishadow[row][col]-prev[row][col])/(x-prev[row][col]))*
	      (double)(image-1) + ((ishadow[row][col]-prev[row][col])/
				   (x-prev[row][col]))*(double)(image);

	  }

	}

	prev[row][col] = x;
	
      } /* End col loop. */
      
    } /* End row loop. */
    
  } /* End image loop. */
  
  return t;

} /* End findShadowTimes. */


/* Computes the reference points. */
pixel findReferencePoint(struct image *im, int refRow, int contrast) {

  pixel p;
  int col, dcol, maxCols, maxRows, x, imin, imax, icontrast;
  double ishadow, current, prev;

  /* Initialize our return structure. */
  p = (pixel)malloc(sizeof(struct pixel));
  p->row = refRow;
  p->col = -1;

  /* Get the dimensions of the image. */
  maxRows = getNRows(im);
  maxCols = getNCols(im);
  
  /* Make sure that the reference line is on the image. */
  if(refRow<0 || refRow >= maxRows) {
    fprintf(stderr,"The reference row is off of the image\n");
    return NULL;
  }

  /* Initialize the variables. */
  imax = imin = getPixel(im, refRow, SIDECUTOFFLEFT);
  icontrast = 0;
  ishadow = imax;

  /* Now lets loop through the entire row. */
  dcol = -1;
  for(col=SIDECUTOFFLEFT+1;col<maxCols-SIDECUTOFFRIGHT;col++) {
    x = getPixel(im, refRow, col);

    /* Update the variables. */
    if(x<imin)
      imin = x;
    if(x>imax)
      imax = x;
    icontrast = imax - imin;
    ishadow = ((double)imax + (double)imin)/2.0;

    /* Check to see if we've crossed the threshold. */
    if(icontrast>contrast)
      if(x<ishadow) {
	dcol = col;
	break;
      }

  }

  /* If we didn't find any crossings, return NULL. */
  if(dcol==-1)
    return NULL;

  /* Now we have to do a linear interpolation to find the exact pixel
     at which it crossed, down to the sub-pixel level. */
  if(dcol!=0) {
    current = getPixel(im, refRow, dcol);
    prev = getPixel(im, refRow, dcol-1);
    p->col = (1.0-(ishadow-prev)/(current-prev))*(double)(dcol-1) + 
      ((ishadow-prev)/(current-prev))*(double)(dcol);
  } else {
    p->col = dcol;
  } 

  return p;

} /* End findReferencePoint. */


/* Function pixelTo3D
 ********************
 * Fills x, y, and z with coordinates of the 3D point,
 * which corresponds to the (px, py) pixel,
 * where:
 * sh_a, sh_b, sh_c, sh_d are the shadow parameters,
 * which define the shadow plane of the form:
 * sh_a*x + sh_b*y + sh_c*z = sh_d
 * f is the focal lenght of the camera
 * (px, py) are the image coordinates (pixel)
 */
void pixelTo3D (double *x, double *y, double *z,
		double sh_a, double sh_b, double sh_c, double sh_d,
		double px, double py, double f) {
  /* 
   * find the intersection between
   * the shadow plane and the vector (px, py, f)
   */
  vectorPlaneIntersect (x, y, z,
			sh_a, sh_b, sh_c, sh_d,
			px, py, f);
  /* (*x, *y, *z) contain the coordinates of the 3D point we need */
}


/* Function: findShadowPlane
 ***************************
 * This function finds the shadow plane of the form:
 * ax + by + cz = d
 * when given the following parameters:
 * (nx, ny, nz) is the normal vector from the camera to the desktop plane,
 * dist is the distance from the camera to the desktop plane
 * f is the focal lenght of the camera
 * (light_x, light_y, light_z) are the 3D coordinates of the light
 * (top_x, top_y) are the image coordinates of the top point
 * in the shadow edge
 * (bot_x, bot_y) are the image coordinates of the bottom point
 * in the shadow edge
 */
void findShadowPlane (double *a, double *b, double *c, double *d,
		      double nx, double ny, double nz,
		      double dist, double f,
		      double light_x, double light_y, double light_z,
		      double top_x, double top_y,
		      double bot_x, double bot_y) {

  double desk_a, desk_b, desk_c, desk_d;
  double x1, y1, z1;
  double x2, y2, z2;
  
  /* find the parameters for the desktop plane */
  desk_a = nx;
  desk_b = ny;
  desk_c = nz;
  /* check this... */
  desk_d = dist / sqrt (nx*nx + ny*ny + nz*nz);
  
  /* find the 3D point corresponding to the top shadow edge point */
  vectorPlaneIntersect (&x1, &y1, &z1,
			desk_a, desk_b, desk_c, desk_d,
			top_x, top_y, f);
  
  /* find the 3D point corresponding to the bottom shadow edge point */
  vectorPlaneIntersect (&x2, &y2, &z2,
			desk_a, desk_b, desk_c, desk_d,
			bot_x, bot_y, f);
  
  /* 
   * we have the three points that define the shadow plane
   * -> get the shadow plane equation
   */
  pointsToPlane(a, b, c, d,
		x1, y1, z1,
		x2, y2, z2,
		light_x, light_y, light_z);
}


/* Function vectorPlaneIntersect
 *******************************
 * This function finds the intersection between a vector and a plane,
 * which, we hope, is a 3D point (*x, *y, *z).
 * The plane is defined by parameters a, b, c, and d
 * and is of the form ax + by + cz = d
 * while the vector is defined by (vx, vy, vz).
 */
void vectorPlaneIntersect (double *x, double *y, double *z,
			   double a, double b, double c, double d,
			   double vx, double vy, double vz) {
  double k, den;
  /*
   * one finds the intersection between
   * a plane of the form ax + by + cz = d
   * and a vector of the form (x,y,z)
   * in the following manner
   * -----------------------------------------
   * the point is going to lie on the vector
   * therefore, it's coordinates are going to
   * be (kx, ky, kz), where k is an unknown,
   * while x, y, and z are known from the
   * vector.
   * -> (ax)k + (by)k + (cz)k = d
   * -> k = d / (ax + by + cz)
   * x, y, and z are known from the vector
   * a, b, c, and d are known from the plane
   */
  
  /* check that we are not dividing by 0 */
  den = a*vx + b*vy + c*vz;
  if (den) {
    k = d / den;
  }
  else {
    fprintf(stderr, "O-oh...  Cannot find the intersection!\n");
    exit(1);
  }
  
  /* our 3D point is just (k*vx, k*vy, k*vz) */
  *x = k * vx;
  *y = k * vy;
  *z = k * vz;
}


/* Function: pointsToPlane
 **************************
 * This function takes three 3D points and
 * fills in parameters for the plane equation
 * of the form ax + by + cz = d
 * The three points are:
 * (x1, y1, z1), (x2, y2, z2), and (x3, y3, z3)
 */
void pointsToPlane (double *pa, double *pb, double *pc, double *pd,
		    double x1, double y1, double z1,
		    double x2, double y2, double z2,
		    double x3, double y3, double z3) {
  /*
   * A = |1 y1 z1| B = |x1 1 z1| C = |x1 y1 1| D = |x1 y1 z1|
   *     |1 y2 z2|     |x2 1 z2|     |x2 y2 1|     |x2 y2 z2|
   *     |1 y3 z3|     |x3 1 z3|     |x3 y3 1|     |x3 y3 z3|
   */
  *pa = determinant (1, y1, z1,
		     1, y2, z2,
		     1, y3, z3);
  
  *pb = determinant (x1, 1, z1,
		     x2, 1, z2,
		     x3, 1, z3);
  
  *pc = determinant (x1, y1, 1,
		     x2, y2, 1,
		     x3, y3, 1);
  
  *pd = determinant (x1, y1, z1,
		     x2, y2, z2,
		     x3, y3, z3);
} /* pointsToPlane */


/* Function: detetrminant
 *************************
 * This function computes the determinant of a 3 by 3 matrix
 */
double determinant (double n00, double n01, double n02,
		    double n10, double n11, double n12,
		    double n20, double n21, double n22) {
  double ret;
  ret =
    n00 * ((n11 * n22) - (n12 * n21)) -
    n01 * ((n10 * n22) - (n12 * n20)) +
    n02 * ((n10 * n21) - (n11 * n20));
  return ret;
} /* determinant */


/* Function: orderEqs
 ********************
 * This function makes sure that:
 * x1 != 0
 * y2 != 0
 * z3 != 0
 * returns 1, if successful
 * 0 otherwise
 */
int orderEqs (double *x1, double *y1, double *z1,
	      double *x2, double *y2, double *z2,
	      double *x3, double *y3, double *z3) {
  /*
   * try all the possible orders, until the right one is found
   * or generate an error
   * orders:
   * 123, 132, 213, 231, 312, 321 -- 3*2 = 6 :)
   * my order:
   * 123, 213, 231, 321, 312, 132
   */
  /* first -> 123 */
  if ( (*x1 != 0) && (*y2 != 0) && (*z3 != 0) ) {
    return 1;
  }
  
  /* swap 1 and 2 -> 213 */
  swap3Doubles(x1, y1, z1, x2, y2, z2);
  if ( (*x1 != 0) && (*y2 != 0) && (*z3 != 0) ) {
    return 1;
  }
  
  /* swap 2 and 3 -> 231 */
  swap3Doubles(x2, y2, z2, x3, y3, z3);
  if ( (*x1 != 0) && (*y2 != 0) && (*z3 != 0) ) {
    return 1;
  }
  
  /* swap 1 and 2 -> 321 */
  swap3Doubles(x1, y1, z1, x2, y2, z2);
  if ( (*x1 != 0) && (*y2 != 0) && (*z3 != 0) ) {
    return 1;
  }
  
  /* swap 2 and 3 -> 312 */
  swap3Doubles(x2, y2, z2, x3, y3, z3);
  if ( (*x1 != 0) && (*y2 != 0) && (*z3 != 0) ) {
    return 1;
  }
  
  /* swap 1 and 2 -> 132 */
  swap3Doubles(x1, y1, z1, x2, y2, z2);
  if ( (*x1 != 0) && (*y2 != 0) && (*z3 != 0) ) {
    return 1;
  }
  
  /* there is no way... */
  return 0;
}


/* Santa's Little Helper */
void swap3Doubles (double *x1, double *y1, double *z1,
		   double *x2, double *y2, double *z2) {

  double tx, ty, tz;
  tx = *x1;
  ty = *y1;
  tz = *z1;
  *x1 = *x2;
  *y1 = *y2;
  *z1 = *z2;
  *x2 = tx;
  *y2 = ty;
  *z2 = tz;

}


/* Function distToPlane
 **********************
 * Finds the distance from point (x,y,z)
 * to the plane ax + by + cz + d = 0
 */
double distToPlane (double x, double y, double z,
		    double a, double b, double c, double d) {
  double k;
  k = (d - a*x - b*y - c*z) / (a*a + b*b + c*c);
  return k;
} /* distToPlane */


/* The main method.  We do everything here.  "Here" is cool. */
int main(int argc, char *argv[]) {
 
  /* Various variables. */
  int refRowTop, refRowBottom, contrast, maxCols, maxRows, numImages, i, j;
  struct image *im;
  char **imageNames;
  FILE *output;
  double **shadowTimeTable;
  double desk_plane_epsilon;
  point **points3D;
  point light;
  calibration_constants cc;
  camera_parameters cp;
  pixel *shadowPlanesTop, *shadowPlanesBottom;

  /* Allocate the image structure. */
  im = (struct image *) malloc(sizeof(struct image));
  
  /* Parse the arguments */
  if(argc < 9) {
    usage(argv[0]);
    return 0;
  }
  
  /* Get the light calibration. */
  if( (light = readLightParams(argv[1]))==NULL) {
    fprintf(stderr,"Unable to read light position file: %s\n",argv[1]);
    return -1;
  }

  /* Get the camera calibration. */
  if(!readParams(argv[2], &cp, &cc)) {
    fprintf(stderr,"Unable to read camera calibration file: %s\n",argv[2]);
    return -1;
  }

  /* Get the contrast. */
  contrast = atoi(argv[3]);
  if(contrast<0 || contrast>255) {
    fprintf(stderr,"Bad contrast value: %d\n",contrast);
    return -1;
  }

  /* Read in the desk plane epsilon (this has no effect if it is zero) */
  desk_plane_epsilon = atof(argv[4]);

  /* Open output file. */
  if(!(output = fopen(argv[7],"w"))) {
    fprintf(stderr,"Unable to open output file %s.",argv[7]);
    return -1;
  }
  
  /* Read in the image names. */
  numImages = argc-8;
  imageNames = (char **)calloc(numImages, sizeof(char *));
  for(i=0;i<numImages;i++) {
    imageNames[i] = (char *)calloc(FILENAMELENGTH, sizeof(char));
    strcpy(imageNames[i],argv[i+8]);
  }

  /* Open the first image and get the size.  We will be needing this.
     If it turns out that the size of any image is different from this
     we will fail. */
  if(readImage(im, imageNames[0])) {
    printf("Error Reading %s!\n", imageNames[0]);
    return -1;
  }
  
  /* get number of columns and rows */
  maxCols = getNCols(im);
  maxRows = getNRows(im);

  /* Get the top reference row. */
  refRowTop = atoi(argv[5]);
  if(refRowTop<0 || refRowTop>maxRows) {
    fprintf(stderr,"Reference row is out of bounds.\n");
    return -1;
  }

  /* Get the bottom reference row. */
  refRowBottom = atoi(argv[6]);
  if(refRowTop<0 || refRowTop>maxRows) {
    fprintf(stderr,"Reference row is out of bounds.\n");
    return -1;
  }

  /* Sanity check the reference rows. */
  if(refRowTop > refRowBottom) {
    fprintf(stderr,"The top reference row should be SMALLER than the bottom reference row.\n");
    return -1;
  }

  /* Allocate the shadow plane table. */
  shadowPlanesTop = (pixel *)calloc(numImages,sizeof(pixel));
  shadowPlanesBottom = (pixel *)calloc(numImages,sizeof(pixel));

  /* Compute shadowTimes and shadowPlanes. */
  shadowTimeTable = findShadowTimes(shadowPlanesTop, shadowPlanesBottom, refRowTop, 
				    refRowBottom, imageNames,numImages,contrast);

  /* Now that we have all of the table data, we need to go through and
     compute each 3D point. */
  
  /* First we initialize the structure: */
  points3D = (point **) calloc(maxRows, sizeof(point *));
  for(i=0;i<maxRows;i++)
    points3D[i] = (point *) calloc(maxCols, sizeof(point));
  
  /* Now we go through and compute each point. */
  for(i=refRowTop;i<=refRowBottom;i++) {
    for(j=SIDECUTOFFLEFT;j<maxCols-SIDECUTOFFRIGHT;j++) {
      
      /* Temporary variables that we need in this block. */
      int prevTime, postTime;
      double time;
      double top_x, top_y, bot_x, bot_y;
      double a, b, c, d;

      /* First we need to check and see if we were able to extract
         enough information about this particular pixel to actually
         compute a point at all. */
      if(shadowTimeTable[i][j]==-1) {
	points3D[i][j] = (point) calloc(1, sizeof(struct point));
	points3D[i][j]->x = points3D[i][j]->y = points3D[i][j]->z = 0.0;
	continue;
      }

      /* If there is a time for this particular pixel, we need to find
         the exact shadow plane that corresponds to it. */
      time = shadowTimeTable[i][j];
      prevTime = (int)time;
      postTime = prevTime+1;

      /* Make sure they exist.  If not return a 0 pixel as we did
         above. */
      if(shadowPlanesTop[prevTime]==NULL || shadowPlanesTop[postTime]==NULL ||
	 shadowPlanesBottom[prevTime]==NULL || shadowPlanesBottom[postTime]==NULL) {

	points3D[i][j] = (point) calloc(1, sizeof(struct point));
	points3D[i][j]->x = points3D[i][j]->y = points3D[i][j]->z = 0.0;
	continue;

      }

      /* Interpolate the top reference point. */
      top_x = (1.0-(time-(double)prevTime))*(double)(shadowPlanesTop[prevTime]->col) + 
	((time-(double)prevTime)/((double)postTime-(double)prevTime))*
	(double)(shadowPlanesTop[postTime]->col); 
      top_y = (1.0-(time-(double)prevTime))*(double)(shadowPlanesTop[prevTime]->row) + 
	((time-(double)prevTime)/((double)postTime-(double)prevTime))*
	(double)(shadowPlanesTop[postTime]->row); 
       
      /* Interpolate the bottom reference point. */
      bot_x = (1.0-(time-(double)prevTime))*(double)(shadowPlanesBottom[prevTime]->col) + 
	((time-(double)prevTime)/((double)postTime-(double)prevTime))*
	(double)(shadowPlanesBottom[postTime]->col); 
      bot_y = (1.0-(time-(double)prevTime))*(double)(shadowPlanesBottom[prevTime]->row) + 
	((time-(double)prevTime)/((double)postTime-(double)prevTime))*
	(double)(shadowPlanesBottom[postTime]->row); 

      /* We now need to compute the shadow plane for this particular
         pixel. */
      findShadowPlane(&a, &b, &c, &d,
		      cc.r3, cc.r6, cc.r9,
		      cc.r3*cc.Tx + cc.r6*cc.Ty + cc.r9*cc.Tz, cc.f,
		      light->x, light->y, light->z,
		      cp.dpx*(top_x-cp.Cx), cp.dpy*(top_y-cp.Cy),
		      cp.dpx*(bot_x-cp.Cx), cp.dpy*(bot_y-cp.Cy));

      /* Finally we compute the point!!!!!!!!!! */
      points3D[i][j] = (point) calloc(1, sizeof(struct point));      
      pixelTo3D(&(points3D[i][j]->x), &(points3D[i][j]->y), &(points3D[i][j]->z),
		a, b, c, d,
		cp.dpx*(j-cp.Cx), cp.dpy*(i-cp.Cy), cc.f);
      
    } /* End for i. */
  } /* End for j. */

  /* Now that we've computed everything, lets dump the results to
     stdout. */
  fprintf(output,"%d\n",refRowBottom - refRowTop + 1);
  fprintf(output,"%d\n",maxCols-SIDECUTOFFLEFT-SIDECUTOFFRIGHT);
  for(i=refRowTop;i<=refRowBottom;i++)
    for(j=SIDECUTOFFLEFT;j<maxCols-SIDECUTOFFRIGHT;j++)
      if(fabs(distToPlane(points3D[i][j]->x,points3D[i][j]->y,points3D[i][j]->z,
			  cc.r3, cc.r6, cc.r9, (cc.r3*cc.Tx+cc.r6*cc.Ty+cc.r9*cc.Tz)/sqrt(cc.r3*cc.r3+cc.r6*cc.r6+cc.r9*cc.r9))) < desk_plane_epsilon)
	fprintf(output,"0 0 0 0\n");
      else
	fprintf(output,"%f %f %f 0\n",points3D[i][j]->x,points3D[i][j]->y,points3D[i][j]->z);
  
  return 0;

}
