#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>

/* Program for displaying 3D grid maps, especially from the
   "crayfish" stereo grid program */

/* Navigation list parameters */
#define MAXNAV  500	/* max number of stereo views remembered */
int Navnum = 0;   /* count of actual views */
double NavCamHead[MAXNAV], NavCamTilt[MAXNAV], NavCamPos[MAXNAV][3];
/* view positions */
char Navimg[MAXNAV][100];  /* view image names */


char nav[200];  int navl;  /* name of (navigation) map file */
char col[200];  int coll;  /* name of colorization file */
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 */


Calib CalL, CalR;  /* calibration parameters and pointers */
char calnam[200], calnamL[200], calnamR[200], img[200];
int imgl;
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) */

Map3D map;                 /* 3D evidence grid map */
char title[200], footer[200], str[1000];
FILE *fp;
float  vl[3],vh[3],co[3];


/* Camera setup parameters, nominal position is stereo pair midpoint */ 
MainFloat CamSep,           /* left/right camera separation, in meters */
          CamPos[3], CamHead, CamTilt,  /* camera center position and orientation */
          CamFocal,         /* focal length of camera, in pixels */
          PosL[3], PosR[3]; /* left and right camera positions */
MainFloat sinH, cosH, sinT, cosT;  /* sin and cos of camera heading and tilt */


float t1, t2, t3, t4, t5, t6;  /* temporary floating variables, for scanf */

void DisplayMapColorized(Map3D map, int flabel) 
  /* Make 3D map view, coloring marked sites
     if flabel is nonzero, append its number, + or -, to file name */
{
    pixel **picout;	/* display picture, with correlation graphics */
    double ct, st, sp, cp, x, y, z, xr, yr, zr;
    double r, g, b;
    double d, v;
    int ix, iy, iz, mx, my, mz, lx, ly, sx, sy, ip, jp;
    int cl;
    FILE *fp;
    
    /* Create map view image */

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

    cp = COS(pan);   sp = SIN(pan);
    ct = COS(tilt);  st = SIN(tilt);

    for (cl=0; cl<ncl; cl++) clcnt[cl] = 0;   /* zero cell counts in region */

    picout = ppm_allocarray(sx = colpicw, sy = colpich);
    for (ip = 0; ip < sy; ip++) for (jp = 0; jp < sx; jp++)
      PPM_ASSIGN(picout[ip][jp],0,0,0);

    for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
      for (ix = 0; ix < mx; ix++)
	if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 0)
	{

	    xr = (ix+0.5)*(map.himv[0]-map.lomv[0])/mx + map.lomv[0];
	    yr = (iy+0.5)*(map.himv[1]-map.lomv[1])/my + map.lomv[1];
	    zr = (iz+0.5)*(map.himv[2]-map.lomv[2])/mz + map.lomv[2];

	      {
		  xr -= (map.lomv[0]+map.himv[0])/2;
		  yr -= (map.lomv[1]+map.himv[1])/2;
		  zr -= (map.lomv[2]+map.himv[2])/2;
		  
		  x = xr*cp - yr*sp;  y = yr*cp + xr*sp;  xr = x;  yr = y;
		  
		  y = yr*ct + zr*st;  z = zr*ct - yr*st;  x = xr;

		  x -= cx;   y -= cy;   z -= cz;

		  y = dy - y;

		  d = SQRT(x*x+y*y+z*z);   v = 0.25 + 1.0/(1+d/(dy*4));
		  
		  ip = mag*sy*z/y + sy/2;  jp = mag*sy*x/y + sx/2;
		  jp = sx-1 - jp;  ip = sy-1 - ip;

		  if(ip>=0 && ip<sy && jp >=0 && jp<sx)
		    {
			r = g = b = v*0.75;
			for (cl = 0; cl < ncl; cl++)
			  if (ix>=cllo[0][cl] && ix<=clhi[0][cl] &&
			      iy>=cllo[1][cl] && iy<=clhi[1][cl] &&
			      iz>=cllo[2][cl] && iz<=clhi[2][cl] )
			    {
				clcnt[cl]++;
				r = clcol[0][cl]*v;
				g = clcol[1][cl]*v;
				b = clcol[2][cl]*v;
			    };
			if (r<0) r = 0;  if (r>1) r = 1;
			if (g<0) g = 0;  if (g>1) g = 1;
			if (b<0) b = 0;  if (b>1) b = 1;
			PPM_ASSIGN(picout[ip][jp],(int) 255*r,(int) 255*g, (int) 255*b);
		    };
	      };
	};
    /* and write out display */
    
    if (flabel>0) sprintf(col,"%s.colorize.%d%d%d.ppm",col,
			 flabel/100,(flabel/10)%10,flabel%10);
    else if ((flabel = -flabel)>0) sprintf(col,"%s.colorize-%d%d%d.ppm",col,
			 flabel/100,(flabel/10)%10,flabel%10);
    else strcat(col,".colorize.000.ppm");

    printf("Writing %s\n",col);
    fp = fopen(col,"w");    col[coll] = 0;
    if (!fp) printf("FAILED!\n");
    ppm_writeppm(fp,picout,sx,sy,256,1);
    fclose(fp);

    ppm_freearray(picout, sy );
};

void DisplayMapSetZ(Map3D map, int DisHigh, int DisWide) 
  /* Make display images for horizontal map slices */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int zplane, frame, xpos, ypos, sx, sy;
    int horwad, verwad, allwad;
    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;

    horwad = (DisWide)/my;  verwad = (DisHigh)/mx;  allwad = horwad*verwad;

    picout = pgm_allocarray(sx = (mx+1)*horwad+1, sy = (my+1)*verwad+1);

    for (wad = 0; wad < mz/allwad;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(my+1)+1;
		zplane = wad*allwad+frame;
		
		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 < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(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; }
	    };

	  strcat(nav,".Zslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,1);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};

void DisplayMapSetY(Map3D map, int DisHigh, int DisWide) 
  /* Make display images for vertical map slices along Y direction */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int yplane, frame, xpos, ypos, sx, sy;
    int horwad, verwad, allwad;
    double dx, dz;
    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;

    horwad = (DisWide)/mx;  verwad = (DisHigh)/mz;  allwad = horwad*verwad;

    picout = pgm_allocarray(sx = (mx+1)*horwad+1, sy = (mz+1)*verwad+1);

    for (wad = 0; wad < my/allwad;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		yplane = wad*allwad+frame;
		
		for (iz = 0; iz < mz; iz++) for (ix = 0; ix < mx; ix++)
		  {
		      t = map.mapm[(iz<<ly)+(yplane<<lx)+ix];
		      picout[ypos+mz-1-iz][xpos+mx-1-ix] = PRB(t)>>23;
		  };
	    };

	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		dx = map.himv[0]-map.lomv[0];
		dz = map.himv[2]-map.lomv[2];
		
		for (tick=1; tick<=dx-1; tick++)
		  { ix = mx*tick/dx;
		    for (i = 0; i < 4; i++)
		      picout[ypos+i][xpos+ix-1] = 
			picout[ypos+mz-i-1][xpos+ix-1] = 0; }
		
		for (tick=1; tick<=dz-1; tick++)
		  { iz = mz-1-mz*tick/dz;
		    for (i = 0; i < 8; i++)
		      picout[ypos+iz-1][xpos+i] = 
			picout[ypos+iz-1][xpos+mx-i-1] = 0; }
	    };

	  strcat(nav,".Yslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,1);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};

void DisplayMapSetX(Map3D map, int DisHigh, int DisWide) 
  /* Make display images for vertical map slices along Y direction */
{
    gray **picout;	/* display picture, with correlation graphics */
    int ix, iy, iz, mx, my, mz, lx, ly, t, t1, tick, i, wad;
    int xplane, frame, xpos, ypos, sx, sy;
    int horwad, verwad, allwad;
    double dy, dz;
    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;

    horwad = (DisWide)/my;  verwad = (DisHigh)/mz;  allwad = horwad*verwad;

    picout = pgm_allocarray(sx = (my+1)*horwad+1, sy = (mz+1)*verwad+1);

    for (wad = 0; wad < my/allwad;  wad++)
      {
	  for (ix = 0; ix < sx; ix++)  for (iy = 0; iy < sy; iy++)
	    picout[iy][ix] = 0;
	  
	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(mx+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		xplane = wad*allwad+frame;
		
		for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
		  {
		      t = map.mapm[(iz<<ly)+(iy<<lx)+xplane];
		      picout[ypos+mz-1-iz][xpos+iy] = PRB(t)>>23;
		  };
	    };

	  for (frame = 0; frame < allwad; frame ++)
	    {
		xpos = (frame % horwad)*(my+1)+1;
		ypos = (frame / horwad)*(mz+1)+1;
		dy = map.himv[1]-map.lomv[1];
		dz = map.himv[2]-map.lomv[2];
		
		for (tick=1; tick<=dy-1; tick++)
		  { iy = my*tick/dy;
		    for (i = 0; i < 4; i++)
		      picout[ypos+i][xpos+iy-1] = 
			picout[ypos+mz-i-1][xpos+iy-1] = 0; }
		
		for (tick=1; tick<=dz-1; tick++)
		  { iz = mz-1-mz*tick/dz;
		    for (i = 0; i < 8; i++)
		      picout[ypos+iz-1][xpos+i] = 
			picout[ypos+iz-1][xpos+mx-i-1] = 0; }
	    };

	  strcat(nav,".Xslices");  t1 = strlen(nav);
	  nav[t1] = '0'+wad;  nav[t1+1] = 0;
	  
	  fp = fopen(strcat(nav,".pgm"),"w"); /* and write out display */
	  nav[navl] = 0;
	  pgm_writepgm(fp,picout,sx,sy,256,1);
	  fclose(fp);
      };
    pgm_freearray(picout, sy );
};


void DisplayInventorMap(Map3D map, int flabel)
{
    /* Write out a .iv file for SGI ivview display */
    int ix, iy, iz, mx, my, mz, lx, ly, thiscol, docol, cl; 
    FILE *fp;
    
    if (flabel>0) sprintf(col,"%s.%d%d%d.iv",col,
			 flabel/100,(flabel/10)%10,flabel%10);
    else if ((flabel = -flabel)>0) sprintf(col,"%s-%d%d%d.iv",col,
			 flabel/100,(flabel/10)%10,flabel%10);
    else strcat(col,".000.iv");
    fp = fopen(col,"w");    col[coll] = 0;

    fprintf(fp,"#Inventor V2.1 ascii\n\n");
    mx = map.msize[0];   my = map.msize[1];   mz = map.msize[2];           
    lx = ILOG2C(mx);
    ly = ILOG2C(my)+lx;
    
    if (ncl==0)  /* No colors at all! */
      {
	  fprintf(fp,"Separator {\n    Info {\n	string	\"Object Name: World\"\n");
	  fprintf(fp,"	}\n	Material { diffuseColor %.3g %.3g %.3g }\n",
		  1.0, 1.0, 1.0);
	  fprintf(fp,"	Coordinate3 {\n	point	[\n");
	  
	  for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
	    for (ix = 0; ix < mx; ix++)
	      if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 0)
		{
		    fprintf(fp,"		%.4g %.4g %.4g,\n",
			    (ix+.5)*(map.himv[0]-map.lomv[0])/mx+map.lomv[0],
			    (iy+.5)*(map.himv[1]-map.lomv[1])/my+map.lomv[1],
			    (iz+.5)*(map.himv[2]-map.lomv[2])/mz+map.lomv[2]);
		};
	  fprintf(fp,"		0 0 0\n");
	  fprintf(fp,"	]\n    }\n    PointSet {\n	startIndex  0 \n	numPoints   -1\n    }\n}\n\n");
      }
    else
    for (docol = 1; docol<ncl; docol++)
      {
	  fprintf(fp,"Separator {\n    Info {\n	string	\"Object Name: %s\"\n",
		  cllabel[docol]);
	  fprintf(fp,"	}\n	Material { diffuseColor %.3g %.3g %.3g }\n",
		  clcol[0][docol], clcol[1][docol], clcol[2][docol]);
	  fprintf(fp,"	Coordinate3 {\n	point	[\n");
	  
	  for (iz = 0; iz < mz; iz++) for (iy = 0; iy < my; iy++)
	    for (ix = 0; ix < mx; ix++)
	      if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 0)
		{
		    thiscol = 0;
		    for (cl = 0; cl < ncl; cl++)
		      if (ix>=cllo[0][cl] && ix<=clhi[0][cl] &&
			  iy>=cllo[1][cl] && iy<=clhi[1][cl] &&
			  iz>=cllo[2][cl] && iz<=clhi[2][cl] )
			thiscol = cl;
		    if (thiscol == docol) 
		      fprintf(fp,"		%.4g %.4g %.4g,\n",
			      (ix+.5)*(map.himv[0]-map.lomv[0])/mx+map.lomv[0],
			      (iy+.5)*(map.himv[1]-map.lomv[1])/my+map.lomv[1],
			      (iz+.5)*(map.himv[2]-map.lomv[2])/mz+map.lomv[2]);
		};
	  fprintf(fp,"		0 0 0\n");
	  fprintf(fp,"	]\n    }\n    PointSet {\n	startIndex  0 \n	numPoints   -1\n    }\n}\n\n");
      };
    fclose(fp);
};


void DisplayMapFiltered(int nfilter)
    {  /* create filtered map displays */
    Map3D Counts;
    MainInt mx, my, mz, lx, ly, lz;  /* Map access constants */
    MainInt loc, hic, msize, psize, rsize;
    MainInt i, iter, magc, magm;
    Int16  *mmapm;				/* map array */
    Int16  *countm;
    
    MakeMap3D(map.msize, map.lomv, map.himv, &Counts);

    mmapm = map.mapm;
    countm = Counts.mapm;
    mx = map.msize[0];  my = map.msize[1];  mz = map.msize[2];
    lx = ILOG2C(mx);   ly = ILOG2C(my)+lx;   lz = ILOG2C(mz)+ly;

    rsize = mx;		/* row size   */
    psize = rsize*my;	/* plane size */
    msize = psize*mz;	/* map size   */

    loc =  1 + rsize + psize;
    hic = msize - loc;

#define m2(b,d) mmapm[(b)+(d)] + mmapm[(b)-(d)]
#define m8(b,h,v) m2(b,h) + m2(b,v) + m2(b,h+v) + m2(b,h-v)

    for (iter = 1; iter < nfilter; iter++)
      {
	  /* count neighborhood */
	  for (i = loc; i < hic; i++)  countm[i] =
	    (m2(i,1)+m8(i-1,rsize,psize)+m8(i,rsize,psize)+m8(i+1,rsize,psize))/26;

	  /* update neighbors */
	  for (i = loc; i < hic; i++)
	    {
		magc = -countm[i];  if (magc<0) magc = -magc;
		magm = -mmapm[i];   if (magm<0) magm = -magm;
		if (magc>magm) mmapm[i] = countm[i];
	    };

	  DisplayMapColorized(map,iter);
	  DisplayInventorMap(map,iter);
      };

#undef m8
#undef m2

};


void FilterMap(Map3D map) /* replace wek cells with stronger local averages */
{
    Map3D Counts;
    MainInt mx, my, mz, lx, ly, lz;  /* Map access constants */
    MainInt loc, hic, msize, psize, rsize;
    MainInt i, magc, magm;
    Int16  *mmapm;				/* map array */
    Int16  *countm;
    
    MakeMap3D(map.msize, map.lomv, map.himv, &Counts);

    mmapm = map.mapm;
    countm = Counts.mapm;
    mx = map.msize[0];  my = map.msize[1];  mz = map.msize[2];
    lx = ILOG2C(mx);   ly = ILOG2C(my)+lx;   lz = ILOG2C(mz)+ly;

    rsize = mx;		/* row size   */
    psize = rsize*my;	/* plane size */
    msize = psize*mz;	/* map size   */

    loc =  1 + rsize + psize;
    hic = msize - loc;

#define m2(b,d) mmapm[(b)+(d)] + mmapm[(b)-(d)]
#define m8(b,h,v) m2(b,h) + m2(b,v) + m2(b,h+v) + m2(b,h-v)
    /* count neighborhood */
    for (i = loc; i < hic; i++)  countm[i] =
      (m2(i,1)+m8(i-1,rsize,psize)+m8(i,rsize,psize)+m8(i+1,rsize,psize))/26;
#undef m8
#undef m2
    
    /* update neighbors */
    for (i = loc; i < hic; i++)
      {
	  magc = -countm[i];  if (magc<0) magc = -magc;
	  magm = -mmapm[i];   if (magm<0) magm = -magm;
	  if (magc>magm) mmapm[i] = countm[i];
      };
    FreeMap3D(map);
    
};


void DisplayImageView(Map3D map, char *fname) 
  /* Make 3D map view from camera perspective, coloring marked sites */
{
    pixel **picout;	/* display picture, with correlation graphics */
    double ct, st, sp, cp, x, y, z, xr, yr, zr, xcell, ycell, zcell;
    double r, g, b;

    int ix, iy, iz, mx, my, mz, lx, ly, sx, sy, ip, jp;
    int ipmin, ipmax, jpmin, jpmax;
    int cl;
    FILE *fp;
    
    /* Create map view image */

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

    cp = COS(pan);   sp = SIN(pan);
    ct = COS(tilt);  st = SIN(tilt);

    picout = ppm_allocarray(sx = Picw/2, sy = Pich/2);
    for (ip = 0; ip < sy; ip++) for (jp = 0; jp < sx; jp++)
      PPM_ASSIGN(picout[ip][jp],0,0,0);

    for (iy = 0; iy < my; iy++)
    for (iz = 0; iz < mz; iz++)
      for (ix = 0; ix < mx; ix++)
	if (map.mapm[(iz<<ly)+(iy<<lx)+ix] > 5)
	{

	    xcell = (map.himv[0]-map.lomv[0])/mx;
	    ycell = (map.himv[1]-map.lomv[1])/my;
	    zcell = (map.himv[2]-map.lomv[2])/mz;
	    xr = (ix+0.5)*xcell + map.lomv[0];
	    yr = (iy+0.5)*ycell + map.lomv[1];
	    zr = (iz+0.5)*zcell + map.lomv[2];

	    xr -= PosL[0];
	    yr -= PosL[1];
	    zr -= PosL[2];
	    
	    y = yr*cosH - xr*sinH;
	    x = xr*cosH + yr*sinH;
	    z = zr*cosT + x*sinT;
	    x = x*cosT - zr*sinT;
	    if (x>0.5)
	      {
		  jpmin = sx/2 - 0.5*CamFocal*(y+0.25*ycell)/x;
		  jpmax = sx/2 - 0.5*CamFocal*(y-0.25*ycell)/x;
		  ipmin = sy/2 - 0.5*CamFocal*(z+0.25*zcell)/x;
		  ipmax = sy/2 - 0.5*CamFocal*(z-0.25*zcell)/x;
		  
		  if(ipmax>=0 && ipmin<sy && jpmax >=0 && jpmin<sx)
		    {
			r = g = b = 0;
			for (cl = 1; cl < ncl; cl++)
			  if (ix>=cllo[0][cl] && ix<=clhi[0][cl] &&
			      iy>=cllo[1][cl] && iy<=clhi[1][cl] &&
			      iz>=cllo[2][cl] && iz<=clhi[2][cl] )
			    {
				r = clcol[0][cl];
				g = clcol[1][cl];
				b = clcol[2][cl];
			    };
			if (r<0) r = 0;  if (r>1) r = 1;
			if (g<0) g = 0;  if (g>1) g = 1;
			if (b<0) b = 0;  if (b>1) b = 1;
			if (r>0 || g>0 || b>0)
			for (ip = ipmin; ip <= ipmax; ip++)
			  for (jp = jpmin; jp <= jpmax; jp++)
			    if(ip>=0 && ip<sy && jp >=0 && jp<sx)
			      PPM_ASSIGN(picout[ip][jp],
					 (int) 255*r,(int) 255*g, (int) 255*b);
		    };
	      };
	};
    /* and write out display */
    
    fp = fopen(fname,"w");
    ppm_writeppm(fp,picout,sx,sy,256,1);
    fclose(fp);

    ppm_freearray(picout, sy );
};


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




int ReadNavigation(char *nav)    /* Set up navigation file framework */
{
    FILE *navp;  int i;  char str[200];
    
    /* 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); };

    /* 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 */
    if (!FishBody(&CalL,picL) || !FishBody(&CalR,picR))
      {printf("No room for rectification tables!\n"); exit(0); };
	  
    /* 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);

    /* camera focal length, in pixels */
    CamFocal = (Picw/2)/TAN((PI/180)*CalL.FOV/2); 

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

    /* 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);
 
    Navnum = 0;
    while (fgets(str,100,navp) != NULL &&
	   ((str[0]>='0' && str[0]<='9') || str[0]=='-' || str[0]=='.'))
      /* Record nav file in nav list */
      {
	  sscanf(str,"%g%g%g %s\n",&t1,&t2,&t3,Navimg[Navnum]);
	  NavCamHead[Navnum] = t1*PI/180;
	  NavCamTilt[Navnum] = CamTilt;
	  NavCamPos[Navnum][0] = t2;
	  NavCamPos[Navnum][1] = t3;
	  NavCamPos[Navnum][2] = CamPos[2];
	  Navnum++;
      }
    
    fclose(navp);
    return(1);
};


void RegisterNavPosition(int Navid)  /* Set up view parameters for a paricular view */
{
    if (Navid >= Navnum) 
      { printf("Out of Nav file bounds! %d > %d\n",Navid,Navnum); exit(0); };

    sscanf(str,"%g%g%g %s\n",&t1,&t2,&t3,Navimg[Navnum]);
    CamHead = NavCamHead[Navid];
    CamTilt = NavCamTilt[Navid];
    CamPos[0] = NavCamPos[Navid][0];
    CamPos[1] = NavCamPos[Navid][1];
    CamPos[2] = NavCamPos[Navid][2];
    sinT = SIN(CamTilt);   cosT = COS(CamTilt);
    sinH = SIN(CamHead);   cosH = COS(CamHead);
    /* set up left camera position */
    PosL[0] = CamPos[0] - sinH*CamSep/2;
    PosL[1] = CamPos[1] + cosH*CamSep/2;
    PosL[2] = CamPos[2];
    /* set up right camera position */
    PosR[0] = CamPos[0] + sinH*CamSep/2;
    PosR[1] = CamPos[1] - cosH*CamSep/2;
    PosR[2] = CamPos[2];
	
}


void ReadNavImages(int Navid)   /* Read a particular navigation image */ 
{
    int i;
    int cols, rows, format;      gray maxval;
    
    RegisterNavPosition(Navid);
    strcpy(img,Navimg[Navid]);
    imgl = strlen(img);

    /* 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 */
    FishTail(picL, &CalL,recL);
    FishTail(picR, &CalR,recR);
};



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

    int i;
 
    InitCmacs();

    if (argc < 2 || argc > 3)
      {
	  printf("Usage: fryfish mapname <colorize>\n");
	  printf("mapname.map3D is a 3D evidence grid binary file\n");
	  printf("colorize.color is a colorization file of <xl xh yl yh zl zh r g b> lines\n");
	  exit(0);
      };

    pgm_init(&argc, argv);
    ppm_init(&argc, argv);

    strcpy(nav,argv[1]); navl = strlen(nav);
    strcpy(col,argc==3?argv[2]:nav);  coll = strlen(col);

    if (!ReadMap3Db(strcat(nav,".map3D"), &map, title, footer))
      {
	  printf("Couldn't read %s !\n",nav);
	  exit(0);
      };
    nav[navl] = 0;

    ReadColorization(nav,map);     /* read colorization file */

DisplayMapColorized(map,0);
exit(0);

    ReadNavigation(nav);     /* read navigation file */
    printf("%d nav pairs\n",Navnum);

    ReadNavImages(0);


    {   int i, j;
	gray **picout;   /* display picture, with correlation graphics */

	picout = pgm_allocarray(Picw/2, Pich/2);
	for (i=0; i<Pich/2; i++)
	  for (j=0; j<Picw/2; j++)
	    picout[i][j] = (recL[(i<<1)][(j<<1)] + recL[(i<<1)+1][(j<<1)+1] + 
	      recL[(i<<1)+1][(j<<1)] + recL[(i<<1)][(j<<1)+1])>>2;
	fp = fopen("test.pgm","w"); /* and write out display */
	pgm_writepgm(fp,picout,Picw/2,Pich/2,256,0);
	fclose(fp);
	
	pgm_freearray(picout, Pich/2 );
    };

    DisplayImageView(map,"test.synth.ppm");


    printf("Map %s (%s) (%s)\n",nav,title,footer);

    printf("\nMap [%d %d %d] [%.3g:%.3g,%.3g:%.3g,%.3g:%.3g]\n",
	   map.msize[0], map.msize[1], map.msize[2],
	   map.lomv[0],map.himv[0],
	   map.lomv[1],map.himv[1],
	   map.lomv[2],map.himv[2]);

    DisplayMapSetZ(map,3*256,4*256);
    DisplayMapSetY(map,3*256,4*256);
    DisplayMapSetX(map,3*256,4*256);

    DisplayMapColorized(map,0);
    DisplayInventorMap(map,0);

    DisplayMapFiltered(2);

    /* list colored region color occupancies */
    for (i=0; i<ncl; i++)
      printf("%8d / %d   	cells in	%s\n",clcnt[i],
	     (clhi[0][i]-cllo[0][i]+1)*(clhi[1][i]-cllo[1][i]+1)
	     *(clhi[2][i]-cllo[2][i]+1),cllabel[i]);
};