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

/* Parameter optimizing program for "fisheye" stereo 3D evidence grid routines
    Repeatedly uilds a grid map from multiple views described in a navigation file,
    with different parameters, and measures the resulting map quality with
    various measures */

/*  Navigation file RUN <expanded to "RUN.nav"> file format:

CALIBFILE    <expanded into "CALIBFILE.L.calib" and "CALIBFILE.R.calib">
CameraSeparation CameraTilt CameraZ  <in meters, degrees, meters height>
GridXlo GridXhi  GridYlo GridYhi       <grid map size, in meters>
CameraHeading1 CameraX1 CameraY1 IMAGE1 <deg,m,m, exp. IMAGE1.L.pgm IMAGE1.R.pgm>
CameraHeading2 CameraX2 CameraY2 IMAGE2
CameraHeading3 CameraX3 CameraY3 IMAGE3
...
<EOF or line beginning with non-number ends file>
*/

int Trace=9;  /* >8 write out all displays  >5 those changed in a search */

/* learning parameters */
#define nsmodels 8	   /* number of sensor models of different weights */
int  evO = 100, evE = 16;  /* ray evidence values */
double Corwtscale = 1.0;   /* Correlation probability scale in gray level steps */

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) */
unsigned Int16 *meanL, *meanR;   /* image sliding means */
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 ichoose, jchoose;
int fudge0 = -1, fudge1 = 1, fudge;  /* fudge for image vertical misalignments */

char img[100];   int imgl;		/*  image name */
char nav[100];   int navl;		/*  navigation track name */

/* colorization file parameters, for enhanced displays */

int ncl = 0; /* boxes to be colorized */
int   cllo[4][1024], clhi[4][1024];   float clcol[4][1024];
char  cllabel[1024][100];   int clcnt[1024];
/* colorized viewing parameters */
double cx=1.5, cy=-0.5, cz=0.0,    /* center of attention */
       pan =0.26, tilt=0.52,        /* view orientation of grid */
       dy=12.0, mag = 3.0;         /* viewing distance, magnification */
int colpich=480, colpicw=640;      /* color viewing picture dimensions */

int nparams = 3, bester = 1;
int workparam = 0;
double param[100], bestparam[100];  /* parameters to learn over */
double paramrng[100][3] = {{400, 100, 10}, /* evO - occupied evidence */
			     {-1, -50, 1}, /* evE - empty evidence */
			     {0.1, 2.0, 0.1} /* Corwtscale - correlation weight */
		       };

/* WORKING VARIABLES */

Calib CalL, CalR;  /* calibration parameters and pointers */

Map3D map;                 /* 3D evidence grid map */
CylSensorModelArray smd[nsmodels+1];    /* Prestored sensor model arrays */
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 */
MainInt mx, my, mz, lx, ly, ix, iy, iz;  /* Map access constants */

int time1, timeR, timeI, timeC, timeT, TtimeR, TtimeI, TtimeC, TtimeT, npix;
int i, j, k, p, intcnt, scani, scanj;
gray maxval;  int format, rows, cols;   
int sil, sjl, sih, sjh, bestj, raycnt, eye, eye2, jposL, jposR;
int ii, jj, sum, colorfile;
MainFloat t;
char calnam[200], calnamL[200], calnamR[200], str[200];
FILE *fp, *navp;
float t1, t2, t3, t4, t5, t6;
MainFloat sinH, cosH, sinT, cosT;  /* sin and cos of camera heading and tilt */
MainInt oldevO = 0, oldevE = 0; /* evidence ray memory */
MainInt topevE, topevO, topthresh, topin, topout; /* search parameters */
MainFloat topscore;
char topstr[500];


void FetchParams()  /* check out a set of learning parameters */
{
    evO = param[0];
    evE = param[1];
    Corwtscale = param[2];
};


void InitParams()  /* initialize learning parameters */
{
    int i;
    for (i = 0; i<nparams; i++)
      {
	  if (paramrng[i][0]>paramrng[i][1]) SWAP(paramrng[i][0],paramrng[i][1]);
	  if (paramrng[i][2]<0) paramrng[i][2] = -paramrng[i][2];
	  if ((paramrng[i][1]-paramrng[i][0])/paramrng[i][2] > 1000)
	    printf("parameter %d range seems too large\n",i);
	  if (paramrng[i][0]+paramrng[i][2]>paramrng[i][1])
	    paramrng[i][1] = paramrng[i][0];
	  param[i] = bestparam[i] = paramrng[i][0];
      };
    workparam = 0;
    bester = 1;
};

int NextParam()  /* increment learning parameters */
{
    int i;
    param[workparam] += paramrng[workparam][2];
    while (param[workparam] > paramrng[workparam][1])
      {
	  param[workparam] = bestparam[workparam];
	  if ((++workparam) >= nparams)
	    {
		if (!bester)
		  {
		      printf("No further improvement!\n");
		      for (i=0; i<nparams; i++) printf("p[%d]=%g ",i,bestparam[i]);
		      printf("\n"); return(0);
		  };
		bester = 0;
		workparam = 0;
	    };
	  param[workparam] = paramrng[workparam][0];
      };
    return(1);
};


void BestYetParam()  /* declare last parameters tested to be best yet */
{
    int i;
    for (i=0; i<nparams; i++) bestparam[i] = param[i];
    bester = 1;
};

int ReadColorization(char *nav, Map3D map)    
{
    FILE *fp;  int i;  char str[200];  float vl[3], vh[3], co[3];
    
    ncl = 0;   /* read colorization file */
    fp = fopen(strcat(nav,".color"),"r");
    if (fp==NULL) {printf("No %s file\n",nav);  return(0);} else
      while (fgets(str,100,fp) != NULL && str[0]!='*')
	if((str[0]>='0'&&str[0]<='9')||str[0]=='-'||str[0]=='.'||str[0]==' ')
	  {   /* read in a colored box */
	      vl[0]=vh[0]=vl[1]=vh[1]=vl[2]=vh[2]=co[0]=co[1]=co[2] = -12345;
	      sscanf(str,"%g%g%g%g%g%g%g%g%g%s\n",
		     &vl[0],&vh[0],&vl[1],&vh[1],&vl[2],&vh[2],
		     &co[0],&co[1],&co[2],cllabel[ncl]);
	      if (co[2] != -12345)
		{
		    for (i=0; i<3; i++)
		      {
			  cllo[i][ncl] = 
			    (vl[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  clhi[i][ncl] = 
			    (vh[i]-map.lomv[i])*map.msize[i]/(map.himv[i]-map.lomv[i]);
			  if(cllo[i][ncl] > clhi[i][ncl])
			    SWAP(cllo[i][ncl],clhi[i][ncl]);
			  if(cllo[i][ncl]<0)  cllo[i][ncl]=0;
			  if(clhi[i][ncl]>=map.msize[i])  clhi[i][ncl]=map.msize[i]-1;
			  if (co[i]<0) co[i]=0;  if (co[i]>9) co[i]=9;
			  clcol[i][ncl] = co[i]/9.0;
		      };
		    ncl++;
		}
	  }
	else if(str[0]=='P' || str[0]=='p') /* read in picture dimensions */
	  sscanf(&str[1],"%d%d",&colpich,&colpicw);
	else if(str[0]=='C' || str[0]=='c') /* read center of attention */
	  {
	      sscanf(&str[1],"%g%g%g",&vl[0],&vl[1],&vl[2]);
	      cx = vl[0];  cy = vl[1];  cz = vl[2];
	  }
	else if(str[0]=='A' || str[0]=='a') /* read pan and tilt angles */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      pan = vl[0]*PI/180;  tilt = vl[1]*PI/180;
	  }
	else if(str[0]=='D' || str[0]=='d') /* read distance, magnification */
	  {
	      sscanf(&str[1],"%g%g",&vl[0],&vl[1]);
	      dy = vl[0];  mag = vl[1];
	  };
    
    fclose(fp);   nav[strlen(nav)-strlen(".color")]=0;
    return(1);
};


void DisplayInterest()  /* Make interest operator display image */
{
    gray **picout;	/* display picture, with interest graphics */
    int i, j, ii, l, ll, p, intcnt, gintcnt, intchoose, inv, glimit;
    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] = (j>(Picw/2))?recL[i][j]:recR[i][j];
    
    glimit = 10000;   /* quality of feature chosen for correlation display */

    p = 0; intcnt = 0;  gintcnt = 0;
    for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
      {
	  inv = (j>(Picw/2))?intopL[p]:intopR[p];  p++;
	  if (inv>0) intcnt++;
	  if (inv>top/glimit) gintcnt++;
	  l = (800*inv+top-1)/top;  if (l<0) l=0;  if (l>8) l = 8;
	  ll = l/2;  l -= ll;
	  for (ii = 4-l; ii < 4+ll; ii++)  /* draw an X */
	    {
		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;
	    };
      }; 
    
    /* pick a random interest point for correlation display */
    intchoose = RAND*gintcnt;  ichoose = 4;  jchoose = 4;

    gintcnt = 0;  p = 0;
    for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
      {
	  inv = (j>(Picw/2))?intopL[p]:intopR[p];  p++;
	  if (inv>top/glimit) if ((gintcnt++) == intchoose)
	    { ichoose = i+4;  jchoose = j+4; }; 
      };

    for (ii = -20; ii <= 20; ii++)  /* draw a + */
      if (ii>4 || ii<-4)
      {
	  i = ichoose+ii;  j = jchoose+ii;
	  if (i<0) i=0; if (i>=Pich) i = Pich-1;
	  if (j<0) j=0; if (j>=Picw) j = Picw-1;
	  picout[i][jchoose] = picout[i][jchoose]<128?Picmax:0;
	  picout[ichoose][j] = picout[ichoose][j]<128?Picmax:0;
      };

    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() 
  /* Make display image for a selected correlation */
{
    gray **picout;	/* display picture, with correlation graphics */
    int i, j, l, eye;
    int scani, scanj, scanpic, bestj;
    int histo[Pich];
    int t, tsum, sil, sjl, sih, sjh;
    double crms[Picw];
    int correl[Picw];   /* correlation result array */

    FILE *fp;
    
    /* Create correlation display image array */
    picout = pgm_allocarray(Picw,Pich);
    
    scani = ichoose;  scanj = jchoose;

    eye = (scanj-4)>(Picw/2);
    fudge = eye?fudge1:fudge0;
    
    bestj = F2Correl7(eye?meanL:meanR, eye?meanR:meanL,
		      eye?recL:recR, scani, scanj, Pich, Picw,
		      eye?recR:recL,
		      sil = sih = scani+fudge,
		      sjl = eye?scanj-joffset:scanj,
		      sjh = eye?scanj:scanj+joffset,
		      correl);

    if (sjl<4) sjl = 4;   if (sjh>Picw-5) sjh = Picw-5;


    /* copy search 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) && j>=sjl-5 && j<=sjh+4) ?
	  ((eye?recR:recL)[i][j]<128?Picmax:0):(eye?recR:recL)[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++)
      if (scanpic+i>=0 && scanpic+i<Pich && scanj+j>=0 && scanj+j<Picw
	&& scani+i>=0 && scani+i<Pich)
      picout[scanpic+i][scanj+j] =
	(i == -32 || i == 31 || j == -32 || j == 31)?0:
	(i == -5  || i == 4  || j == -5  || j == 4)?
	  ((eye?recL:recR)[scani+i][scanj+j]<128)?
	  Picmax:0:(eye?recL:recR)[scani+i][scanj+j];
    

    /* mark location of best match */
    for (i = scani-16; i < scani+16; i++) 
      if (i>=0 && i<Pich && bestj-5>=0 && bestj+4<Picw)
      { 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++)
      if (scani-5>=0 && scani+4<Pich && j>=0 && j<Picw)
      { picout[scani-5][j] = picout[scani-5][j]<128?Picmax:0;
	picout[scani+4][j] = picout[scani+4][j]<128?Picmax:0;  };
    
    for (j = sjl; j<=sjh; j++)
	  crms[j] = SQRT(correl[j]/49.0);   /* 49 if Correl7 is used */

    for (i=0; i<Pich; i++) histo[i] = 0;
    for (j = sjl; j<=sjh; j++)
      if (correl[j]<1e8) /* compute RMS match from raw sums */
      {  l = SQRT(correl[j]/49.0) + 2;   /* 49 if Correl7 is used */
	 if (l>0 && l<Pich-1) /* and accumulate histogram */
	   {
	       histo[l]++;
	       picout[l][j] = Picmax;	
	       picout[l-1][j] = 0;
	       picout[l+1][j] = 0;
	   }
     };
    
    tsum = 1;
    for (i=0; i<Pich; i++) if (histo[i]>tsum) tsum=histo[i];
    
    for (i=0; i<Pich; i++)
      {   	  
	  t = (histo[i]*200)/tsum;
	  for (j= Picw-t-5; j<Picw-1; j++) picout[i][j]=0;
	  for (j=Picw-t-1; j<Picw-1; j++) picout[i][j] = Picmax;
      };

    {  /* consolidate correlation curve, produce probability profile */
	double wt[Picw]; /* Correlartion normalized probabilities */
	int ppos[Picw], k, j, i, nwt;
	
	nwt = CorrelHills(Corwtscale, correl, sjl, sjh, ppos, wt);
	for (j = sjl; j<=sjh; j++)
	  picout[Pich-100/nsmodels][j] = (j>>2)&1?Picmax/4:3*Picmax/4;

	k=-1; while((j=ppos[++k])>=0)
	  for (i = Pich-1-wt[k]*100; i < Pich; i++)
	    {picout[i][j] = Picmax; /* wt[k]<1.0/nsmodels?Picmax/2:Picmax; */
	     picout[i][j-1]=picout[i][j+1]=0; };
    };

    
    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 DisplaySModels(CylSensorModelArray smodels, int modeln) 
  /* 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++;
      };

    nav[navl]='.'; nav[navl+1]='A'+modeln; nav[navl+2]=0;
    fp = fopen(strcat(nav,".smodels.pgm"),"w"); /* and write out display */
    nav[navl] = 0;
    pgm_writepgm(fp,picout,rtot,dmax,256,0);
    fclose(fp);
    pgm_freearray(picout, dmax );
    
};


void SetUpRunFramework(int argc, char *argv[])
{
    InitCmacs();

    if (argc != 2)
      {
	  printf("Usage: flayfish run <calib>\n");
	  printf("run.nav is a navigation file, with pointers to camera calibration files\n");
	  printf("and a sequence of camera positions and stereo pair file names\n");
	  exit(0);
      };

    strcpy(nav,argv[1]); navl = strlen(nav);  nav[navl] = 0;
    pgm_init(&argc, argv);
    ppm_init(&argc, argv);

    /* Open navigation file */
    if ((navp = fopen(strcat(nav,".nav"),"r")) == NULL)
      {printf("No %s file!!\n",nav); exit(0);};
    nav[navl] = 0;

    /* Read calibration files */
    fscanf(navp,"%s\n",calnam);
    strcpy(calnamL,calnam); strcat(calnamL,".L.calib");
    strcpy(calnamR,calnam); strcat(calnamR,".R.calib");
    if (!FishHead(calnamL, &CalL) || !FishHead(calnamR, &CalR))
      {printf("%s %s calibration files didn't work!\n",calnamL,calnamR); exit(0);};
    if ((Picw=CalL.Pw) != CalR.Pw || (Pich=CalL.Ph) != CalR.Ph)
      { printf("L&R calibration sizes [%d,%d], [%d,%d] not equal!!\n",
	      CalL.Ph,CalL.Pw,CalR.Ph,CalR.Pw);  exit(0); };
 
    /* allocate array for low pass filter for correlation */
    meanL = (unsigned Int16 *) malloc(Pich*Picw*sizeof(unsigned Int16));
    meanR = (unsigned Int16 *) malloc(Pich*Picw*sizeof(unsigned Int16));

    /* Create source and rectified image arrays */
    picL = pgm_allocarray(Picw,Pich);
    picR = pgm_allocarray(Picw,Pich);
    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); };

    /* Build rectification tables */
    time1 = -clock() ;  /* time the rectification precalculation */
    if (!FishBody(&CalL,picL) || !FishBody(&CalR,picR))
      {printf("No room for rectification tables!\n"); exit(0); };
    time1 += clock() ;
    printf("L+R Rectification setup time: %g s\n", time1/1e6);
	  
    /* set up camera parameters and sensor model */
    t1 = 0.3;   /* camera separation, in meters */
    t2 = 0;     /* camera downward tilt, in degrees */
    t3 = 1.25;  /* camera height, in meters */
    fscanf(navp,"%g%g%g\n",&t1,&t2,&t3);
    CamSep = t1;    CamTilt = t2*PI/180;    CamPos[2] = t3;
    sinT = SIN(CamTilt);   cosT = COS(CamTilt);

    CamClose = 5;  /* 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);

    t1 = -4;  t2 = 4;  t3 = -4;  t4 = 4;  t5 = 0;  t6 = 2;
    fscanf(navp,"%g%g%g%g%g%g\n",&t1,&t2,&t3,&t4,&t5,&t6);

    fclose(navp);  /* close nav file for now */

    /* set up 3D map */
    msize[0] = 256;  lov[0] = t1;  hiv[0] = t2;   /* meters wide */
    msize[1] = 256;  lov[1] = t3;  hiv[1] = t4;   /* meters deep */
    msize[2] = 64;   lov[2] = t5;  hiv[2] = t6;   /* meters high */
    MakeMap3D(msize, lov, hiv, &map);
    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];
    ly = ILOG2C(my) + (lx = ILOG2C(mx));

    /* read colorization file, for possible diagnostic displays */
    if (!(colorfile = ReadColorization(nav,map))) 
      printf("No colorization file!\n");

    printf("\nMap[%.3g:%.3g,%.3g:%.3g,%.3g:%.3g]\n",
	  lov[0],hiv[0],lov[1],hiv[1],lov[2],hiv[2]);

};


void SetUpRun()  /* Set up sensor models for current parameters */
{   
    if (oldevO != evO  || oldevE != evE)  /* Sensor Models need to be computed? */
      {
	  if (oldevO != 0 || oldevE != 0) /* Release old ones, if any */ 
	    for (i=0; i<nsmodels+1; i++) FreeCylModel(smd[i]);

	  for (i=0; i<nsmodels+1; i++) 	  /* Set up new sensor models */
	    {
		MakeCylModelStereo(Picw, CamFocal, CamSep,
				   (hiv[0]-lov[0])/msize[0],
				   2.0*(hiv[0]-lov[0]),
				   (i*evE)/nsmodels,(i*evO)/nsmodels, &smd[i]);
		TrimCylModel(smd[i]);    /* trim away any blank edges */
	    };
	  oldevE = evE;  oldevO = evO;
	  
	  if(Trace>5) 	  /* Make display picture of sensor models */
	    for (i=0; i<nsmodels+1; i++) DisplaySModels(smd[i],i);
      };
};


void DoRun()   /* Process image sequence, with given set of sensor parameters */
{
    for (i = mx*my*mz-1; i >= 0; i--) map.mapm[i] = 0;  /* clear map */
     
    /* open navigation file */
    if ((navp = fopen(strcat(nav,".nav"),"r")) == NULL)
      {printf("No %s file!!\n",nav); exit(0);};  nav[navl] = 0;
    fscanf(navp,"%s\n",calnam);  /* skip over setup info */
    fscanf(navp,"%g%g%g\n",&t1,&t2,&t3);
    fscanf(navp,"%g%g%g%g%g%g\n",&t1,&t2,&t3,&t4,&t5,&t6);
    
    npix = TtimeR = TtimeI = TtimeC = TtimeT = 0;   /* accumulate nav file total times */
    
    while (fgets(str,100,navp) != NULL &&
	   ((str[0]>='0' && str[0]<='9') || str[0]=='-' || str[0]=='.'))
      /* Run through nav file */
      {
	  sscanf(str,"%g%g%g %s\n",&t1,&t2,&t3,img);
	  CamHead = t1*PI/180;  CamPos[0] = t2;  CamPos[1] = t3;
	  imgl = strlen(img);
	  if (Trace>8)
	    printf("\nPan, Tilt %.3g %.3g deg, Position %.3g %.3g %.3g m, image %s\n",
		   CamHead*180/PI,CamTilt*180/PI,CamPos[0],CamPos[1],CamPos[2],img);
	  
	  sinH = SIN(CamHead);   cosH = COS(CamHead);
	  
	  /* 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); };
	  pgm_readpgminit(fp, &cols, &rows, &maxval, &format );
	  Picw = cols;   Pich = rows;   Picmax = maxval;
	  if (CalL.Ph != Pich || CalL.Pw != Picw)
	    {printf("Calibration size [%d,%d] not equal Image size [%d,%d]!\n",
		    CalL.Ph,CalL.Pw,Pich,Picw);    exit(0); };
	  for (i = 0; i < rows; i++)
	    pgm_readpgmrow(fp,picL[i],cols,maxval,format);
	  fclose(fp);
	  
	  fp = fopen(strcat(img,".R.pgm"),"r");  img[imgl]=0;
	  if (fp == NULL) {printf("No %s file!!\n",img); exit(0); };
	  pgm_readpgminit(fp, &cols, &rows, &maxval, &format );
	  Picw = cols;   Pich = rows;   Picmax = maxval;
	  if (CalL.Ph != Pich || CalL.Pw != Picw)
	    {printf("Calibration size [%d,%d] not equal Image size [%d,%d]!\n",
		    CalL.Ph,CalL.Pw,Pich,Picw);    exit(0); };
	  for (i = 0; i < rows; i++)
	    pgm_readpgmrow(fp,picR[i],cols,maxval,format);
	  fclose(fp);
	  
	  /* apply rectifications */
	  timeR = -clock();
	  FishTail(picL, &CalL,recL);
	  FishTail(picR, &CalR,recR);
	  timeR += clock();
	  
	  /* apply Interest Operator to left and right images */
	  timeI = -clock();
	  top = HInterest(recL,Pich,Picw,intopL);
	  top = HInterest(recR,Pich,Picw,intopR);
	  timeI += clock();
	  
	  /* compute correlation window means (slow, simple now - sliding later) */
	  for (i = 3;  i < Pich-3; i++) for (j = 3; j < Picw-3; j++)
	    { 
		sum = 0;
		for (ii = -3; ii <= 3; ii++) for (jj = -3; jj <=3; jj++)
		  sum += recL[i+ii][j+jj];
		meanL[k = i*Picw+j] = sum;
		
		sum = 0;
		for (ii = -3; ii <= 3; ii++) for (jj = -3; jj <=3; jj++)
		  sum += recR[i+ii][j+jj];
		meanR[k] = sum;
	    };
	  
	  if(Trace>8)
	    { 
		DisplayInterest();  /* Make interest operator display image */
		DisplayCorrelate(); /* And display one correlation */
	    };
	  
	  /* set up positions for ray throwing */
	  PosL[0] = CamPos[0] - sinH*CamSep/2;
	  PosL[1] = CamPos[1] + cosH*CamSep/2;
	  PosL[2] = CamPos[2];
	  
	  PosR[0] = CamPos[0] + sinH*CamSep/2;
	  PosR[1] = CamPos[1] - cosH*CamSep/2;
	  PosR[2] = CamPos[2];
	  
	  raycnt = 0;
	  timeC = timeT = 0;
	  intcnt = 0;
	  
	  for (eye = 0; eye <= 1; eye++)  /* right eye, then left */
	    {
		int correl[Picw];   /* correlation result array */
		double wt[Picw]; /* Correlartion normalized probabilities */
		int ppos[Picw];  /* Correlation peak positions */
		int peak, evwt;
		
		fudge = eye?fudge1:fudge0;
		
		/* find features matching left interest points in right image */
		p = 0;
		for (i = 0; i < Pich-7; i += 8) for (j = 0; j < Picw-7; j += 8)	
		  if((eye?intopL[p++]:intopR[p++]) > 0
		     && (eye?(j>Picw/2):(j<Picw/2))) 
		    {	
			
			timeC -= clock(); /* time correlations */
			intcnt++;
			scani = i+4;  scanj = j+4;
			/* Correlate !! */
			bestj = 
			  F2Correl7(eye?meanL:meanR, eye?meanR:meanL,
				    eye?recL:recR, scani, scanj, Pich, Picw,
				    eye?recR:recL,
				    sil = sih = scani+fudge,
				    sjl = eye?scanj-joffset:scanj,
				    sjh = eye?scanj:scanj+joffset,
				    correl);
			timeC += clock(); /* time correlations */
			
			/* throw found feature into 3D grid */
			timeT -= clock(); /* time ray throwing */
			
			/* weight and sort correlations */
			CorrelHills(Corwtscale, correl, sjl, sjh,ppos, wt);
			
			peak = -1;  /* and throw rays for the better ones */
			while ((evwt = wt[++peak]*nsmodels+0.5)>0 && peak<5)
			  {	
			      bestj = ppos[peak];
			      
			      jposL = eye?scanj:bestj;   jposR = eye?bestj:scanj;
			      
			      for (eye2 = 1; eye2 >= 0; eye2--) /* throw from each eye */
				{
				    /* feature position, in camera coordinates */
				    t=jposL-jposR; if(t<=0) t=1;  t /= CamSep;
				    Feat[0] = CamFocal/t;        /* forward */
				    Feat[1] = ((eye2?jposL:jposR)-Picw/2)/t;  /* lateral */
				    Feat[2] = (Pich/2-scani)/t;  /* height */
				    
				    /* ray throwing direction, in map coordinates */
				    t = (Feat[0]*cosT+Feat[2]*sinT);
				    Dir[0]= t*cosH+Feat[1]*sinH;
				    Dir[1]= t*sinH-Feat[1]*cosH;
				    Dir[2]= Feat[2]*cosT - Feat[0]*sinT;
				    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[evwt],map);
				};
			  };
			timeT += clock();
		    }; 
		
	    };
	  npix++;
	  TtimeR += timeR; TtimeI += timeI; TtimeC += timeC; TtimeT += timeT;
	  if (Trace>8)
	    {
		printf("            Timing for %d features(Seconds)\n",intcnt);
		printf("Rectification   Interest Op     Correlation     Ray Throw\n");
		printf("%8.3g%16.3g%16.3g%16.3g\n",
		       timeR/1e6,timeI/1e6,timeC/1e6,timeT/1e6);
	    };
      };
    fclose(navp);  /* close nav file */

    if (Trace>8)   /* report summary timings */
      {
	  printf("\n            Average over %d stereo pairs (Seconds)\n",npix);
	  printf("Rectification   Interest Op     Correlation     Ray Throw\n");
	  printf("%8.3g%16.3g%16.3g%16.3g\n",
		 TtimeR/(npix*1e6),TtimeI/(npix*1e6),TtimeC/(npix*1e6),TtimeT/(npix*1e6));
      };

};

    
void MapStatistics()    
{
    /* compute map statistics */
    if (Trace>8)
      {	/* grid and floor cell counts */
	  int npos, nneg, nzer, nposZ[map.msize[2]];
	  
	  npos = nneg = nzer = 0;  for (iz = 0; iz < mz; iz++) nposZ[iz] = 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++; };
		  if (t1>0) nposZ[iz]++;
	      };
	  printf ("\n Map counts (+ -):  %d %d, (z0 z1 z2): %d %d %d = %d\n",
		  npos, nneg, nposZ[0],nposZ[1],nposZ[2], nposZ[0]+nposZ[1]+nposZ[2]);
      };
};
    




/* parameters    0 evO   1 evE   2 Corwtscale             */

int main(int argc, char *argv[])
/* 3D stereo grid parameter evaluating program */
{
    SetUpRunFramework(argc, argv);
    
    Trace=9;  /* fully log sample run */
    evO = 100;
    evE = -9;
    Corwtscale = 0.5;

    SetUpRun();
    DoRun(); 


    /* write out 3D map */
    MapStatistics();
    sprintf(topstr,"Cwt %4.2g Ev(%d %d)",Corwtscale, evO, evE);
    printf("%s\n",topstr);

    WriteMap3Db(map,"DB Room", topstr, strcat(nav,".map3D"));  nav[navl] = 0;


#if (0)    
    /* Now start learning sequence */
    Trace=0; /* don't trace learning sequence */

    topscore = -1e7; 
    topevE = topevO = topthresh = topin = topout = 0;
    
    InitParams();  /* initialize learning parameters */

    do  /* optimize each parameter in turn */
      {
	  FetchParams();

	  SetUpRun();
	  DoRun(); 
	  
	  score = MapScore(map);
	  if (score>topscore) 
	    {
		/* write out the parameters for this best map so far */
		BestYetParam();
		MapStatistics();
		WriteMap3Db(map,"DB Room", topstr, strcat(nav,".lrn.map3D"));
		nav[navl] = 0;
	    };
      }
    while (NextParam());

#endif

    exit(0);
};