// group.cc
//
// process disparity information into groups of similar velocity
#include "image.h"
#include <iostream.h>
#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "lib.h"

#define MAXGROUP  16
#define MINGROUPSIZE   (50)
// width/height ratio  (discard if less than)
#define MINGROUPRATIO  (1.5)

// main, duh
int main(int argc, char *argv[]) {
  float *image[argc-1];
  int *maxx, *maxy;

  if (argc<3) {
    cerr << "usage: group 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-1;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;
    int xsize=maxx[i-1];
    int ysize=maxy[i-1];

    // load disparity maps (already filtered out marginal motion)
    int xmap[xsize*ysize];
    int ymap[xsize*ysize];
    load_disp(argv[i],xmap,ymap);
    cout << "loading disparity maps: disp-x-"<<argv[i]<<", disp-y-"
	 <<argv[i]<<endl;

    // for group colouring later.
    int col=0; 
    while (1) {
      // hunt for prominent velocity (max is +/- 8)
      int c[17*17]; bzero(c,sizeof(int)*17*17);
      for (int j=0;j<xsize*ysize;j++) {
	if ((xmap[j]==0) && (ymap[j]==0)) continue;
	c[17*(8+ymap[j])+8+xmap[j]]++; 
      }
      int max=0; int whichx=0; int whichy=0; int prevx,prevy;
      for (int j=0;j<17*17;j++) if (c[j]>max) { 
	whichx=j%17-8; whichy=j/17-8; max=c[j]; 
      }
      if ((whichx==0) && (whichy==0)) break;  // all out of motion
      //      cout << "prominent velocity: ("<<whichx<<","<<whichy<<")"<<endl;
      // filter out everything but things within |2| of prominent velocity
      int gxmap[xsize*ysize]; 
      int gymap[xsize*ysize];
      find_similar_blobs(xmap,ymap,gxmap,gymap,xsize,ysize,whichx,whichy,1,1);
      // prepare mask
      int mask[xsize*ysize];
      int wmask[xsize*ysize];
      bzero(mask,sizeof(int)*xsize*ysize);
      
      // prepare a copy of the disparity map
      int txmap[xsize*ysize];
      int tymap[xsize*ysize];
      for (int j=0;j<xsize*ysize;j++) { txmap[j]=gxmap[j]; tymap[j]=gymap[j]; }
      
      // colour in the groups
      for (int y=0;y<ysize;y++) {
	if (col>MAXGROUP) break;
	for (int x=0;x<xsize;x++) {
	  if (col>MAXGROUP) break;
	  // recursively group pixels around (x,y) radius 1
	  int gsize=build_group(txmap,tymap,mask,xsize,ysize,x,y,1,col+1);
	  if (gsize>0) {
	    // clear out the old mask and generate working mask
	    bcopy(mask,wmask,sizeof(int)*xsize*ysize);
	    bzero(mask,sizeof(int)*xsize*ysize);
	    // build x,y pixel histograms so we can define boundaries
	    int xhist[xsize]; bzero(xhist,sizeof(int)*xsize);
	    int yhist[ysize]; bzero(yhist,sizeof(int)*ysize);
	    compute_xy_histogram(wmask,xhist,yhist,xsize,ysize);
	    int x1,y1,x2,y2,cx,cy;
	    find_bounding_rect(xhist,yhist,xsize,ysize,0.8,x1,x2,y1,y2,cx,cy);
	    // is the target an acceptable shape?
	    if ((y2-y1+1)<MINGROUPRATIO*(x2-x1+1)) continue;
	    gsize=(int)(0.8*gsize);
	    if (gsize<MINGROUPSIZE) continue;
	    
	    // acceptable!
	    
	    col++;
	    cout << "group "<<col<<": size="<<gsize<<"  velocity=("
		 <<whichx<<","<<whichy<<")"<<endl;
	    
	    char filename[120]; bzero(filename,120);
	    sprintf(filename,"group-%d-%s",col,argv[i]);
	    //	    float *imcpy=copy_image(image[i-1],xsize,ysize);
	    float *imcpy=image[i-1];
	    // draw in the bounding box
	    for (int x=x1;x<=x2;x++) imcpy[y1*xsize+x]=255;
	    for (int x=x1;x<=x2;x++) imcpy[y2*xsize+x]=255;
	    for (int y=y1;y<=y2;y++) imcpy[y*xsize+x1]=255;
	    for (int y=y1;y<=y2;y++) imcpy[y*xsize+x2]=255;
	    for (int x=x1;x<=x2;x++) imcpy[cy*xsize+x]=0;
	    for (int y=y1;y<=y2;y++) imcpy[y*xsize+cx]=0;


	    write_pgm_image(filename,imcpy,xsize,ysize,"hum");
	    //	    free_image(imcpy);
	  }
	}
      }
      // delete the processed velocities from the disparity map
      for (int j=0;j<xsize*ysize;j++) {
	xmap[j]-=gxmap[j]; ymap[j]-=gymap[j];
      }
    }
    cout << "assembled "<<col<<" groups" << endl;
  }
}