/** merge.c -- a program to merge two point clouds togethor into a
 *             single point cloud
 *
 * Paul Blaer <psb15@cs.columbia.edu>
 *
 * Compilation:  make
 *
 * Usage: merge <cloud1> <cloud2> <outcloud>
 *
 * This program takes two point clouds in pts format and merges them
 * into a single point cloud.  Both clouds have to have the same grid
 * structure.  If a point is listed as (0, 0, 0) we take that to mean
 * that there was no data at that point (which is a good assumption
 * since the optical center of the imaging system is at (0,0,0)).  We
 * assume that the first point cloud has "better" data than the
 * second, so if the first cloud has a valid point at a given grid
 * position, it is printed into the output cloud.  If the first cloud
 * has no data at a given point we fall back to the second cloud's
 * data.
 *
 **/

#include <stdio.h>
#include <stdlib.h>

/* The point structure. */
typedef struct {
  double x, y, z;
} point;

/* The main method. */
int main(int argc, char *argv[]) {

  FILE *fp1, *fp2, *out;
  int i, row1, col1, row2, col2, size;
  double junk;
  point in1, in2;

  /* Check that the arguments are correct. */
  if(argc!=4) {
    fprintf(stderr,"Usage:  %s <cloud1> <cloud2> <outcloud>\n",argv[0]);
    return -1;
  }

  /* Open the various files involved. */
  if(!(fp1 = fopen(argv[1],"r"))) {
    fprintf(stderr,"Unable to open cloud 1\n");
    return -1;
  }

  if(!(fp2 = fopen(argv[2],"r"))) {
    fprintf(stderr,"Unable to open cloud 2\n");
    return -1;
  }
  
  if(!(out = fopen(argv[3],"w"))) {
    fprintf(stderr,"Unable to open the output cloud\n");
    return -1;
  }

  /* Read in and verify the grid size. */
  fscanf(fp1,"%d %d", &row1, &col1);
  fscanf(fp2,"%d %d", &row2, &col2);

  if(row1!=row2 || col1!=col2) {
    fprintf(stderr,"The grid size is different between the two clouds\n");
    return -1;
  }

  /* Output the grid size. */
  fprintf(out,"%d\n%d\n",row1,col1);

  /* Compute the overall grid size. */
  size = row1 * col1;

  /* Run through the grid of points, and pick which data set we are
     going to use for each one.  Then output the selected point. */
  for(i=0;i<size;i++) {
    fscanf(fp1,"%lf %lf %lf %lf",&in1.x,&in1.y,&in1.z,&junk);
    fscanf(fp2,"%lf %lf %lf %lf",&in2.x,&in2.y,&in2.z,&junk);
    if( in1.x==0 && in1.y==0 && in1.z==0) {
      if( in2.x==0 && in2.y==0 && in2.z==0)
	fprintf(out,"0 0 0 0\n");
      else
	fprintf(out,"%f %f %f 0\n",in2.x,in2.y,in2.z);
    } else {
      fprintf(out,"%f %f %f 0\n",in1.x,in1.y,in1.z);
    }
  }
  

  /* We're done. */
  return 0;

}
