// The People Killer <tm>
//
// Sean McVeigh 95125799 
// CS498q - Richard Mann

#include "image.h"
#include <iostream.h>
#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "lib.h"

// main()
// 
// load command line arguments (image filenames)
int main(int argc, char *argv[]) 
{
  float *image[argc-1];
  int *maxx, *maxy;

  if (argc<3) {
    cerr << "usage: disparity file1.pgm file2.pgm [file3.pgm ...]" << endl;
    exit(-1);
  }
  
  // allocate space
  maxx=new int[argc-1];
  maxy=new int[argc-1];
  
  for (int i=1;i<argc;i++) {
    cout << "loading image "<<i<<": "<<argv[i]<<"...";
    image[i-1] = read_pgm_image(argv[i],&maxx[i-1],&maxy[i-1]);
    cout << maxx[i-1] << "x" << maxy[i-1] << endl;
  }

  float theta;
  int tx,ty;
  for (int i=1;i<argc-1;i++) {
    // find image alignment parameters: rotation, translation
    cout << argv[i] <<" -> "<<argv[i+1]<<": ";
    align(image[i-1],image[i],maxx[i],maxy[i],
	  10,10,50,50,
	  (5*PI/180), 10, 
	  10, 10, 
	  theta, tx, ty);
    cout << "aligned at theta="<<theta*180/PI<<" degrees, tx="<<tx<<", ty="<<ty<<endl;
    
    // compute image disparity using block matching algorithm
    // rotate and translate image 1 to match image 2
    float *temp=make_image(maxx[i],maxy[i]);
    float *image1=make_image(maxx[i],maxy[i]);
    float *image2=copy_image(image[i],maxx[i],maxy[i]);
    rotate(image[i-1],temp,maxx[i],maxy[i],(maxx[i]/2),(maxy[i]/2),theta);
    translate(temp,image1,maxx[i],maxy[i],tx,ty);
    free_image(temp);
    // blur both images....
    smooth_image(image1,maxx[i],maxy[i],0.5);
    smooth_image(image2,maxx[i],maxy[i],0.5);
    // calculate disparity by block matching algorithm, output disparity
    // map (2d) into 2 component maps.
    int *XMAP=new int[maxx[i]*maxy[i]];
    int *YMAP=new int[maxx[i]*maxy[i]];
    cout << "computing forward disparity" <<endl;
    compute_disparity(image1,image2,XMAP,YMAP,maxx[i],maxy[i],3,3,8);

    // compute reverse disparity
    int *XXMAP=new int[maxx[i]*maxy[i]];
    int *YYMAP=new int[maxx[i]*maxy[i]];
    cout << "computing reverse disparity" <<endl;
    compute_disparity(image2, image1,XXMAP,YYMAP,maxx[i],maxy[i],3,3,8);
    
    cout << "removing inconsistencies"<<endl;
    // filter inconsistencies
    for (int y=0;y<maxy[i];y++) {
      for (int x=0;x<maxx[i];x++) {
	int xx=x+XMAP[y*maxx[i]+x];
	int yy=y+YMAP[y*maxx[i]+x];
	// inconsistency threshold = 3x3 pixel landing zone
	if (abs(y-(yy+YYMAP[yy*maxx[i]+x]))>1) {
	  XMAP[y*maxx[i]+x]=0; YMAP[y*maxx[i]+x]=0; continue;
	}
	if (abs(x-(xx+XXMAP[y*maxx[i]+xx]))>1) {
	  XMAP[y*maxx[i]+x]=0; YMAP[y*maxx[i]+x]=0; continue;
	}
      }
    }
    delete(XXMAP); delete(YYMAP);

    // filtered velocities (<2)
    int *FXMAP=new int[maxx[i]*maxy[i]];
    int *FYMAP=new int[maxx[i]*maxy[i]];
    // remove velocities < 2
    find_similar_blobs(XMAP,YMAP,FXMAP,FYMAP,maxx[i],maxy[i],
		       0,0,2,0);

    // output disparity maps
    cout << "saving disparity maps: disp-x-"<<argv[i]<<", disp-y-"<<argv[i]<<endl<<endl;
    save_disp(argv[i],FXMAP,FYMAP,maxx[i],maxy[i]);
    delete(FXMAP); delete(FYMAP);
    free_image(image1); free_image(image2); 
  }
}