#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 Float64 double
#define Float32 float
#define Int32 int
#define Int16 short
#define Int8 char
#define PI 3.14159265358979  /* Pies are round */

/* define only one of the options below */
/* CAUTION: the FULL_POINTERS option targets only a single array, set by getfish */
/* #define FULL_POINTERS     /* full is 25% faster, but twice as big (about 2MB) */
#define OFFSET_POINTERS	  /* offset is slower, but only about 1MB of storage */

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


/* Structure to hold raw calibration parameters and rectification tables */
typedef struct {Int32 Ph, Pw;		/* Height and Width of image */
		Float32 Yaim, Xaim, Distance, FOV;  /* Calibration setup */
		Float32 Pi, Pj, AS; /* pixel optical center and aspect ratio */
		Float32 Si, Sj;     /* optical image center in spot coords */
		Float32 AN;	     /* Roll angle of the grid image */
		Int32  PN;	    /* degree of radial distortion polynomials */
		Float32 *P;         /* Spot->Pixel radius polynomial */
		Float32 *S;         /* Pixel->Spot radius polynomial */
		signed Int8 *di, *dj; /* rectification offset tables */
		gray **pij;   /* rectification full pointer table */
	      }  Calib;


int getfish(char *CalFile, Calib *Cal, gray **picin)
{	
    FILE *fp;  float t;
    double rp, is, js, rs, ang, sang, cang;
    double Xaim, Yaim, AN;
    int  i, j, k, l, ii, jj, Ph, Pw, PN;
    float isc, jsc;	  /* Image axis center */
    double ipc, jpc, aspect, rootaspect; 
    double SpotSpacing;

#ifdef OFFSET_POINTERS
    int di, dj;
#endif
#ifdef FULL_POINTERS
    gray **trect;	/* full pointers */
#endif
	    
    /* Read in calibration parameters from .calib file */
    
    fp = fopen(CalFile,"r"); if (!fp) return(0);
   /* read in 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", &(*Cal).Ph, &(*Cal).Pw, 
	   &(*Cal).Yaim, &(*Cal).Xaim, &(*Cal).Distance, &(*Cal).FOV);
    fscanf(fp,"%d%g%g%g%g%g%g\n", 
	   &(*Cal).PN,&(*Cal).Pi,&(*Cal).Pj,&(*Cal).AS,&(*Cal).Si,&(*Cal).Sj,&(*Cal).AN);

    (*Cal).P = (Float32 *) malloc((*Cal).PN*sizeof(Float32));
    (*Cal).S = (Float32 *) malloc((*Cal).PN*sizeof(Float32));
   
    for (i=0; i<(*Cal).PN; i++)
      { fscanf(fp,"%g",&t);  (*Cal).P[i] = t;};  fscanf(fp,"\n");
    for (i=0; i<(*Cal).PN; i++) 
      { fscanf(fp,"%g",&t);  (*Cal).S[i] = t;};  fscanf(fp,"\n");
    fclose(fp);

    /* Now calculate the per pixel rectification, and store in either the
       offset or the direct pointer tables, depending on compile switches */

#ifdef OFFSET_POINTERS
    (*Cal).di = (signed char *) malloc((*Cal).Ph*(*Cal).Pw);  /* precalculated rectification */
    (*Cal).dj = (signed char *) malloc((*Cal).Ph*(*Cal).Pw);  /* storage arrays */
#endif
#ifdef FULL_POINTERS
    trect = (*Cal).pij = (gray **) malloc((*Cal).Pw*(*Cal).Ph*sizeof(gray *));
#endif

    Ph = (*Cal).Ph;   Pw = (*Cal).Pw;  PN = (*Cal).PN;
    Xaim = (*Cal).Xaim;  Yaim = (*Cal).Yaim;  AN = (*Cal).AN;
    ipc = (*Cal).Pi;     jpc = (*Cal).Pj;
    aspect = (*Cal).AS;   isc = (*Cal).Si;  jsc = (*Cal).Sj;
    
    rootaspect = sqrt(aspect);
    /* Desired spot spacing calculated from user FOV request and Distance */
    SpotSpacing = Pw/((*Cal).Distance*2.0*tan((*Cal).FOV*PI/360.0));

    l=0;  /* calculate the rectification */
    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 = (*Cal).P[PN-1]; for (k = PN-2; k>=0; k--)  rp = rp*rs+(*Cal).P[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-AN);  cang = cos(ang-AN);
	  
	  ii = rp*sang/rootaspect + ipc;
	  jj = rp*cang*rootaspect + jpc;
	  
	  /* make sure neither picture nor offset bounds are exceeded */
	  
	  if (ii<0) ii=0; else if (ii>Ph-1) ii=Ph-1;
	  if (jj<0) jj=0; else if (jj>Pw-1) jj=Pw-1;
	  
#ifdef FULL_POINTERS
	  *(trect++) = &(picin[ii][jj]);
#endif
#ifdef OFFSET_POINTERS
	  di = ii-i;   dj = jj-j;
	  if (di>127) di=127; else if (di<-127) di=-127;
	  if (dj>127) dj=127; else if (dj<-127) dj=-127;
	  (*Cal).di[l] = di;  (*Cal).dj[l] = dj;  l++;
#endif
      };
    return(1);
};


void applyfish(gray **picin, Calib *Cal, gray **picout)
{
    int Ph, Pw, i, j;
    gray *tout;		/* outgoing pixel pointer */
#ifdef OFFSET_POINTERS
    signed char *tdi, *tdj;  /* the offsets */
#endif
#ifdef FULL_POINTERS
    gray **trect;	/* the full pixel addresses */
#endif

    Ph = (*Cal).Ph;  Pw = (*Cal).Pw;

#ifdef OFFSET_POINTERS
    tdi = (*Cal).di;   tdj = (*Cal).dj;
    for (i=0; i<Ph; i++)
      { tout = picout[i];
	for (j=0; j<Pw; j++) *(tout++) = picin[i+ *(tdi++)][j+ *(tdj++)]; };
#endif

#ifdef FULL_POINTERS
    trect = (*Cal).pij;
    for (i=0; i<Ph; i++)
      { tout = picout[i];
	for (j=0; j<Pw; j++) *(tout++) = **(trect++);  };
#endif
}