#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <pbm.h>
#include <pgm.h>
#include <assert.h>
#include <cmacs.h>
#include <fisheye.h>
#include <volsense.h>

/* Test program for "fisheye" stereo 3D evidence grid routines */

gray **picL, **picR;  	/* left and right incoming pictures */
gray **recL, **recR;	/* rectified outgoing picture */
int	Pich = 600, /* Picture height, pixels (576 or 480 typical) */
        Picw = 800,  /* Picture width, pixels  (768 or 640 typical) */
        Picmax = 256; /* Maximum pixel value (256 usual) */
int intopL[(600>>3)*(800>>3)];  /* interest operator results, left image */
int intopR[(600>>3)*(800>>3)];  /* interest operator results, right image */
int top;  /* max value of interest operator */
int  joffset;  /* leftmost bound of each search, for CamClose range */
int fudge;	/* fudge for image vertical misalignments */

int correl[800], corri[800];   /* correlation result arrays */
char img[100];   int imgl;		/*  image name */


void DisplayInterest(int *intop)  /* Make interest operator display image */
{
    gray **picout;	/* display picture, with interest graphics */
    int i, j, ii, l, ll, p, intcnt;
    FILE *fp;
    
    /* Create interest display image array */
    picout = pgm_allocarray(Picw,Pich);
    for (i=0; i<Pich; i++) for (j=0; j<Picw; j++)
      picout[i][j] = recL[i][j];
    
    p = 0; intcnt = 0;
    for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
      {
	  l = (800*intop[p++]+top-1)/top; if (l>0) intcnt++; 
	  if (l<0) l=0;  if (l>8) l = 8;
	  ll = l/2;  l -= ll;
	  for (ii = 4-l; ii < 4+ll; ii++)
	    {
		picout[i+ii][j+ii] = picout[i+ii][j+ii]<128?Picmax:0;
		picout[i+ii][j+7-ii] = picout[i+ii][j+7-ii]<128?Picmax:0;
	    };
      }; 
    
    printf("Number of interest points: %d\n",intcnt);
    
    fp = fopen(strcat(img,".intop.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,Picw,Pich,256,0);
    fclose(fp);
    pgm_freearray(picout, Pich );
};


void DisplayCorrelate(int iint, int jint) 
  /* Make display image for a selected correlation */
{
    gray **picout;	/* display picture, with correlation graphics */
    int i, j, l;
    int scani, scanj, scanpic, bestj;
    int histo[Pich];
    int t, tsum, sil, sjl, sih, sjh;
    FILE *fp;
    
    /* Create correlation display image array */
    picout = pgm_allocarray(Picw,Pich);
    
    scani = iint;  scanj = jint;
    
    bestj = Correl7(recL, scani, scanj, Pich, Picw,
		    recR, sil = scani, sjl = scanj-joffset,
		    sih = scani+5, sjh = scanj,
		    correl,corri);
    
    if (sjl<4) sjl = 4;

    printf("Best vertical offset fudge corri[%d] = %d\n",
	   bestj,corri[bestj]-scani);
    
    /* copy right image into display, and mark search area */
    for (i=0; i<Pich; i++) for (j=0; j<Picw; j++)
      picout[i][j] = 
	(i==sil-5 || i==sih+4 || j==sjl-5 || j==sjh+4)?
	  (recR[i][j]<128?Picmax:0):recR[i][j];
    
    /* inset search window from left image in right image display */
    scanpic = scani<Pich/2?scani+40:scani-40;
    for (i=-32; i<32; i++) for (j=-32; j<32; j++)
      picout[scanpic+i][scanj+j] =
	(i == -32 || i == 31 || j == -32 || j == 31)?0:
	(i == -5  || i == 4  || j == -5  || j == 4)?
	  (recL[scani+i][scanj+j]<128)?Picmax:0:recL[scani+i][scanj+j];
    
    /* mark location of best match */
    for (i = scani-16; i < scani+16; i++) 
      { picout[i][bestj-5] = picout[i][bestj-5]<128?Picmax:0;
	picout[i][bestj+4] = picout[i][bestj+4]<128?Picmax:0;  };
    for (j = bestj-5; j <= bestj+4; j++)
      { picout[corri[bestj]-5][j] = picout[corri[bestj]-5][j]<128?Picmax:0;
	picout[corri[bestj]+4][j] = picout[corri[bestj]+4][j]<128?Picmax:0;  };
    
    for (i=0; i<Pich; i++) histo[i] = 0;
    for (j = sjl; j<=sjh; j++)
      {  l = (1+((Pich-2)*correl[j])) >> 22;
	 if (l<1) l = 1;  if (l>Pich-2) l = Pich-2;
	 histo[l]++;
	 picout[l][j] = Picmax;	
	 picout[l-1][j] = 0;
	 picout[l+1][j] = 0;
     };
    
    tsum = histo[0];
    for (i=0; i<Pich; i++) if (histo[i]>tsum) tsum=histo[i];
    
    for (i=0; i<Pich; i++)
      {   for (j= Picw-50; j<Picw-1; j++) picout[i][j]=0;
	  t = (histo[i]*200)/tsum;
	  for (j=Picw-t-1; j<Picw-1; j++) picout[i][j] = Picmax;
      };
    
    fp = fopen(strcat(img,".correl.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,Picw,Pich,256,0);
    fclose(fp);
    pgm_freearray(picout, Pich );
    
};


void DisplayMapSlice(Map3D map, int zplane, char* fname) 
  /* Make display image for a selected horizontal map slice */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1;
    int npos, nneg, nzer;
    FILE *fp;
    
    /* Create map slice display image array */

    printf("zplane = %d\n",zplane);

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    picout = pgm_allocarray(mx,my);

    npos = nneg = nzer = 0;
    for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
      for (iz = 0; iz < mz; iz++)
	{
	    t1 = map.mapm[(iz<<ly)+(iy<<lx)+ix];
	    if (t1<0) nneg++; else { if (t1>0) npos++; else nzer++; };
	};
    printf ("\n Map counts:  %d zero, %d pos, %d neg \n",nzer,npos,nneg);

    for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
      {
	  t = map.mapm[(zplane<<ly)+(iy<<lx)+ix];
	  picout[ix][iy] = PRB(t)>>23;
      };

    fp = fopen(strcat(img,fname),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,mx,my,256,0);
    fclose(fp);
    pgm_freearray(picout, my );
    
};
void DisplayMapSet(Map3D map) 
  /* Make display image for a selected horizontal map slice */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i;
    int npos, nneg, nzer, zplane, frame, xpos, ypos, sx, sy;
    double dx, dy;
    FILE *fp;
    
    /* Create map slice display image array */

    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;

    picout = pgm_allocarray(sx = mx*4+4, sy = my*3+4);
    for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
      picout[iy][ix] = 0;

    npos = nneg = nzer = 0;
    for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
      for (iz = 0; iz < mz; iz++)
	{
	    t1 = map.mapm[(iz<<ly)+(iy<<lx)+ix];
	    if (t1<0) nneg++; else { if (t1>0) npos++; else nzer++; };
	};
    printf ("\n Map counts:  %d zero, %d pos, %d neg \n",nzer,npos,nneg);

    for (frame = 0; frame < 12; frame ++)
      {
	  xpos = (frame % 4)*(mx+1)+1;
	  ypos = (frame / 4)*(my+1)+1;
	  zplane = mz*(frame/12.0+0.75)/map.himv[2];

	  for (iy = 0; iy < my; iy++) for (ix = 0; ix < mx; ix++)
	    {
		t = map.mapm[(zplane<<ly)+(iy<<lx)+ix];
		picout[ypos+ix][xpos+iy] = PRB(t)>>23;
	    };
      };

    for (frame = 0; frame < 12; frame ++)
      {
	  xpos = (frame % 4)*(mx+1)+1;
	  ypos = (frame / 4)*(my+1)+1;
	  dx = map.himv[0]-map.lomv[0];
	  dy = map.himv[1]-map.lomv[1];

	  for (tick=1; tick<=dx-1; tick++)
	    { ix = mx*tick/dx;
	    for (i = 0; i < 8; i++)
	      picout[ypos+ix-1][xpos+i] = 
	      picout[ypos+ix-1][xpos+mx-1-i] = 0; }

	  for (tick=1; tick<=dy-1; tick++)
	    { iy = my*tick/dy;
	    for (i = 0; i < 8; i++)
	      picout[ypos+i][xpos+iy-1] = 
	      picout[ypos+my-i-1][xpos+iy-1] = 0; }
      };

    fp = fopen(strcat(img,".slices.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,sx,sy,256,0);
    fclose(fp);
    pgm_freearray(picout, sy );
    
};


void DisplaySModels(CylSensorModelArray smodels) 
  /* Make display image for the sensor model array */
{
    gray **picout;	/* display picture, with correlation graphics */
    int i, j, rtot, dmax, rs, ds, ri, di, t, log2rs;
    FILE *fp;
    
    /* Create sensor model display image array */

    rtot = 1;  dmax = 0;
    for (i=0; i<smodels.nmodels; i++)
      {
	  rtot += smodels.models[i].rsize + 1;
	  ds = smodels.models[i].dsize;  if (ds>dmax) dmax = ds;
      };

    printf("dmax = %d, rtot = %d\n",dmax,rtot);

    picout = pgm_allocarray(rtot,dmax);
    for (i=0; i<dmax; i++) for (j=0; j<rtot; j++) picout[i][j]=0;

    rtot = 1;
    for (i=0; i<smodels.nmodels; i++)
      {
	  rs = smodels.models[i].rsize;
	  ds = smodels.models[i].dsize;
	  log2rs = ILOG2C(rs);
	  for (ri = 0; ri < rs; ++ri)
	  {
	    for (di = 0; di < ds; ++di)
	      {
		t = smodels.models[i].modelm[(di<<log2rs)+ri];
		picout[di][rtot] = PRB(t)>>23;
	      }
	    rtot++;
	  }
	  rtot++;
      };

    fp = fopen(strcat(img,".smodels.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,rtot,dmax,256,0);
    fclose(fp);
    pgm_freearray(picout, dmax );
    
};


int main(argc, argv)
int argc;
char *argv[];
{
    Calib CalL, CalR;  /* calibration parameters and pointers */

    Map3D map1;                 /* 3D evidence grid map */
    CylSensorModelArray smd;    /* Prestored sensor model array */
    MainInt msize[3];           /* 3D grid parameters, grid size */
    MainFloat lov[3],  hiv[3],  /* grid physical dimensions */
              PosL[3], PosR[3], Dir[3],   /* evidence ray position, direction */
              rng;              /* evidence ray range */

    /* Camera setup parameters, nominal position is stereo pair midpoint */ 
    MainFloat CamSep,           /* left/right camera separation, in meters */
              CamPos[3], CamHead, CamTilt,  /* camera position and orientation */
              CamFocal,         /* focal length of camera, in pixels */
              CamClose = 1;     /* minimum range to examine, in camera separations */

    MainFloat Feat[3];          /* feature position, in camera frame */
    int time1, time2;
    int i, j, p, it, intcnt, scani, scanj, iint, jint;
    gray maxval;  int rows, cols;   char calnamL[100], calnamR[100];
    int maxint, sil, sjl, sih, sjh, bestj, raycnt, eye, eye2, jposL, jposR;
    MainFloat t;
    FILE *fp;

    InitCmacs();

    if (argc < 2 || argc > 3)
    {
    printf("Usage: rayfish image <cal>\n");
    printf("image.L.pgm and image.R.pgm are a stereo pair\n");
    printf("optional cal.L.calib cal.R.calib are flatfish calibration files\n");
    printf("program locates and ranges features, and builds 3D evidence grid\n");
    exit(0);
      };

    strcpy(img,argv[1]); imgl = strlen(img);  img[imgl] = 0;

    pgm_init(&argc, argv);

    /* Read in raw images */

    fp = fopen(strcat(img,".L.pgm"),"r");  img[imgl]=0;
    if (fp == NULL) {printf("No %s file!!\n",img); exit(0); };
    picL = pgm_readpgm(fp, &cols, &rows, &maxval );
    fclose(fp);
    Picw = cols;   Pich = rows;   Picmax = maxval;
    fp = fopen(strcat(img,".R.pgm"),"r");  img[imgl]=0;
    if (fp == NULL) {printf("No %s file!!\n",img); exit(0); };
    picR = pgm_readpgm(fp, &cols, &rows, &maxval );
    fclose(fp);


    if (Picw != cols || Pich != rows || Picmax > 255)
      { printf("Picture sizes [%d,%d,%d], [%d,%d,%d] no good!!\n",
	      Pich,Picw,Picmax,rows,cols,maxval);  exit(0); };

    if (argc >= 3)
      {strcpy(calnamL,argv[2]); strcat(calnamL,".L.calib");
       strcpy(calnamR,argv[2]); strcat(calnamR,".R.calib"); }
    else { strcpy(calnamL,"Pic0326.calib"); strcpy(calnamR,"Pic0326.calib"); };

    printf(img);  printf("\n");
    printf(" %d high by %d wide, max pixel %d\n\n",Pich,Picw,Picmax);

    time1 = -clock() ;  /* time the rectification precalculation */
    if (!GoFish(calnamL, &CalL, picL))
      {printf("%s calibration file didn't work!\n",calnamL); exit(0); };
    time1 += clock() ;
    printf("L Rectification setup time: %g s\n", time1/1e6);

    time1 = -clock() ;  /* time the rectification precalculation */
    if (!GoFish(calnamR, &CalR, picR))
      {printf("%s calibration file didn't work!\n",calnamR); exit(0); };
    time1 += clock() ;
    printf("R Rectification setup time: %g s\n", time1/1000000.0);
	
    if (CalL.Ph != Pich || CalL.Pw != Picw || CalR.Ph != Pich || CalR.Pw != Picw)
      printf("Calibration size [%d,%d] [%d %d] different from Image size [%d,%d]\n",
	     CalL.Ph,CalL.Pw,CalR.Ph,CalR.Pw,Pich,Picw);

    /* Create rectified image arrays */
    recL = pgm_allocarray(Picw,Pich);
    recR = pgm_allocarray(Picw,Pich);

    /* check for array contiguity -- needed for fast access */
    for (i=1; i<Pich; i++)
      if (recL[i]-recL[i-1] != Picw || recR[i]-recR[i-1] != Picw)
	{ printf("pgm returned non-contiguous rows - can't use!\n"); exit(0); };

    /* CalL.FOV = CalR.FOV = 90;  fake FOV for timing tests */

    /* set up 3D grid, and sensor model framework */

    /* set up 3D map */
    msize[0] = 256;  lov[0] = 0.0;  hiv[0] = 5.0;   /* meters wide */
    msize[1] = 256;  lov[1] = 0.0;  hiv[1] = 5.0;   /* meters deep */
    msize[2] =  64;  lov[2] = 0.0;  hiv[2] = 2.0;   /* meters high */
    MakeMap3D(msize, lov, hiv, &map1);

    /* apply rectifications */
    time1 = -clock();
    FishTail(picL, &CalL,recL);
    FishTail(picR, &CalR,recR);
    time1 += clock();
    printf("Rectification Time %g seconds\n",time1/1e6);

    /* pgm_freearray(picL, Pich );  don't do this? - need array for future pix? */
    /* pgm_freearray(picR, Pich );  don't do this? - need array for future pix? */

    /* Apply Interest Operator to left and right images */
    time1 = -clock();
    top = Interest(recL,Pich,Picw,intopL);
    top = Interest(recR,Pich,Picw,intopR);
    time1 += clock();
    printf("L&R Image Interest Operator Time %g seconds\n",time1/1e6);
    printf("Interest top = %d\n",top);
	
    DisplayInterest(intopL);  /* Create display image for interest operator */

    CamSep = 0.3;  /* camera separation, in meters */
    CamPos[0] = (lov[0]+hiv[0])/2; 
    CamPos[1] = lov[1];
    CamPos[2] = 1.25;
    CamHead = 90*PI/180;  CamTilt = 0;
    CamClose = 2;  /* closest point to search, in camera separations */

    /* calculate correlation bounds from camera pair geometry */
    CamFocal = (Picw/2)/TAN((PI/180)*CalL.FOV/2);  /* camera focal length, in pixels */
    joffset = CamFocal/CamClose;
    printf("FOV %g, CamFocal = %g, CamClose = %g, joffset = %d\n",
	   CalL.FOV,CamFocal,CamClose,joffset);

    /* Set up sensor model */
    MakeCylModelStereo(Picw, CamFocal, CamSep,
		       (hiv[0]-lov[0])/msize[0],
			2.0*(hiv[0]-lov[0]), -2, 16, &smd);  /* make sensor model */
    TrimCylModel(smd);  /* trim away any blank edges */

    DisplaySModels(smd) ;
    /* set up parameters for ray throwing */
    PosL[0] = CamPos[0] - CamSep/2;   PosR[0] = CamPos[0] + CamSep/2;
    PosL[1] = PosR[1] = CamPos[1];    PosL[2] = PosR[2] = CamPos[2];
    raycnt = 0;
    time1 = time2 = 0;
    intcnt = 0;

    for (eye = 0; eye <= 1; eye++)  /* right eye, then left */
      {
	  joffset = (eye?-Picw:Picw)/2;
	  fudge = eye?4:-4;
	  
	  /* find features matching left interest points in right image */
	  p = 0;  iint = jint = 200;  maxint = 0;
	  for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
	    if((it=eye?intopL[p++]:intopR[p++])>0 && (eye?(j>Picw/2):(j<Picw/2))) 
	      {	
		  time1 -= clock(); /* time correlations */
		  intcnt++;
		  scani = i+4;  scanj = j+4;
		  if (it>maxint) {iint = scani;  jint = scanj; maxint = it;};
		  /* Correlate !! */
		  bestj = Correl7(eye?recL:recR, scani, scanj, Pich, Picw,
				  eye?recR:recL, sil = scani+fudge,
				  sjl = scanj+joffset,
				  sih = scani+fudge, sjh = scanj,
				  correl,corri);
		  time1 += clock(); /* time correlations */
		  
		  /* throw found feature into 3D grid */
		  
		  time2 -= clock(); /* time ray throwing */
		  /* calculate the feature position, in camera coordinates */
		  jposL = eye?scanj:bestj;   jposR = eye?bestj:scanj;
		  
		  for (eye2 = 1; eye2 >= 0; eye2--) /* throw from each eye */
		    {
			t=jposL-jposR; if(t<=0) t=1;  t /= CamSep;
			Feat[0] = ((eye2?jposL:jposR)-Picw/2)/t;  /* lateral */
			Feat[1] = CamFocal/t;        /* forward */
			Feat[2] = (Pich/2-scani)/t;  /* height */
			
			Dir[0] = Feat[0];  Dir[1] = Feat[1];  Dir[2] = Feat[2];
			rng = SQRT(Feat[0]*Feat[0]+Feat[1]*Feat[1]+Feat[2]*Feat[2]);
			
			raycnt++;
			/* Throw the Ray !! */
			if (rng>0.5 && rng<30.0)
			  AddCylReading(rng,eye2?PosL:PosR,Dir,smd,map1);
		    };
		  time2 += clock();
	      }; 
	  
      };
    printf("Correlation time %g sec, Ray Throwing time %g sec, for %d windows\n",
	   time1/1e6,time2/1e6,intcnt);
	  
    if (1) {iint = Pich/2+154;  jint = Picw/2-155; }  /* a hand-picked feature */
    /* Display correlation for selected feature */
    /* DisplayCorrelate(iint, jint) ; */

    /* Display Map slices for selected heights */
    DisplayMapSet(map1);

    exit(0);

};