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

/* 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 (= paced for robot runtime)
   fisheye image rectifier, applying  calibration parameters
   from the "flatfish" wide angle image calibration program. 

   Fast interest operator, samples left member of stereo image pair,
   classifying regions suitable for stereo ranging.

   Fast correlator, for finding regions in right image corresponding to
   selected feastures in left image.

                              Created 4/96
                              Hans Moravec <hpm@cmu.edu>
                              Carnegie Mellon University
			      Pittsburgh, PA  15213
			      working at Daimler Benz research, Berlin */


/************************************************************************\

                 Camera Rectification Routines

\***********************************************************************/

int FishHead(char *CalFile, Calib *Cal)
/* FishHead reads in flatfish calibration file named "CalFile",
   into a calibration structure Cal.  It does not expand the
   parameters into a pointer table in Cal (done by FishBody) */
{	
    FILE *fp;  float t;
    int i;

    /* 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);
    return(1);
};


int FishBody(Calib *Cal, gray **picin)
{
    /* FishBody calculates the per pixel rectification, and store in either the
       offset or the direct pointer tables, depending on compile switches */

    double Xaim, Yaim, AN;
    int  i, j, k, l, ii, jj, Ph, Pw, PN;
    float isc, jsc;	  /* Image axis center */
    double rp, is, js, rs, ang, sang, cang;
    double ipc, jpc, aspect, rootaspect; 
    double SpotSpacing;

#ifdef OFFSET_POINTERS
    int di, dj;
#endif
#ifdef FULL_POINTERS
    gray **trect;	/* full pointers */
#endif

#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 */
    if (!(*Cal).dj || !(*Cal).di) return(0);
#endif
#ifdef FULL_POINTERS
    trect = (*Cal).pij = (gray **) malloc((*Cal).Pw*(*Cal).Ph*sizeof(gray *));
    if (!trect) return(0);
#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);
};



int GoFish(char *CalFile, Calib *Cal, gray **picin)
/* GoFish reads in flatfish calibration file named "CalFile",
   expands the parameters into a pointer table into pgm picture
   "picin", whose parameters must match those given in CalFile,
   and stores the result in structure "Cal"  */
{
    return (FishHead(CalFile, Cal)?
	    FishBody(Cal,picin):0);
};



void FishTail(gray **picin, Calib *Cal, gray **picout)
/* FishTail applies the transformation stored in "Cal" to the pgm
   picture "picin" to produce the rectified image "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
}





/********************************************************************\

                 Stereoscopic routines begin here

\********************************************************************/


int Interest(gray **picin, int Ph, int Pw, int *out)
/* Interest Operator, computes interest function on 8 x 8 windows.
   Image regions scored high by the interest operator contain enough
   brightness variations in all directions to be good candidates for
   finding by correlator in other views of the same scene */
/* picin is picin[Ph][Pw], out must be out[(Ph>>3)*(Pw>>3)]  */
{
    int i, j, k, l, rz;
    int top, ring;
    int temp[(Ph>>3)*(Pw>>3)];

/* Macros for in-line coding entire 8x8 interest window calculation */
#define A1 sumv+=ISQ[(ti=*(pptr+Pw))-(t=*pptr)]; sumh+=ISQ[(tj=*(++pptr))-t]; 
#define A2 sumhv+=ISQ[*(pptr+Pw)-t]; sumvh+=ISQ[tj-ti]; 
#define A3 sumv+=ISQ[(ti=*(pptr+Pw))-tj]; sumh+=ISQ[(t=*(++pptr))-tj]; 
#define A4 sumhv+=ISQ[*(pptr+Pw)-tj]; sumvh+=ISQ[t-ti]; 
#define A5 sumv+=ISQ[(ti=*(pptr+Pw))-t]; sumh+=ISQ[(tj=*(++pptr))-t]; 
#define A6 A2
#define LL A1 A2 A3 A4 A5 A6 A3 A4 A5 A6 A3 A4 A5 A6 
#define LN LL pptr += Pw-7;

    k = 0;
    /* Compute raw interest operator: among horizontal, vertical, right
       and left diagonal directions, return minimum sum of squares of
       differences of adjacent pixels, computed over non-overlapping
       8x8 windows */

    for (i = 0; i < Ph-7; i += 8) for (j = 0; j < Pw-7; j += 8)
      {  
	  register gray *pptr;
	  register int t, ti, tj, sumh, sumv, sumhv, sumvh;
	  sumh = sumv = sumhv = sumvh = 0;  pptr = &(picin[i][j]);
	  LN LN LN LN LN LN LL
	  if (sumv<sumh) sumh = sumv;  if (sumhv<sumh) sumh=sumhv;
	  temp[k++] = (sumvh<sumh)? sumvh:sumh;
      };
#undef A1
#undef A2
#undef A3
#undef A4
#undef A5
#undef A6
#undef LL
#undef LN

    /* high-pass filter the raw operator */
    rz = Pw>>3;  top = 0;  k = 0;
    for (i = 0; i < (Ph>>3); i++)  for (j=0; j<rz; j++) 
      if (i==0 || i==(Ph>>3)-1 || j==0 || j==rz-1) out[k++]=0; else
	{
	    ring = temp[k-1] + temp[k+1] + temp[k-rz] + temp[k+rz]
	      +temp[k-rz-1]+temp[k-rz+1]+temp[k+rz-1]+temp[k+rz+1];
	    out[k] = l = (temp[k]<<3) - ring;
	    k++;
	    if (l>top) top = l;
	};

    return(top);
};

int HInterest(gray **picin, int Ph, int Pw, int *out)
/* Horizontal Interest Operator, computes interest function on 8 x 8 windows.
   Image regions scored high by the interest operator contain enough
   brightness variations in horizontal directions to be good candidates for
   finding by correlator in other views of the same scene */
/* picin is picin[Ph][Pw], out must be out[(Ph>>3)*(Pw>>3)]  */
{
    int i, j, k, l, rz;
    int top, ring;
    int temp[(Ph>>3)*(Pw>>3)];

/* Macros for in-line coding entire 8x8 interest window calculation */
#define A1 sumv+=ISQ[(ti=*(pptr+Pw))-(t=*pptr)]; sumh+=ISQ[(tj=*(++pptr))-t]; 
#define A2 sumhv+=ISQ[*(pptr+Pw)-t]; sumvh+=ISQ[tj-ti]; 
#define A3 sumv+=ISQ[(ti=*(pptr+Pw))-tj]; sumh+=ISQ[(t=*(++pptr))-tj]; 
#define A4 sumhv+=ISQ[*(pptr+Pw)-tj]; sumvh+=ISQ[t-ti]; 
#define A5 sumv+=ISQ[(ti=*(pptr+Pw))-t]; sumh+=ISQ[(tj=*(++pptr))-t]; 
#define A6 A2
#define LL A1 A2 A3 A4 A5 A6 A3 A4 A5 A6 A3 A4 A5 A6 
#define LN LL pptr += Pw-7;

    k = 0;
    /* Compute raw interest operator: among horizontal, vertical, right
       and left diagonal directions, return minimum sum of squares of
       differences of adjacent pixels, computed over non-overlapping
       8x8 windows */

    for (i = 0; i < Ph-7; i += 8) for (j = 0; j < Pw-7; j += 8)
      {  
	  register gray *pptr;
	  register int t, ti, tj, sumh, sumv, sumhv, sumvh;
	  sumh = sumv = sumhv = sumvh = 0;  pptr = &(picin[i][j]);
	  LN LN LN LN LN LN LL

	  /*printf("temp[%d] = %d\n", k, sumh);*/	
	  temp[k++] = sumh;
      };
#undef A1
#undef A2
#undef A3
#undef A4
#undef A5
#undef A6
#undef LL
#undef LN
    /*    for (i=0; i<20; ++i) printf("temp[%d] = %d ", i, temp[i]);*/
    /*printf("\n");*/

    /* high-pass filter the raw operator */
    rz = Pw>>3;  top = 0;  k = 0;
    for (i = 0; i < (Ph>>3); i++)  for (j=0; j<rz; j++) 
      if (i==0 || i==(Ph>>3)-1 || j==0 || j==rz-1) out[k++]=0; else
	{
	    ring = temp[k-1] + temp[k+1] + temp[k-rz] + temp[k+rz]
	      +temp[k-rz-1]+temp[k-rz+1]+temp[k+rz-1]+temp[k+rz+1];
	    /*printf("out[%d] = %d", k, (temp[k] << 3) - ring);*/
	    out[k] = l = (temp[k]<<3) - ring;
	    k++;
	    /*printf(" %d %d\n", out[k-1], l);*/
	    if (l>top) top = l;
	};

    /*    for (i=0; i<20; ++i) printf("out[%d] = %d ", i, out[i]);*/
    /*printf("\ntop = %d\n", top);*/
    /*    exit(0);*/
    return(top);
};


int Correl8(gray **left, int ileft, int jleft, int Ph, int Pw,
	    gray **right, int irlo, int jrlo, int irhi, int jrhi,
	    int *correl, int *corri)
/* Scan image feature in "left" image centered on [ileft,jleft] across
   centers in "right" image rectangle bounded by [irlo,jrlo] [irhi,jrhi]
   inclusive.  The best correlation values for each j position, over all
   i positions are returned in indices jrlo to jrhi of array "correl".
   The array "corri" contains the i value at which the best match for
   each j was found. Correl8 is hard coded to use 8x8 windows around
   the selected locations. */
{
   /* Correlate across selected position */
    int i, j, lasti, cptr, bestcorel, bestj;
    int Lsum, Rsum;    gray lefts[64];
    register int t, tsum, sum;  register gray *lptr, *rptr;
    
    /* set up source window in left image */
    Lsum = 0;  lptr = lefts;
    for (i = -4; i<4; i++) for (j=-4; j<4; j++)
      Lsum += *(lptr++) = left[i+ileft][j+jleft];
    
    /* Macros for building inline version of correlation window matching step */
#define PIXa   Rsum += (t = *(rptr++));  tsum += ISQ[*(lptr++) - t];
#define PIXb   Rsum += (t = *(rptr));  tsum += ISQ[*(lptr++) - t];
#define PIXc   Rsum += (t = *(++rptr));  tsum += ISQ[*(lptr++) - t];
#define PIXd   Rsum += (t = *(rptr += Pw-7));  tsum += ISQ[*(lptr++) - t];
#define R1c   PIXa PIXa PIXa PIXa PIXa PIXa PIXa PIXb  /* first window row */
#define RNc   PIXd PIXc PIXc PIXc PIXc PIXc PIXc PIXc  /* Other rows */
    
    /* censor, then scan destination area in right image */
    
    if (irlo>irhi) SWAP(irlo,irhi);
    if (jrlo>jrhi) SWAP(jrlo,jrhi);
    if (irlo<4) irlo=4;  if (irhi>Ph-4) irhi=Ph-4;
    if (jrlo<4) jrlo=4;  if (jrhi>Pw-4) jrhi=Pw-4;

    cptr = jrlo;   bestj = bestcorel = 1e9;
    for (j = jrlo; j <= jrhi; j++)
      {	
	  lasti = sum = 1e9;
	  for (i = irlo; i <= irhi; i++)
	    {
		tsum = Rsum = 0;  lptr = lefts;
		rptr = &(right[i-4][j-4]);
		/* accumulate sum of squares of differences */
		R1c RNc RNc RNc RNc RNc RNc RNc
		  tsum -= ISQ[(Lsum-Rsum)>>3]; /* subtract out means */
		if (tsum<sum) {sum = tsum; lasti = i; }
	    };
	  correl[cptr] = sum;  corri[cptr++] = lasti;
	  if (sum<bestcorel) { bestcorel = sum;  bestj = j; };
      };
#undef PIXa
#undef PIXb
#undef PIXc
#undef PIXd
#undef R1c
#undef RNc
    return(bestj);
};


int Correl7(gray **left, int ileft, int jleft, int Ph, int Pw,
	    gray **right, int irlo, int jrlo, int irhi, int jrhi,
	    int *correl, int *corri)
/* Scan image feature in "left" image centered on [ileft,jleft] across
   centers in "right" image rectangle bounded by [irlo,jrlo] [irhi,jrhi]
   inclusive.  The best correlation values for each j position, over all
   i positions are returned in indices jrlo to jrhi of array "correl".
   The array "corri" contains the i value at which the best match for
   each j was found. Correl7 is hard coded to use 7x7 windows around
   the selected locations. */
{
   /* Correlate across selected position */
    int i, j, lasti, cptr, bestcorel, bestj, k;
    int Lsum, Rsum;    gray lefts[49];
    register int t, tsum, sum;  register gray *lptr, *rptr;
    
    /* set up source window in left image */
    Lsum = 0;  lptr = lefts;
    for (i = -3; i<=3; i++) for (j=-3; j<=3; j++)
      Lsum += *(lptr++) = left[i+ileft][j+jleft];
    
    /* Macros for building inline version of correlation window matching step */
#define PIXa   Rsum += (t = *(rptr++));  tsum += ISQ[*(lptr++) - t];
#define PIXb   Rsum += (t = *(rptr));  tsum += ISQ[*(lptr++) - t];
#define PIXc   Rsum += (t = *(++rptr));  tsum += ISQ[*(lptr++) - t];
#define PIXd   Rsum += (t = *(rptr += Pw-6));  tsum += ISQ[*(lptr++) - t];
#define R1c   PIXa PIXa PIXa PIXa PIXa PIXa PIXb  /* first window row */
#define RNc   PIXd PIXc PIXc PIXc PIXc PIXc PIXc  /* Other rows */
    
    /* censor, then scan destination area in right image */
    
    if (irlo>irhi) SWAP(irlo,irhi);
    if (jrlo>jrhi) SWAP(jrlo,jrhi);
    if (irlo<4) irlo=3;  if (irhi>Ph-4) irhi=Ph-4;
    if (jrlo<4) jrlo=3;  if (jrhi>Pw-4) jrhi=Pw-4;

    cptr = jrlo;   bestj = bestcorel = 1e9 + 1;
    for (j = jrlo; j <= jrhi; j++)
      {	
	  lasti = irlo;   sum = 1e9;
	  for (i = irlo; i <= irhi; i++)
	    {
		tsum = Rsum = 0;  lptr = lefts;
		rptr = &(right[i-3][j-3]);
		/* accumulate sum of squares of differences */
		R1c RNc RNc RNc RNc RNc RNc
		k = Lsum-Rsum;  if (k<0) k = -k;
		if (k < 49*20)
		  {
		      tsum -= ISQ[k]/49;
		      if (tsum<sum) {sum = tsum; lasti = i; }
		  };
	    };
	  correl[cptr] = sum;  corri[cptr++] = lasti;
	  if (sum<bestcorel) { bestcorel = sum;  bestj = j; };
      };
#undef PIXa
#undef PIXb
#undef PIXc
#undef PIXd
#undef R1c
#undef RNc
    return(bestj);
};


int FCorrel7(gray **leftmean, gray **rightmean,
	     gray **left, int ileft, int jleft, int Ph, int Pw,
	    gray **right, int irlo, int jrlo, int irhi, int jrhi,
	    int *correl, int *corri)
/* Scan image feature in "left" image centered on [ileft,jleft] across
   centers in "right" image rectangle bounded by [irlo,jrlo] [irhi,jrhi]
   inclusive.  The best correlation values for each j position, over all
   i positions are returned in indices jrlo to jrhi of array "correl".
   The array "corri" contains the i value at which the best match for
   each j was found. Correl7 is hard coded to use 7x7 windows around
   the selected locations. FCorrel7 eliminates windows whose averages
   differ more than 20 from the prototype */
{
   /* Correlate across selected position */
    int i, j, lasti, cptr, bestcorel, bestj, k, Lmn, sum;
    gray lefts[49];
    register int tsum;  register gray *lptr, *rptr;
    
    /* set up source window in left image */
    Lmn = leftmean[ileft][jleft];
    lptr = lefts;
    for (i = -3; i<=3; i++) for (j=-3; j<=3; j++)
      *(lptr++) = left[i+ileft][j+jleft];
    
    /* Macros for building inline version of correlation window matching step */
#define PIXa    tsum += ISQ[*(lptr++) -  *(rptr++)];
#define PIXb    tsum += ISQ[*(lptr++) - *(rptr)];
#define PIXc    tsum += ISQ[*(lptr++) - *(++rptr)];
#define PIXd    tsum += ISQ[*(lptr++) - *(rptr += Pw-6)];
#define R1c   PIXa PIXa PIXa PIXa PIXa PIXa PIXb  /* first window row */
#define RNc   PIXd PIXc PIXc PIXc PIXc PIXc PIXc  /* Other rows */
    
    /* censor, then scan destination area in right image */
    
    if (irlo>irhi) SWAP(irlo,irhi);  if (jrlo>jrhi) SWAP(jrlo,jrhi);
    if (irlo<4) irlo=3;  if (irhi>Ph-4) irhi=Ph-4;
    if (jrlo<4) jrlo=3;  if (jrhi>Pw-4) jrhi=Pw-4;

    cptr = jrlo;   bestj = bestcorel = 1e9 + 1;
    for (j = jrlo; j <= jrhi; j++)
      {	
	  lasti = irlo;   sum = 1e9;
	  for (i = irlo; i <= irhi; i++)
	    if ((k = rightmean[i][j] - Lmn) > -25 && k < 25) 
	      {
		  lptr = lefts;
		  rptr = &(right[i-3][j-3]);
		  /* accumulate sum of squares of differences */
		  tsum = -ISQ[k*7]; /* subtract out means */
		  R1c RNc RNc RNc RNc RNc RNc
		    if (tsum<sum) {sum = tsum; lasti = i; }
	      };
	  correl[cptr] = sum;  corri[cptr++] = lasti;
	  if (sum<bestcorel) { bestcorel = sum;  bestj = j; };
      };
#undef PIXa
#undef PIXb
#undef PIXc
#undef PIXd
#undef R1c
#undef RNc
    return(bestj);
};

int F2Correl7(unsigned Int16 *leftmean, unsigned Int16 *rightmean,
	     gray **left, int ileft, int jleft, int Ph, int Pw,
	    gray **right, int irlo, int jrlo, int jrhi, int *correl)
/* Scan image feature in "left" image centered on [ileft,jleft] across
   centers in "right" image rectangle bounded by [irlo,jrlo] [irlo,jrhi]
   inclusive.  The best correlation values for each j position are
   returned in indices jrlo to jrhi of array "correl".
   F2Correl7 is hard coded to use 7x7 windows around the selected locations,
   and eliminates windows whose averages differ more than 20 from the prototype */
{
   /* Correlate across selected position */
    int i, j, bestcorel, bestj, k, Lmn;
    unsigned short int *rmpt;
    gray lefts[49];
    register int tsum;  register gray *lptr, *rptr;
    
    /* set up source window in left image */
    Lmn = leftmean[ileft*Pw+jleft];
    lptr = lefts;
    for (i = -3; i<=3; i++) for (j=-3; j<=3; j++)
      *(lptr++) = left[i+ileft][j+jleft];
    
    /* Macros for building inline version of correlation window matching step */
#define PIXa    tsum += ISQ[*(lptr++) -  *(rptr++)];
#define PIXb    tsum += ISQ[*(lptr++) - *(rptr)];
#define PIXc    tsum += ISQ[*(lptr++) - *(++rptr)];
#define PIXd    tsum += ISQ[*(lptr++) - *(rptr += Pw-6)];
#define R1c   PIXa PIXa PIXa PIXa PIXa PIXa PIXb  /* first window row */
#define RNc   PIXd PIXc PIXc PIXc PIXc PIXc PIXc  /* Other rows */
    
    /* censor, then scan destination area in right image */
    
    if (jrlo>jrhi) SWAP(jrlo,jrhi);
    if (irlo<4) irlo=3;  if (irlo>Ph-4) irlo=Ph-4;
    if (jrlo<4) jrlo=3;  if (jrhi>Pw-4) jrhi=Pw-4;
#define meantol 25.75   /* 25.75 optimum for run1 data set */
    bestj = bestcorel = 1e9 + 1;  rmpt = &rightmean[irlo*Pw+jrlo];
    for (j = jrlo; j <= jrhi; j++)
      {	  tsum = 1e9;
	  if ((k = *(rmpt++) - Lmn) > -meantol*49 && k < meantol*49) 
	    {
		lptr = lefts;
		rptr = &(right[irlo-3][j-3]);
		/* accumulate sum of squares of differences */
		tsum = -((ISQ[k]+25)/49); /* subtract out means */
		R1c RNc RNc RNc RNc RNc RNc
		  if (tsum<bestcorel) { bestcorel = tsum;  bestj = j; };
	    };
	  correl[j] = tsum;
      };
#undef PIXa
#undef PIXb
#undef PIXc
#undef PIXd
#undef R1c
#undef RNc
    return(bestj);
};


int Correl6(gray **left, int ileft, int jleft, int Ph, int Pw,
	    gray **right, int irlo, int jrlo, int irhi, int jrhi,
	    int *correl, int *corri)
/* Scan image feature in "left" image centered on [ileft,jleft] across
   centers in "right" image rectangle bounded by [irlo,jrlo] [irhi,jrhi]
   inclusive.  The best correlation values for each j position, over all
   i positions are returned in indices jrlo to jrhi of array "correl".
   The array "corri" contains the i value at which the best match for
   each j was found. Correl6 is hard coded to use 8x8 windows around
   the selected locations. */
{
   /* Correlate across selected position */
    int i, j, lasti, cptr, bestcorel, bestj;
    int Lsum, Rsum;    gray lefts[36];
    register int t, tsum, sum;  register gray *lptr, *rptr;
    
    /* set up source window in left image */
    Lsum = 0;  lptr = lefts;
    for (i = -3; i<3; i++) for (j=-3; j<3; j++)
      Lsum += *(lptr++) = left[i+ileft][j+jleft];
    
    /* Macros for building inline version of correlation window matching step */
#define PIXa   Rsum += (t = *(rptr++));  tsum += ISQ[*(lptr++) - t];
#define PIXb   Rsum += (t = *(rptr));  tsum += ISQ[*(lptr++) - t];
#define PIXc   Rsum += (t = *(++rptr));  tsum += ISQ[*(lptr++) - t];
#define PIXd   Rsum += (t = *(rptr += Pw-5));  tsum += ISQ[*(lptr++) - t];
#define R1c   PIXa PIXa PIXa PIXa PIXa PIXb  /* first window row */
#define RNc   PIXd PIXc PIXc PIXc PIXc PIXc  /* Other rows */
    
    /* censor, then scan destination area in right image */
    
    if (irlo>irhi) SWAP(irlo,irhi);
    if (jrlo>jrhi) SWAP(jrlo,jrhi);
    if (irlo<3) irlo=3;  if (irhi>Ph-3) irhi=Ph-3;
    if (jrlo<3) jrlo=3;  if (jrhi>Pw-3) jrhi=Pw-3;

    cptr = jrlo;   bestj = bestcorel = 1e9;
    for (j = jrlo; j <= jrhi; j++)
      {	
	  lasti = sum = 1e9;
	  for (i = irlo; i <= irhi; i++)
	    {
		tsum = Rsum = 0;  lptr = lefts;
		rptr = &(right[i-3][j-3]);
		/* accumulate sum of squares of differences */
		R1c RNc RNc RNc RNc RNc
		  tsum -= ISQ[(Lsum-Rsum)/6]; /* subtract out means */
		if (tsum<sum) {sum = tsum; lasti = i; }
	    };
	  correl[cptr] = sum;  corri[cptr++] = lasti;
	  if (sum<bestcorel) { bestcorel = sum;  bestj = j; };
      };
#undef PIXa
#undef PIXb
#undef PIXc
#undef PIXd
#undef R1c
#undef RNc
    return(bestj);
};


int CorrelHills(double wtscale, int *correl, int sjl, int sjh,
		int *ppos, double *wt)
/* find and weight the peaks in a correlation curve
   wtscale is the distance scale, in pixel gray levels
   correl[sjl to sjh] is the raw correlation curve array - gets wiped
   ppos gets the j positions of the peaks,
   wt gets the normalized weights, 1.0 = perfect  */
{  
    double wtsum;    int jll, jul, bestj, bestcorrel, k, j, nwt;
    
    nwt = 0;  wtsum = 0;
    while (1) /* find, catalog and remove hills */
      {
	  bestj = -1;  bestcorrel = 1e9;  /* find peak */
	  for (j = sjl; j<=sjh; j++) if (correl[j]<bestcorrel)
	    {bestcorrel = correl[j];  bestj = j;};
	  if((ppos[nwt] = bestj)<0) {wt[nwt]=0; break;};  /* no more peaks? */
	  wtsum += (wt[nwt++] = EXP(-SQRT(correl[bestj]/49.0)/wtscale));
	  /* /49.0 if window is 7x7 */
	  jul = bestj+1; /* mark right slope */
	  while (jul<=sjh && correl[jul]<1e8 &&
		 correl[jul]>=correl[jul-1]) jul++;
	  jll = bestj-1; /* mark left slope */
	  while (jll>=sjl && correl[jll]<1e8 &&
		 correl[jll]>=correl[jll+1]) jll--;
	  if (jll<sjl) jll=sjl;  if (jul>sjh) jul=sjh;
	  for (j=jll; j<=jul; j++) correl[j]=1e9; /* remove whole hill */
      };
    
    k=0; while (ppos[k]>=0) wt[k++] /= wtsum;  /* normalize probabilities */
    
    return(nwt);
};