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

#define PI 3.14159265358979  /* Pies are round */

#define PhMax	1000  /* Maximum picture height */
#define PwMax	2000  /* Maximum picture width */

/* Fast image rectifier, applying calibration parameters
   from the "flatfish" radial distortion finder and rectifier
   for solid state cameras with wide angle lenses.
                              Created 4/96
                              Hans Moravec <hpm@cmu.edu>
                              Carnegie Mellon University
			      Pittsburgh, PA  15213
			      working at Daimler Benz research, Berlin */

float t;

/* Calibration setup */
float Yaim = 0.0, Xaim = -2.62, Distance = 8.5, FOV = 90.0;
/* Daimler Benz old camera calibration setup 
   Yaim = 0.0, Xaim = 0.0, Distance = 40.33, FOV = 30.0; */
/* CMU red camera
   Yaim = 0.0, Xaim = -2.62, Distance = 8.5, FOV = 90.0; */
int CalibPh, CalibPw;  /* Image size for this calibration */
float CalibPi, CalibPj, CalibAS;
/* Calibrated pixel image center and aspect ratio */
float CalibSi, CalibSj;   /* Calibrated spot image center */
int CalibPN = 0;  /* Scatter polynomial size */
float CalibP[100];  /* Spot->Pixel radius scatter plot polynomial */
float CalibS[100];  /* Pixel->Spot radius scatter plot polynomial */
float CalibAN;	     /* Angle of the gid image from horizontal (vertical) */


int main(argc, argv)
int argc;
char *argv[];
{
    gray **picin;  	/* raw incoming picture */
    gray **picout;	/* rectified outgoing picture */
    gray *tout;
    gray **allrect, **trect;	/* complete pointer */
    int	Ph,	/* Picture height, pixels (576 or 480 typical) */
        Pw;	/* Picture width, pixels  (768 or 640 typical) */

    float isc, jsc;	  /* Image axis center */
    double ipc, jpc, aspect, rootaspect; 
    int i;  gray maxval;  int rows, cols;
    FILE *fp;  char img[100], cal[100];  int imgl;	     /*  image name */

    if (argc < 2 || argc > 3)
      {
	  printf("Usage: fastfish image.pgm <cam.calib>\n");
	  printf("image.pgm contains a picture to be rectified\n");
	  printf("using the 'flatfish' calibration parameters in cam.calib\n");
	  printf("if cam.calib is not given, the program tries image.calib\n");
	  
	  exit(0);
      };

    strcpy(img,argv[1]); imgl = strlen(img);
    if (imgl>5) if (img[imgl-4]=='.') imgl -= 4;
    img[imgl] = 0;

    pgm_init(&argc, argv);

    /* Read in raw image */

    fp = fopen(argv[1],"r");
    if (fp == NULL) {printf("No %s file!!\n",argv[1]); exit(0); };
    picin = pgm_readpgm(fp, &cols, &rows, &maxval );
    fclose(fp);
    Pw = cols;   Ph = rows;  
    if (maxval > 255)
      { printf("Digitized pic pixels too big: %d instead of %d!!\n",
	       maxval,255);  exit(0); };
    
    if (argc >= 3) strcpy(cal,argv[2]); else strcpy(cal,strcat(img,".calib"));
    img[imgl] = 0;  /* reset file name to base */

    /* Read in calibration parameters into .calib file */
    
    fp = fopen(cal,"r"); if (!fp) {printf("Couldn't read %s!\n",cal); exit(0); };
   /* write out the calibration setup, 
      Ph, Pw, Xaim, Yaim, Distance, FOV,
      The polynomial degree+1,
      the distortion center i and j pixel coordinates, the aspect ratio,
      the distortion center i and j spot coordinates, the image rotation angle,
      the Spot->Pixel polynomial coefficients,
      the Pixel->Spot polynomial coefficients */
   
    fscanf(fp,"%d%d%g%g%g%g\n", &CalibPh, &CalibPw, &Yaim, &Xaim, &Distance, &FOV);
    fscanf(fp,"%d%g%g%g%g%g%g\n", 
	   &CalibPN,&CalibPi, &CalibPj,&CalibAS,&CalibSi,&CalibSj,&CalibAN);
   
    for (i=0; i<CalibPN; i++)
      { fscanf(fp,"%g",&t);  CalibP[i] = t;};  fscanf(fp,"\n");
    for (i=0; i<CalibPN; i++) 
      { fscanf(fp,"%g",&t);  CalibS[i] = t;};  fscanf(fp,"\n");
    fclose(fp);

    if (CalibPh != Ph || CalibPw != Pw)
      printf("Calibration size [%d,%d] different from Image size [%d,%d]\n",
	     CalibPh,CalibPw,Ph,Pw);

    if (Yaim>10 || Yaim<-10 || Xaim>15 || Xaim<-15 
	|| Distance<3 || Distance>50 || FOV < 10 || FOV > 180)
      {
	  printf("(Yaim %.3g Xaim %.3g Distance %.3g FOV %.3g) seem unreasonable\n",
		 Yaim, Xaim, Distance, FOV);
	  exit(0);
      };

    printf(argv[1]);  printf("\n");
    printf(" %d high by %d wide, max pixel %d\n\n",Ph,Pw,maxval);

    /* Create display image array */
    picout = pgm_allocarray(Pw,Ph);

    /* Rectify image  */
    { 
	

	double rp, is, js, rs, ang, sang, cang;
	int deg, i, j, k, l, m, ii, jj;
	int errc, errd, di, dj, mindi, maxdi, mindj, maxdj;
	double SpotSpacing;
	int time1, time2 ;
        signed char *rectdi, *rectdj, *tdi, *tdj;

	/* Desired spot spacing calculated from user FOV request and Distance */
	SpotSpacing = Pw/(Distance*2.0*tan(FOV*PI/360.0));

	printf("Rectified image spot spacing %.4g pixels\n",SpotSpacing); 
	printf("Calibration parameters:\n"); 
	printf("ip %.4g  jp %.4g  aspect %.4g  is %.4g js %.4g angle %.4g\n",
	       CalibPi,CalibPj,CalibAS,CalibSi,CalibSj,CalibAN); 
	printf("S->P Polynomial:");
	for (i=0; i<CalibPN; i++)
	  { printf("  %.4g",CalibP[i]);  }
	printf("\n");   printf("P->S Polynomial:");
	for (i=0; i<CalibPN; i++)
	  { printf("  %.4g",CalibS[i]);  }
	printf("\n");
	
	time1 = clock() ;  /* time the rectification precalculation */
	
	deg = CalibPN;   ipc = CalibPi;     jpc = CalibPj;
	aspect = CalibAS;   isc = CalibSi;  jsc = CalibSj;
	
	rootaspect = sqrt(aspect);
	errc = errd = 0;  mindi=maxdi=mindj=maxdj=0;
	
	/* Make grid spacing SpotSpacing pixels between spot centers */
	
	rectdi = (signed char *) malloc(Ph*Pw);  /* precalculated rectification */
	rectdj = (signed char *) malloc(Ph*Pw);  /* storage arrays */
	allrect = (gray **) malloc(Pw*Ph*sizeof(gray *));

	l=0;  /* precalculate rectification */
	trect = allrect;
	for (i=0; i<Ph; i++)  for (j=0; j<Pw; j++)
	  {   
	      /* calculate spot coordinates desired for this destination pixel */
	      is = (i - Ph/2)/SpotSpacing + Yaim;
	      js = (j - Pw/2)/SpotSpacing + Xaim;
	      
	      /* calculate radius from spot optical axis */
	      is -= isc;    js -= jsc;
	      rs = sqrt(is*is + js*js);
	      
	      /* transform spot radius to pixel radius */
	      rp = CalibP[deg-1]; for (k = deg-2; k>=0; k--)  rp = rp*rs+CalibP[k];
	      
	      /* project pixel radius back onto to spot radius direction,
		 rotated by grid angle from horizontal, and recenter  */
	      ang = atan2(is,js);
	      sang = sin(ang-CalibAN);  cang = cos(ang-CalibAN);
	      
	      ii = rp*sang/rootaspect + ipc;
	      jj = rp*cang*rootaspect + jpc;
	      
	      /* make sure neither picture nor offset bounds are exceeded */

	      if (ii<0) {ii=0; errc++;} else if (ii>Ph-1) {ii=Ph-1; errc++;};
	      if (jj<0) {jj=0; errc++;} else if (jj>Pw-1) {jj=Pw-1; errc++;};
	      di = ii-i;   dj = jj-j;
	      if (di<mindi) mindi = di;  if (di>maxdi) maxdi = di;
	      if (dj<mindj) mindj = dj;  if (dj>maxdj) maxdj = dj;
	      if (di>127) {di=127; errd++;} else if (di<-127) {di=-127; errd++;};
	      if (dj>127) {dj=127; errd++;} else if (dj<-127) {dj=-127; errd++;};

	      *(trect++) = &(picin[ii][jj]);
	      rectdi[l] = di;  rectdj[l] = dj;  l++;
	  };
	time2 = clock() ;
	printf("\nRectification setup time: %g s\n", (time2 - time1)/1000000.0);
	printf("%d and %d pixels from beyond the edge\n",errc,errd); 
	printf("Di[%d:%d], Dj[%d:%d]\n",mindi,maxdi,mindj,maxdj);


	printf("\nRectification Times, seconds\n");
	printf("Full pointers      Offset pointers\n");


	time1 = clock();    /* time the rectification - needs to be fast */
	for (m = 0; m<100; m++) /* repeat, to average cache effects */
	{ 
	    /* apply full-pointer precalculated rectification: uses one
	       image array of pointers - twice as big as offset, but 25% faster*/
	    trect = allrect;
	    for (i=0; i<Ph; i++)
	      { tout = picout[i];
		for (j=0; j<Pw; j++) *(tout++) = **(trect++);  };
	};
	time2 = clock();
	printf("%10g",(time2 - time1)/1e8);

	time1 = clock();    /* time the rectification - needs to be fast */
	for (m = 0; m<100; m++) /* repeat, to average cache effects */
	{ 
	    /* apply offset-based precalculated rectification: uses two
	       image arrays of signed bytes - 1/2 size of full, 25% slower */
	    tdi = rectdi;   tdj = rectdj;
	    for (i=0; i<Ph; i++)
	      { tout = picout[i];
		for (j=0; j<Pw; j++) *(tout++) = picin[i+ *(tdi++)][j+ *(tdj++)]; };
	};
	time2 = clock() ;
	printf("%20g\n",(time2 - time1)/1e8);

	fp = fopen(strcat(img,".frect.pgm"),"w"); /* and write out display */
	img[imgl] = 0;
	pgm_writepgm(fp,picout,Pw,Ph,256,0);
	fclose(fp);
    }
    exit(0);
};