#include <stdio.h>
#include <math.h>
#include "cmacs.h"
#include "volsense.h"

MainFloat Pinc[NP];

#define MaxRead 10000     /* maximum number of range samples */
MainInt nsamples;
MainFloat Rrange[MaxRead], Rp[MaxRead][3], Ra[MaxRead][3];


Map3D map1, ideal;
CylSensorModelArray smd;
#define maxread 1000  /* maximum number of range samples to read from file */


#define scanmax 50 /* longest param search losing streak tolerated*/
#define scancoarse 12.3285  /* grain coarsener for initial search */
#define scaninc 1.519911083  /* scan search refinement factor */


MainFloat trymodel(tmodel,tmap) MainInt *tmodel, *tmap;
    {MainInt t1, i;

      t1=TickCount();  /* time sensor model make */
      FillCylModel(smd);
      *tmodel=TickCount()-t1;
      ClearMap3D(map1);
      t1=TickCount();  /* time map build */
      for (i=0; i<nsamples; ++i)
	AddCylReading(Rrange[i], Rp[i], Ra[i], smd, map1);
      *tmap=TickCount()-t1;
      return(MatchMap3D(map1, ideal));
    }

writemodel(matchv, tmodel, tmap)  MainFloat matchv; MainInt tmodel, tmap;
    { char titl[200], note[200];
      sprintf(titl,"Map3D ");
      sprintf(note,
      "{%.4g,%.4g,%.4g, %.4g,%.4g,%.4g, %.4g,%.4g,%.4g, %.4g,%.4g,%.4g, %.4g,%.4f,%.4f}->%.2f, (*%.2f %.2f*)",
		    P[0],P[1],P[2],P[3],P[4],P[5],P[6],P[7],P[8],
	            P[9],P[10],P[11],P[12],P[13],P[14],
		    matchv,
		    tmodel/60.0,tmap/60.0);
      printf("%s\n",note);
      fflush(stdout);
      WriteMap3D(map1,titl,note,"Hillmap");
      WriteCylModel(smd, "Hillsensor");
      TraceCylModel(smd, "Hillsensortrace");
   }

axisclimb()
{ MainInt tmodel, tmap, i, s, change, pchange, scan;
  MainFloat  oldbest, oldpval, t, incr, incrf;

  oldbest = trymodel(&tmodel,&tmap); writemodel(oldbest,tmodel,tmap);
  
  for (incrf=scancoarse; incrf>=1.0; incrf /= scaninc)
    { printf("coarseness %g\n",incrf); fflush(stdout); 
      change = 1;
      while (change)
	{ change=0;
	  for (s = 0; s <= 1; s++)
	    for (i=0; i<NP; i++)
	      { pchange=0;  oldpval = P[i]; 
		incr = (s?-incrf:incrf)*Pinc[i];
		for (scan=0; scan<scanmax; scan++)
		  { P[i] += incr;
		    if (P[i]>=Pl[i] && P[i]<=Ph[i]) 
		      { t = trymodel(&tmodel,&tmap);
			if (t>oldbest) { scan = 0; oldbest = t;
					 oldpval = P[i]; pchange=1;}
		      }
		  }
		P[i] = oldpval;
		if (pchange)  { change=1; t = trymodel(&tmodel,&tmap);
				writemodel(t,tmodel,tmap); }
	      }
	}
    }
  printf("That's all folks!\n");
};


MakeWorld(map)
     Map3D map;
    { MainInt k, km;
      km = 1<<(ILOG2C(map.msize[0])+ILOG2C(map.msize[1])+ILOG2C(map.msize[2]));
      for (k=0; k<km; k++) map.mapm[k] = 0;
    }

TypeoutCKSumMap3D(map)
Map3D map;
/*** 3D ***************************************************************\
*     CKSumMap3D(map)
*		types out a checksum hash of a map
\**********************************************************************/
{ MainInt ix, iy, iz, mx, my, mz, lx, ly, np, nn, nz, cks, t;
  FILE *mfile;

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

  srandom(213);
  np = 0;  nn = 0;  nz = 0;  cks = 0;
  for (iz = 0; iz < mz; iz++)
    for (iy = 0; iy < my; iy++)
      for (ix = 0; ix < mx; ix++)
    {
      t = map.mapm[(iz<<ly)+(iy<<lx)+ix];
      if (t<0) nn++; else if (t>0) np++; else nz++;
      cks += t^random();
    }
  printf("**********   Map -0+ (%d,%d,%d), Checksum %d   **********\n",nn,nz,np,cks);
}


TypeoutMap3D(map)
Map3D map;
/*** 3D ***************************************************************\
*     TypeoutMap3D(map)
*		writes a map into tty
\**********************************************************************/
{ MainInt ix, iy, iz, mx, my, mz, lx, ly, ixi, iyi, izi, t, nz, nzl;
  FILE *mfile;
            
  printf("[%d %d %d] [%g %g %g]  [%g %g %g]\n",
	 mx = map.msize[0], my = map.msize[1], mz = map.msize[2],
	 map.lomv[0],  map.lomv[1],  map.lomv[2],
	 map.himv[0],  map.himv[1],  map.himv[2] );
  lx = ILOG2C(mx);
  ly = ILOG2C(my)+lx;

  ixi = iyi = izi = 1;
  while (mx/ixi > 32) ixi *= 2;
  while (mx/iyi > 32) iyi *= 2;
  while (mx/izi > 32) izi *= 2;  

  nzl = 0;      
  for (iz = 0; iz < mz; iz += izi)
    {
      t = 0;
      for (iy = 0; iy < my; iy += iyi)
	for (ix = 0; ix < mx; ix += ixi)
	  t |= map.mapm[(iz<<ly)+(iy<<lx)+ix];
      if (t)
	{ if (nzl) {printf("\n"); nzl=0;};
	  printf("-----------------------------Level %d-----------------------------\n",iz); nz = 0;
	  for (iy = 0; iy < my; iy += iyi)
	    { nz++;
	      for (ix = 0; ix < mx; ix += ixi)
		if(map.mapm[(iz<<ly)+(iy<<lx)+ix]) 
		  {if (nz>1) printf("+%d\n",nz-1); nz=0;};
	      if (nz==0)
		{ printf("|");
		  for (ix = 0; ix < mx; ix += ixi)
		    {t = map.mapm[(iz<<ly)+(iy<<lx)+ix];
		     printf("%c%c",t<0?(t<-35?'=':'-'):t>0?(t>35?'#':'+'):' ',
			    " 123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[(t<0?-t:t)%36]);
		   }
		  printf("|\n");
		}
	    }
	  if(nz>0) printf("+%d\n",nz);
	} else {printf("-%d",iz); nzl++;}
    }
  if (nzl) printf("\n");
}

main()
    {
      FILE *readbeam;
      Int32  i, j, nrays, bl, bh;
      MainFloat tsense, tray, tsetup, tfill, tmatch, rng, t1, t;
      float ft1, ft2, ft3, ft4, ft5, ft6, ft7;  /* single floats for scanfs */
      MainInt msize[3];
      MainFloat lov[3],  hiv[3], Pos[3], Dir[3];
      char maphead[100], mapfoot[100];
 
      InitCmacs();
      tsense = -TickCount();  /* time sensor model build */

 if(0) { InitCylModelParams(); /* fat, sonar-type, rays */
      MakeBlankCylModel(30,0.02, 64,128, 1.0,10.0 ,&smd);
      nrays = 5000;   /* number of rays to throw */     }
 
else { InitCylModelParams();      P[an0]=.00002;    /* thin rays! */
      MakeBlankCylModel(30,0.01, 4,128, 1.0,10.0 ,&smd);
      nrays = 50000;   /* number of rays to throw */    };

      FillCylModel(smd);
      TrimCylModel(smd);
      tsense += TickCount();
 
if(0) {  /* do rays numbered bl to bh */
         bl = 753;  bh = 754;     bl = 0;  bh = 50000;  ACRrays = bl;
	 for (i=0; i<bl; i++) for (j=0; j<6; j++) random();
	 nrays = bh-bl; };
  
      TraceCylModel(smd, "cylmodel.trace"); 

      for (i=0; i<3; i++) {msize[i]=128; lov[i]=0.0; hiv[i]=10.0;};
      MakeMap3D(msize, lov, hiv, &map1);

      tray = -TickCount();  /* time ray throw */
      tsetup = tfill = 0.0;
      for (j=0; j<nrays; j++)
	{
#define skin 0.4
	  rng = 0.0;
	  for (i=0; i<3; i++)
	    { Pos[i] = (10.0-2.0*skin)*RAND+skin;
	      Dir[i] = (10.0-2.0*skin)*RAND+skin - Pos[i];
	      rng += Dir[i]*Dir[i]; };
	  rng = SQRT(rng);
if (1) {  /* let rays escape */
	    rng = 1.0 + 9.0*RAND;  for (i=0; i<3; i++) Dir[i] = RAND; }
	  AddCylReading(rng,Pos,Dir,smd,map1);  ACRrays++;
	  tsetup += ACRtsetup;   tfill += ACRtfill;
	}
      tray += TickCount();   printf("\n");

      TypeoutCKSumMap3D(map1);

if(1) { /* compare maps with previous run */
      ReadMap3Db("Map3D",&ideal,maphead,mapfoot);
      WriteMap3Db(map1,"volmodel","test","Map3D");
      tmatch = -TickCount();
      t1 = MatchMap3D(map1,map1);
      t = MatchMap3D(map1,ideal);
      tmatch += TickCount();
      printf("Map Match: %g/%g   time %g seconds\n",t,t1,tmatch/120.0);
    }
if (0)    TypeoutMap3D(map1);

      printf("Sensor time %g seconds; Throwing %d rays, %d hits\n",
	     tsense/60.0,ACRrays,ACRhits);
      printf("Ray throw (%d ray avg) %.4g ms (%.4g setup + %.4g cast)\n",
	     nrays, tray/(.06*nrays), tsetup/(.06*nrays),tfill/(.06*nrays));
      
exit(0);

      if ((readbeam=fopen("room.920515","r"))==NULL)
	{printf("Couldn't open reading file\n"); exit(0);}
      nsamples = 0;
      while(nsamples<maxread && fscanf(readbeam,"%f %f %f %f",
				       &ft1, &ft2, &ft3, &ft4) != EOF)
	{Rrange[nsamples] = ft1;
	 Rp[nsamples][0]=ft2;  Rp[nsamples][1]=ft3;  Rp[nsamples][2]=ft4;  
	 Ra[nsamples][0]=ft5;  Ra[nsamples][1]=ft6;  Ra[nsamples][2]=ft7;  
	nsamples++;};
      fclose(readbeam);
		
      axisclimb();
   }