#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>
#include "wrap.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  GridZlo GridZhi <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 = 255; /* Maximum pixel value (255 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 */

/* 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;
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 */



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 = xfopen(strcat(img,".intop.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,Picw,Pich,255,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 = xfopen(strcat(img,".correl.pgm"),"w"); /* and write out display */
    img[imgl] = 0;
    pgm_writepgm(fp,picout,Picw,Pich,255,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 = xfopen(strcat(nav,".smodels.pgm"),"w"); /* and write out display */
    nav[navl] = 0;
    pgm_writepgm(fp,picout,rtot,dmax,255,0);
    fclose(fp);
    pgm_freearray(picout, dmax );
    
};


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

    if (argc != 2)
      {
	  fprintf(stderr, "Usage: %s run <calib>\n", argv[0]);
	  fprintf(stderr, "run.nav is a navigation file, with pointers to camera calibration files\n");
	  fprintf(stderr, "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));

    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);
      };
    for (i = mx*my*mz-1; i >= 0; i--) map.mapm[i] = 0;  /* clear map */
};


void ProcessStereoSet()
/* Turn the images found in picL and picR into lots of rays! */
{
  
  /* 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);
    };
};


void DoOldRun()   /* Process image sequence, with given set of sensor parameters */
{
    /* 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);

	ProcessStereoSet();
      };
    
    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]);
      };
};
    


void DoNewRun()   /* Digitize and process new image sequence */
{
    npix = TtimeR = TtimeI = TtimeC = TtimeT = 0;
    /* accumulate nav file total times */
    

   while (0 /* digitize successive image pairs */) 
      {
	CamHead = 0; /* set up camera heading for this pair */
        CamPos[0] = 0; /* set up camera X position */
	CamPos[1] = 0; /* set up camera Y position */
	/* CamPos is halfway between lens centers */

	CamSep = CamSep; /* set up distance between cameras */
	/* A different sensor model is needed for different CamSep !!!! */

	sinH = SIN(CamHead);   cosH = COS(CamHead);
	
	/* Digitize raw images */

	/* Digitize left image */  /* -> picL */
	/* Digitize right image */ /* -> picR */

	ProcessStereoSet();
      };
    
    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));
      }
};


int prototype_main(int argc, char *argv[])
{
  SetUpRunFramework(argc, argv);
  /* Read nav file header, make camera calibration, pix arrays and grid */
    
  Trace=9;  /* fully log sample run */

  evO = 100;  /* ray occupied region height */
  evE = -9;   /* ray empty region depth */
  Corwtscale = 0.5;  /* Correlation peak probability weighting */

  SetUpRun(); /* Set up sensor models for current parameters, clear map */
  


  if (1) DoOldRun();  /* read in image files indicated in nav file */
  else DoNewRun();  /* digitize and process new images */

  /* write out 3D map */
  MapStatistics();

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

  exit(0);
};