#include <stdio.h>
#include "/local/rsi/idl_5/external/export.h"
#include <math.h>

#define ABS(x) ((x) >= 0? (x): -(x))
#define MIN(x,y) ((x) < (y)? (x): (y))
static	IDL_LONG	badmatch;

int getmin(float *p, float *x0, float *y0);

IDL_VPTR ana_gridmatch(int argc, IDL_VPTR argv[])
/* the call is offsets = gridmatch(m1,m2,gx,gy,dx,dy,gwid)
	where	m1 = reference input image
	m2 = image to compare with m1, m1 and m2 must be same size
	gx = array of x gridpoints
	gy = array of y gridpoints, gx and gy must have same size
	dx and dy are the window size, and gwid is the gaussian mask width

  Authors:  Richard A. Shine (original)
            Louis H. Strous (port from ANA to IDL)
            Lockheed-Martin Advanced Technology Center, Palo Alto, CA, USA
*/
{
  static IDL_EZ_ARG arg_struct[] = {
    { IDL_EZ_DIM_MASK(2), IDL_TYP_B_SIMPLE, IDL_EZ_ACCESS_R, IDL_TYP_FLOAT,
      0, 0 },			/* m1: 2D FLOAT read-only array */
    { IDL_EZ_DIM_MASK(2), IDL_TYP_B_SIMPLE, IDL_EZ_ACCESS_R, IDL_TYP_FLOAT,
      0, 0 },			/* m2: 2D FLOAT read-only array */
    { IDL_EZ_DIM_MASK(2), IDL_TYP_B_SIMPLE, IDL_EZ_ACCESS_R, IDL_TYP_LONG,
      0, 0 },			/* gx: 2D LONG read-only array */
    { IDL_EZ_DIM_MASK(2), IDL_TYP_B_SIMPLE, IDL_EZ_ACCESS_R, IDL_TYP_LONG,
      0, 0 },			/* gy: 2D LONG read-only array */
    { IDL_EZ_DIM_MASK(0), IDL_TYP_B_SIMPLE, IDL_EZ_ACCESS_R, IDL_TYP_LONG,
      0, 0 },			/* dx: FLOAT read-only scalar */
    { IDL_EZ_DIM_MASK(0), IDL_TYP_B_SIMPLE, IDL_EZ_ACCESS_R, IDL_TYP_LONG,
      0, 0 },			/* dy: FLOAT read-only scalar */
    { IDL_EZ_DIM_MASK(0), IDL_TYP_B_SIMPLE, IDL_EZ_ACCESS_R, IDL_TYP_FLOAT,
      0, 0 }			/* gwid: FLOAT read-only scalar */
  };
  IDL_VPTR	result;
  IDL_VARIABLE	zero;
  float	*out, *p1, *p2, *gwx, *gwy, xoffset, yoffset, gwid;
  IDL_LONG	dims[IDL_MAX_ARRAY_DIM], *gx, *gy, nx, ny, nxg, nyg, dx2, dy2,
    nc, dx, dy, i1, i2, j1, j2;
  void	gwind0(float *, float *, float, IDL_LONG, IDL_LONG, IDL_LONG,
	       IDL_LONG),
    match_1(float *, float *, IDL_LONG, IDL_LONG, IDL_LONG, IDL_LONG,
	    IDL_LONG, IDL_LONG, float *, float *, float *, float *);

  IDL_EzCall(argc, argv, arg_struct);
  
  /* the dimensions of m1 and m2 must be equal */
  nx = arg_struct[0].value.arr->dim[0];
  ny = arg_struct[0].value.arr->dim[1];
  if (arg_struct[1].value.arr->dim[0] != nx
      || arg_struct[1].value.arr->dim[1] != ny) {
    IDL_Message(IDL_M_NAMED_GENERIC, IDL_MSG_RET,
		"First two arguments (images) have unequal dimensions");
    IDL_EzCallCleanup(argc, argv, arg_struct);
    zero.type = IDL_TYP_INT;
    zero.value.i = 0;
    result = &zero;
    return result;
  }

  p1 = (float *) arg_struct[0].value.arr->data;
  p2 = (float *) arg_struct[1].value.arr->data;

  /* the dimensions of gx and gy must be equal */
  nxg = arg_struct[3].value.arr->dim[0];
  nyg = arg_struct[3].value.arr->dim[1];
  if (arg_struct[3].value.arr->dim[0] != nxg
      || arg_struct[3].value.arr->dim[1] != nyg) {
    IDL_Message(IDL_M_NAMED_GENERIC, IDL_MSG_RET,
		"3rd and 4th arguments (grid points) have unequal dimensions");
    IDL_EzCallCleanup(argc, argv, arg_struct);
    zero.type = IDL_TYP_INT;
    zero.value.i = 0;
    result = &zero;
    return result;
  }

  gx = (IDL_LONG *) arg_struct[2].value.arr->data;
  gy = (IDL_LONG *) arg_struct[3].value.arr->data;

  dx = arg_struct[4].value.l;
  dy = arg_struct[5].value.l;
  gwid = arg_struct[6].value.f;

  /* prepare output symbol: It has the same dimensions as <gx> with an */
  /* extra dimension of 2 prepended. */
  dims[0] = 2;
  memcpy(dims + 1, arg_struct[2].value.arr->dim, 2*sizeof(IDL_LONG));
  out = (float *) IDL_MakeTempArray(IDL_TYP_FLOAT, 3, dims,
				    IDL_BARR_INI_ZERO, &result);

  /* prepare the gaussian kernel */
  gwx = (float *) malloc((nx + ny)*sizeof(float));
  if (!gwx) {
    IDL_Message(IDL_M_NAMED_GENERIC, IDL_MSG_RET, 
		"Could not allocate memory for the gaussian kernel");
    IDL_EzCallCleanup(argc, argv, arg_struct);
    zero.type = IDL_TYP_INT;
    zero.value.i = 0;
    result = &zero;
    return result;
  }
  gwy = gwx + nx;
  nc = nxg*nyg;			/* the number of subimages */
  dx2 = dx/2;
  dy2 = dy/2;
  badmatch = 0;
  while (nc--) {		/* loop over all subimages */
    i1 = *gx - dx2;		/* lower x coordinate */
    if (i1 < 0)			/* outside array? */
      i1 = 0;
    i1++;
    i2 = *gx++ + dx2;		/* upper x coordinate */
    if (i2 > nx)
      i2 = nx;
    j1 = *gy - dy2;		/* lower y coordinate */
    if (j1 < 0)
      j1 = 0;
    j1++;
    j2 = *gy++ + dy2;		/* upper y coordinate */
    if (j2 > ny)
      j2 = ny;
    xoffset = yoffset = 0;
    i1--; i2--; j1--; j2--;
    gwind0(gwx, gwy, gwid, i1, i2, j1, j2); /* get gaussian kernels */
    match_1(p1, p2, i1, i2, j1, j2, nx, ny, gwx, gwy,
	    &xoffset, &yoffset); /* get offsets */
    *out++ = xoffset;
    *out++ = yoffset;
  }

  if (badmatch)
    printf("GRIDMATCH - %d bad matches\n", badmatch);

  free(gwx);
  IDL_EzCallCleanup(argc, argv, arg_struct);
  return result;
}
/*-----------------------------------------------------------------*/
void gwind0(float *gwx, float *gwy, float gwid, IDL_LONG nxa, IDL_LONG nxb,
	    IDL_LONG nya, IDL_LONG nyb)
{
  float	wid, xcen, ycen, xq;
  IDL_LONG	i;
  
  wid = gwid*0.6005612;
  if (wid > 0) {
    xcen = (nxa + nxb)/2;
    ycen = (nya + nyb)/2; 
    for (i = nxa; i <= nxb; i++) {
      xq = (i - xcen)/wid;
      gwx[i] = exp(-(xq*xq));
    } 
    for (i = nya; i <= nyb; i++) {
      xq = (i - ycen)/wid;
      gwy[i] = exp(-(xq * xq));
    } 
  } else {
    for (i = nxa; i <= nxb; i++) 
      gwx[i] = 1.0;
    for (i = nya; i <= nyb; i++)
      gwy[i] = 1.0; 
  }
}
/*-----------------------------------------------------------------*/
void match_1(float *p1, float *p2, IDL_LONG nxa, IDL_LONG nxb,
	     IDL_LONG nya, IDL_LONG nyb, IDL_LONG nx, IDL_LONG ny,
	     float *gwx, float *gwy, float *xoffset, float *yoffset)
{
  IDL_LONG	idelx, idely, i, j, k, ndmx = 1000, done[9];
  IDL_LONG	di, dj, in, jn, iter, dd, badflag = 0;
  float	av1, av2, cx, cy, cxx, cxy, cyy, avdif, t, res[9], buf[9], t1, t2;
  static IDL_LONG	itmax = 20;
  static IDL_LONG	stretch_clip = 19;
  void unbias(float *m1, float *m2, IDL_LONG nxa, IDL_LONG nxb,
	      IDL_LONG nya, IDL_LONG nyb, IDL_LONG nxs, IDL_LONG nys,
	      float *gx, float *gy, float *av1, float *av2, float *cx,
	      float *cy, float *cxx, float *cxy, float *cyy,
	      IDL_LONG idelx, IDL_LONG idely);
  float resid(float *m1, float *m2, IDL_LONG idx, IDL_LONG idy, IDL_LONG nxa,
	      IDL_LONG nxb, IDL_LONG nya, IDL_LONG nyb, IDL_LONG nxs,
	      IDL_LONG nys, IDL_LONG ndmx, float *gx, float *gy, float bs);

  for (i = 0; i < 9; i++)
    done[i] = 0;
  idelx = rint(*xoffset);
  idely = rint(*yoffset); 
  unbias(p1, p2, nxa, nxb, nya, nyb, nx, ny, gwx, gwy, &av1, &av2, &cx, &cy,
	 &cxx, &cxy, &cyy, idelx, idely);
  /* look at a 3x3 matrix of residuals centered at 0 offset, find the location
     of the minimum, if not at center, then look at new 3x3 centered
	on the edge minimum; repeat until min at center */
  iter = itmax;
  badflag = 0;
  while (iter--) {
    for (k = 0; k < 9; k++) {
      if (done[k] == 0) {
	i = idelx + (k % 3) - 1;
	j = idely + (k / 3) - 1;
	avdif = av2 +  i*cx + j*cy + i*i*cxx + i*j*cxy + j*j*cyy - av1;
	res[k] = resid(p1, p2, i, j, nxa, nxb, nya, nyb, nx, ny, ndmx,
		       gwx, gwy, avdif);
      }
    }
    t = res[0];
    i = 0;
    for (k = 1; k < 9; k++) 
      if (res[k] < t) {
	t = res[k];
	i = k;
      }
    if (t < 0) {		/* added LS 19feb95 */
      printf("match - ran out of data at edge\n");
      badflag = 1;
      break;
    }
    idelx += (i % 3) - 1;
    idely += (i / 3) - 1;
    /* check if we have gone too far */
    if (ABS(idelx) > stretch_clip || ABS(idely) > stretch_clip) {
      printf("match - stretch_clip exceeded\n");
      badflag = 1;
      break;
    }
    if (i == 4)
      break;			/* done if it is the center one */
    /* not in center, shuffle what we have to put the edge min in center */
    di = (i % 3) - 1;
    dj = (i / 3) - 1;
    dd = dj * 3 + di;
    for (k = 0; k < 9; k++) {
      in = k%3 + di;
      jn = k/3 + dj;
      if (in >= 0 && jn >= 0 && in < 3 && jn < 3) { /* in range */
	done[k] = 1;
	buf[k] = res[k + dd];
      } else 
	done[k] = 0;		/* not in range, mark undone */
    }
    for (k = 0; k < 9; k++)
      res[k] = buf[k];		/* put back in res array */
  } /* end of iter while */
  /* done or reached itmax, which ? */
  if (iter <= 0) {
    printf("match - exceeded maximum iteration\n");
    badflag = 1;
  }
  if (badflag) {
    printf("cell index range = %d %d %d %d\n", nxa, nxb, nya, nyb);
    badmatch++;
    *xoffset = *yoffset = 0;
    return;
  }
				/* must have been OK so far */
  getmin(res, &t1, &t2);
  *xoffset = idelx + t1;
  *yoffset = idely + t2;
}
/*------------------------------------------------------------------------- */
void unbias(float *m1, float *m2, IDL_LONG nxa, IDL_LONG nxb,
	    IDL_LONG nya, IDL_LONG nyb, IDL_LONG nxs, IDL_LONG nys,
	    float *gx, float *gy, float *av1, float *av2, float *cx,
	    float *cy, float *cxx, float *cxy, float *cyy,
	    IDL_LONG idelx, IDL_LONG idely)
{
  float	t0, t1, t2, t3, t4, t5;
  float averag(float *p, IDL_LONG nxa, IDL_LONG nxb, IDL_LONG nya,
	       IDL_LONG nyb, IDL_LONG nxs, IDL_LONG nys, IDL_LONG idx,
	       IDL_LONG idy, float *gx, float *gy);

  /*  find weighted means of m1 & m2 over the window 
      sets up quadratic fit to average of m2 as a fcn. of offsets */
  *av1 = averag(m1, nxa, nxb, nya, nyb, nxs, nys, 0, 0, gx, gy); 
  t0 = averag(m2, nxa, nxb, nya, nyb, nxs, nys, idelx, idely, gx, gy); 
  t1 = averag(m2, nxa, nxb, nya, nyb, nxs, nys, idelx + 1, idely, gx, gy); 
  t2 = averag(m2, nxa, nxb, nya, nyb, nxs, nys, idelx - 1, idely, gx, gy); 
  t3 = averag(m2, nxa, nxb, nya, nyb, nxs, nys, idelx, idely + 1, gx, gy); 
  t4 = averag(m2, nxa, nxb, nya, nyb, nxs, nys, idelx, idely - 1, gx, gy); 
  t5 = averag(m2, nxa, nxb, nya, nyb, nxs, nys, idelx + 1, idely + 1, gx, gy); 
  *av2 = t0; 
  *cx = 0.5*(t1 - t2); 
  *cy = 0.5*(t3 - t4); 
  *cxx = 0.5*(t1 - 2*t0 + t2); 
  *cyy = 0.5*(t3 - 2*t0 + t4); 
  *cxy = t5 + t0 - t1 - t3;
}
/*------------------------------------------------------------------------- */
float averag(float *p, IDL_LONG nxa, IDL_LONG nxb, IDL_LONG nya,
	     IDL_LONG nyb, IDL_LONG nxs, IDL_LONG nys, IDL_LONG idx,
	     IDL_LONG idy, float *gx, float *gy)
/* finds weighted average of array m over the block defined */
{
  IDL_LONG	nxc, nxd, nyc, nyd, i, j, jj;
  float	sum, sumg, sumx, sumgx;

  /* fix limits so sum doesn't run off edge of image */
  nxc = (nxa + idx < 0)? -idx: nxa;
  nyc = (nya + idy < 0)? -idy: nya;
  nxd = (nxb + idx > nxs)? nxs - idx: nxb;
  nyd = (nyb + idy > nys)? nys - idy: nyb;
  sum = sumg = sumgx = 0.0; 
  for (i = nxc; i < nxd; i++)
    sumgx += gx[i];
  /* weighted sum in window */
  for (j = nyc; j < nyd; j++) {
    sumx = 0.0;
    jj = idx + nxs*(j + idy);
    for (i = nxc; i < nxd; i++)
      sumx += gx[i]*p[i + jj];
    sum += gy[j]*sumx;
    sumg += gy[j]*sumgx;
  } /* end of j loop */
  return sum/sumg;
}
/*------------------------------------------------------------------------- */
float resid(float *m1, float *m2, IDL_LONG idx, IDL_LONG idy, IDL_LONG nxa,
	    IDL_LONG nxb, IDL_LONG nya, IDL_LONG nyb, IDL_LONG nxs,
	    IDL_LONG nys, IDL_LONG ndmx, float *gx, float *gy, float bs)
{
  IDL_LONG	nxc, nxd, nyc, nyd, nx, ny;
  float	*p1, *p2, *ps;
  float	sum, sumx, t, ndmx2;
  IDL_LONG	i, j;
  float   sumg;
  static IDL_LONG	mxc, mxd, myc, myd;
  static float	gsum;

  /*set up limits */
  nxc = nxa;
  if (nxc + idx < 0)
    nxc = -idx;
  nyc = nya;
  if (nyc + idy < 0)
    nyc = - idy;
  nxd = nxb;
  if (nxd + idx >= nxs)
    nxd = nxs - idx - 1;
  nyd = nyb;
  if (nyd + idy >= nys)
    nyd = nys - idy - 1;
  sum = sumg = 0.0;

  nx = nxd - nxc +1;
  p2 = gy + nyc;
  ps = gx + nxc;

  if (nxc != mxc || nxd != mxd || nyc != myc || nyd != myd) {
    /* sum gaussians over rectangle to get normalization */
    /* (only if limits change)*/
    j = nyd -nyc + 1;
    if (j <= 0 || nxd - nxc + 1 <= 0)
      return -1;		/* added 19feb95 LS */
    while (j) {
      i = nx;
      p1 = ps;
      while (i) {
	sumg += (*p1++) * (*p2);
	i--;
      }
      p2++;
      j--;
    }
    gsum = sumg;
    mxc = nxc;
    mxd = nxd;
    myc = nyc;
    myd = nyd;
  } else
    sumg = gsum;

  m1 += nyc*nxs + nxc;
  m2 += (nyc + idy)*nxs + nxc + idx;
  ny = nxs - nx;		/*residual increment after inner loop */
  p2 = gy + nyc;
  
  /* now the loop to compute the residual */
  j = nyd - nyc +1;
  ndmx2 = ndmx*ndmx;
  while (j) {
    i = nx;
    p1 = ps;
    sumx = 0.0;
    while (i) {
      t = *m1++ - *m2++;
      t = t + bs;
      t = t*t;
      t = MIN(t, ndmx2);
      sumx += (*p1++) * t;
      i--;
    }
    sum += (*p2++) * sumx;
    m1 += ny;
    m2 += ny;
    j--;
  }
  /*return normalized residual */
  return sum/sumg;
}
/*------------------------------------------------------------------------- */
int getmin(float *p, float *x0, float *y0)
{
  float	f11, f12, f13, f21, f22, f23, f31, f32, f33;
  float	fx, fy, t, fxx, fyy, fxy;

  /* find the min, p points to a 3x3 array */
  f11 = *p++;	f21 = *p++;	f31 = *p++;
  f12 = *p++;	f22 = *p++;	f32 = *p++;
  f13 = *p++;	f23 = *p++;	f33 = *p++;

  fx = 0.5 * ( f32 - f12 );	fy = 0.5 * ( f23 - f21 );
  t = 2.* ( f22 );
  fxx =  f32 + f12 - t;	fyy = f23 + f21 - t;
  /* find in which quadrant the minimum lies */
  if (f33 < f11) {
    if (f33 < f31) {
      if (f33 < f13)
	fxy = f33+f22-f32-f23;
      else
	fxy = f23+f12-f22-f13;
    } else {
      if (f31 < f13)
	fxy = f32+f21-f31-f22;
      else
	fxy = f23+f12-f22-f13;
    }
  } else { 
    if (f11 < f31) {
      if (f11 < f13) 
	fxy = f22+f11-f21-f12;
      else
	fxy = f23+f12-f22-f13;
    } else {
      if (f31 < f13)
	fxy = f32+f21-f31-f22;
      else
	fxy = f23+f12-f22-f13;
    }
  }
  t = -1./(fxx *fyy - fxy *fxy);
  *x0 = t * (fx * fyy - fy * fxy);
  *y0 = t * (fy * fxx - fx * fxy);
  if (ABS(*x0) >= 0.75 || ABS(*x0) >= 0.75) {
    *x0 = -fx/fxx;
    *y0 = -fy/fyy;
  }
  return 1;
}
/*------------------------------------------------------------------------- */