/* modified version of shawn beckers program ``img_warp.c'';
   for pchirp.c ; ongoing project of shawn and i */

/* reversed v and h from  vH+h to hV+v (consistent with ordering typical
   of arrays in FORTRAN) */

/* removed pointer to function from argument list */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "basic.h"
#include "img.h"
#include "math2d.h"
#include "dst_to_src_rechirp2.h"

/* all point aggregate types are in row column matrix order 
 * i.e. a[0][0] a[0][1] a[0][2]
 *      a[1][0] a[1][1] a[1][2]
 *      a[2][0] a[2][1] a[2][2]
 */
typedef point_type rect_type[2][2]; /* rectangle */
typedef point_type quad_type[2][2]; /* quadralateral */
typedef point_type hood_type[3][3]; /* uniform or resampled grid */

/* the value of all source pixels outside the given source image */
static double background_grey=0;/*BACKGROUND value: NaN NAN nan black default*/

/******** LOCAL VARIABLES and CONSTANTS ********/

/* describe the current src and dst images, point and neighborhood */
static image_type src, dst;
static  hood_type src_nbrhd;
static point_type src_pnt, dst_pnt;

/******** LOCAL FUNCTIONS ********/

static double interp_1(double src_v, double src_h);
static double interp_3(double src_v, double src_h);
static double sample_1(double half_width);
static double sample_4(double avg_rad);
static double sample_8(void);
static double resample_9(void);
static double resample_scatter(point_type *rand_delta_dst, int L);
static double resample_uniform(double cutoff_M);

/* sums up the values of all pixels contained in the given src quadralateral 
 */
static void scan_convert_src_area(quad_type src_quad,
				  double *sum,
				  long *cnt );

/* initialize the src pixel locations for the 3x3 neighborhood
 * of dst pixels and return the average radius, or zero if outside src.
 */
static double init_src_nbrhd(void);

#ifdef __GNUC__
inline double get_src( int v, int h )
#else
double get_src( int v, int h )
#endif
{
  printf("img_resample;get_src: v=%d, h=%d\n",v,h);
  printf("img_resample;get_src says NaN=%g\n",background_grey);
  if( INSIDE(src.vdim,src.hdim,v,h) )
    return src.start[h*src.stride + v];
  else
    return background_grey;
    printf("img_resample INSIDE says that NaN=%g\n",background_grey);
}


/* a general function intended to handle any type of
 * image transformation definable by a dst_to_src 2d resampling
 * function. very special care is taken to properly handle
 * various degrees of subpixel interpolation and various levels of 
 * macro pixel antialiasing where required.
 */
void img_resample(double *src_start,  /* source image */
                  int src_stride, int src_vdim, int src_hdim,
	          int interp_order, int sample_level,
	          double *dst_start,  /* destination image */
	          int dst_stride, int dst_vdim, int dst_hdim,
	          double background_grey)
{
  double val, avg_rad;

  /* initialize the global placeholders 
   */
  src.start = src_start;
  src.stride = src_stride;
  src.vdim = src_vdim;
  src.hdim = src_hdim;

  dst.start = dst_start;
  dst.stride = dst_stride;
  dst.vdim = dst_vdim;
  dst.hdim = dst_hdim;

  printf("img_resample says that NaN=%g\n",background_grey);

  for( dst_pnt.v=0; dst_pnt.v<dst.vdim; dst_pnt.v++ ) {
    fprintf(stderr,"doing raster %d of %d\r", (int)dst_pnt.v, dst.vdim);
    for( dst_pnt.h=0; dst_pnt.h<dst.hdim; dst_pnt.h++ ) {

      /* get the src pixel for this dst pixel */
      dst_to_src_once_per_pixel( (int)dst_pnt.v, (int)dst_pnt.h, &src_pnt.v, &src_pnt.h);

      /* default zero order/level interpolation/sampling */
      val = get_src(RND(src_pnt.v),RND(src_pnt.h));

      /* interpolation only */
      if( sample_level < 0 ) {
	switch( interp_order ) {
	case 3:
	  val = interp_3(src_pnt.v, src_pnt.h);
	  break;
	case 1:
	  val = interp_1(src_pnt.v, src_pnt.h);
	  break;
	}
      }

      /* all orders/levels of interpolation/sampling */
      else if( (interp_order > 0) || (sample_level > 0) ) {

	/* find the stretch or shrinkage for this neighborhood */
	avg_rad = init_src_nbrhd();

	/* if the neighborhood is completely outside the src
	 * then radius will be zero, so use the zero order value 
	 */
	if( avg_rad > 0.0 ) {
	  
	  /* need to interpolate */
	  if( avg_rad < 1.0 ) {
	    switch( interp_order ) {
	    case 3:
	      val = interp_3(src_pnt.v, src_pnt.h);
	      break;
	    case 1:
	      val = interp_1(src_pnt.v, src_pnt.h);
	      break;
	    }
	  }
	  /* need to sample */
	  else {
	    switch( sample_level ) {
	    case 8:
	      val = sample_8();
	      break;
	    case 4:
	      val = sample_4(avg_rad);
	      break;
	    case 1:
	      val = sample_1(avg_rad);
	      break;
	    }
	  }
	}
      }

      /* dst.start[(int)dst_pnt.v*dst.stride+(int)dst_pnt.h] = val; */
      dst.start[(int)dst_pnt.h*dst.stride+(int)dst_pnt.v] = val;
    }
  }
}


/* bilinearly interpolate within the src neighborhood
 */
static double interp_1(double src_v, double src_h)
{
  rect_type ref_rect;
  point_type src_point;
  int floor_src_v, floor_src_h, ceiling_src_v, ceiling_src_h;
  
  floor_src_v = floor(src_v);
  floor_src_h = floor(src_h);
  ceiling_src_v = floor_src_v + 1;
  ceiling_src_h = floor_src_h + 1;
  
  ref_rect[0][0].v = floor_src_v;
  ref_rect[0][0].h = floor_src_h;
  ref_rect[0][0].d[0] = get_src(floor_src_v,floor_src_h);
  printf("img_resample;interp_1: floor,floor get_src=%g\n",get_src(floor_src_v,floor_src_h));

  ref_rect[0][1].v = floor_src_v;
  ref_rect[0][1].h = ceiling_src_h;
  ref_rect[0][1].d[0] = get_src(floor_src_v,ceiling_src_h);
  printf("img_resample;interp_1: floor,ceiling get_src=%g\n",get_src(floor_src_v,ceiling_src_h));
  
  ref_rect[1][0].v = ceiling_src_v;
  ref_rect[1][0].h = floor_src_h;
  ref_rect[1][0].d[0] = get_src(ceiling_src_v,floor_src_h);
  printf("img_resample;interp_1: celingH,floor get_src=%g\n",get_src(ceiling_src_v,floor_src_h));
  
  ref_rect[1][1].v = ceiling_src_v;
  ref_rect[1][1].h = ceiling_src_h;
  ref_rect[1][1].d[0] = get_src(ceiling_src_v,ceiling_src_h);
  printf("img_resample;interp_1: celingH,ceiling get_src=%g\n",get_src(ceiling_src_v,ceiling_src_h));
  
  src_point.v = src_v;
  src_point.h = src_h;
  
  bilinear_interp( ref_rect, &src_point, 1);
  
  return src_point.d[0];
}

/* cubic interpolation
 */
static double interp_3(double src_v, double src_h)
{
  int base_v, base_h, v, h;
  point_type cubic_matrix[4][4];
  point_type src_point;
  
  /* setup the matrix used to cubic interpolate at
   * this src pixel 
   */
  base_v = floor(src_v);
  base_h = floor(src_h);
  for( v = -1; v<3; v++ ) {
    for( h = -1; h<3; h++ ) {
      cubic_matrix[v+1][h+1].v = v;
      cubic_matrix[v+1][h+1].h = h;
      cubic_matrix[v+1][h+1].d[0] = get_src((base_v+v),(base_h+h));
    }
  }
  src_point.v = src_v-base_v;
  src_point.h = src_h-base_h;
  
  bicubic_interp( cubic_matrix, &src_point, 1);
  
  return src_point.d[0];
}

/* average all pixel values over a square of given half width
 */
static double sample_1(double half_width)
{
  double total;
  int v, h, r, cnt, sv,sh;
  
  total = 0.0;
  cnt = 0;
  sv = RND( src_pnt.v );
  sh = RND( src_pnt.h );
  r =  RND(half_width);
  for( v=(sv-r); v<=(sv+r); v++ ) {
    for( h=(sh-r); h<=(sh+r); h++ ) {
      if( INSIDE( src.vdim, src.hdim, v,h) ) {
	total += get_src(v,h);
	cnt++;
      }
    }
  }
  if( cnt > 0 )
    return total/(double)cnt;
  else
    return background_grey;
    printf("img_resample sample_1 says that NaN=%g\n",background_grey);
}

/* average all pixel values over the inverse quadralateral of the current
 * src pixel defined by the averages of the neighboring 4-groups.
 * ASSUMES the src_nbrhd has been initialized.
 */
static double sample_4(double avg_rad)
{
  int dv, dh, v, h;
  double total_sum, v_tot, h_tot;
  long total_cnt;
  quad_type src_quad;
  
  /* quadralateral sampling means find the average intensity
   * over the bounding neighborhood described by a quadralateral.
   * each dst pixel maps to a src quadralateral that linearly separates 
   * its data from a neighboring dst pixel mapped to src. 
   */

  /* average the 4 4-groups of points to find the quadralateral vertices.
   */
  for( dv=0; dv<2; dv++ ) {   /* the dv,dh 'th 4-group */
    for( dh=0; dh<2; dh++ ) {
      v_tot = 0.0;
      h_tot = 0.0;
      for( v=0; v<2; v++ ) {  /* the v,h 'th vertex of this 4-group */
	for( h=0; h<2; h++ ) {
	  v_tot += src_nbrhd[dv+v][dh+h].v;
	  h_tot += src_nbrhd[dv+v][dh+h].h;
	}
      }
      src_quad[dv][dh].v = v_tot/4.0;
      src_quad[dv][dh].h = h_tot/4.0;
    }
  }
  
  /* find summed intensity and pixel count over 
   * the src quadralateral
   */
  scan_convert_src_area( src_quad, &total_sum, &total_cnt );

  if( total_cnt > 0 )
    return total_sum/(double)total_cnt;
  else
    return background_grey;
    printf("img_resample, scan convert says that NaN=%g\n",background_grey);
}

/* average all pixel values over the inverse octogon of the current
 * src pixel. This is done by finding the 4 quadralaterals defined by
 * the src pixel, averages of the neighboring 4-groups, and the dx,dy
 * edge pairs.
 * ASSUMES the src_nbrhd has been initialized.
 */
static double sample_8()
{
  int dv, dh, v, h;
  double sum, total_sum;
  long cnt, total_cnt;
  quad_type sub_quad;
  rect_type ref_rect;
  point_type src_point;
  
  /* octogon sampling means find the average intensity
   * over the exact bounding neighborhood.
   * each dst pixel maps to a src octogon that linearly separates 
   * its data from a neighboring dst pixel mapped to src. The octogon
   * is described as 4 quadralaterals. Find the average intensity
   * within these quadralaterals.
   */
  
  /* initialize the 4 h,v coordinates of the 
   * reference quadralateral to unit coordinates 
   * in preparation for bilinear interpolation.
   */
  for( v=0; v<2; v++ ) {
    for( h=0; h<2; h++ ) {
      ref_rect[v][h].v = (double)v;
      ref_rect[v][h].h = (double)h;
    }
  }
  
  total_sum = 0.0;
  total_cnt = 0;
  
  /* define each of the 4 dv,dh sub quadralaterals that describe
   * the octogon, and sum up the total intensity sum and
   * pixel counts.
   */
  for( dv=0; dv<2; dv++ ) {
    for( dh=0; dh<2; dh++ ) {
      
      /* set 2d values of the 4 corners of the reference 
       * quadralateral to be the actual coordinates of 
       * the 4 src neighbor points that surround the 
       * dv,dh sub quadralateral.
       */
      for( v=0; v<2; v++ ) {
	for( h=0; h<2; h++ ) {
	  ref_rect[v][h].d[0] = src_nbrhd[dv+v][dh+h].v;
	  ref_rect[v][h].d[1] = src_nbrhd[dv+v][dh+h].h;
	}
      }
      
      /* interpolate 2d values at 4 reference points within
       * the reference quadralateral. These coordinates make the
       * reference points half way to the nearest reference rectangle
       * vertex. values for each of the 4 reference point are the 
       * src coordinates for the dv,dh sub quadralateral's 4 v,h 
       * vertices.
       * (these vertex coordinates are simply h,v averages between
       * groups of 4, 2 or 1 points in the source neighborhood. 
       * calculation could have been done explicitly for each 
       * different vertex of each sub quadralateral, but using bilinear
       * interpolation with a general formulation is 'more elegant'.
       */
      for( v=0; v<2; v++ ) {
	for( h=0; h<2; h++ ) {
	  src_point.v = (1-dv)*0.5 + v*0.5;
	  src_point.h = (1-dh)*0.5 + h*0.5;
	  bilinear_interp( ref_rect, &src_point, 2 );
	  sub_quad[v][h].v = src_point.d[0];
	  sub_quad[v][h].h = src_point.d[1];
	}
      }
      
      /* find summed intensity and pixel count over 
       * the dv,dh sub quadralateral */
      scan_convert_src_area( sub_quad, &sum, &cnt );
      
      total_sum += sum;
      total_cnt += cnt;
      
    } /* dh */
  } /* dv */
  
  if( total_cnt > 0 )
    return total_sum/(double)total_cnt;
  else
    return background_grey;
    printf("img_resample, scan convert says that NaN=%g\n",background_grey);
}

/* map a std_dev = 1/3 gaussian to the current dst pixel,
 * and use these as contribution weights for pixels from the src.
 *
 * Here's the matlab script
 *
   E = [1/9 0; 0 1/9];
   u = [0 0]';
   x_ctr = [0 0]';
   x_side = [0 0.5]';
   x_cnr = [0.5 0.5]';
   f_ctr = 1/((2*pi)*det(E)^0.5)*exp((-0.5)*(x_ctr-u)'*inv(E)*(x_ctr-u) );
   f_side = 1/((2*pi)*det(E)^0.5)*exp((-0.5)*(x_side-u)'*inv(E)*(x_side-u) );
   f_cnr = 1/((2*pi)*det(E)^0.5)*exp((-0.5)*(x_cnr-u)'*inv(E)*(x_cnr-u) );
   total = f_ctr + 4*(f_side + f_cnr);
   g_ctr = f_ctr/total
   g_side = f_side/total
   g_cnr = f_cnr/total
 *
 * f(x) = a point on a 2-D std_dev = 1/3 gaussian surface
 * g(x) = f(x) normalized so that all discrete samples of f(x) sum to 1.0
 * 
 * ASSUMES src_nbrhd has been initialized.
 */
static double resample_9()
{
  int v,h;
  double total;
  hood_type new_nbr;

#define g_ctr  0.3676
#define g_side 0.1193
#define g_cnr  0.0387

  /* now create a "new" neighborhood which defines the boundary between this
   * src pixel and all adjoining ones. Assign the "value" of each to be
   * the contribution of this dst subpixel's to the unit dst gaussian.
   */

  /* the corners */
  new_nbr[0][0].v = (src_nbrhd[0][0].v+src_nbrhd[0][1].v +
		     src_nbrhd[1][0].v+src_nbrhd[1][1].v)/4.0;
  new_nbr[0][2].v = (src_nbrhd[0][1].v+src_nbrhd[0][2].v +
		     src_nbrhd[1][1].v+src_nbrhd[1][2].v)/4.0;
  new_nbr[2][0].v = (src_nbrhd[1][0].v+src_nbrhd[1][1].v +
		     src_nbrhd[2][0].v+src_nbrhd[2][1].v)/4.0;
  new_nbr[2][2].v = (src_nbrhd[1][1].v+src_nbrhd[1][2].v +
		     src_nbrhd[2][1].v+src_nbrhd[2][2].v)/4.0;

  new_nbr[0][0].h = (src_nbrhd[0][0].h+src_nbrhd[0][1].h +
		     src_nbrhd[1][0].h+src_nbrhd[1][1].h)/4.0;
  new_nbr[0][2].h = (src_nbrhd[0][1].h+src_nbrhd[0][2].h +
		     src_nbrhd[1][1].h+src_nbrhd[1][2].h)/4.0;
  new_nbr[2][0].h = (src_nbrhd[1][0].h+src_nbrhd[1][1].h +
		     src_nbrhd[2][0].h+src_nbrhd[2][1].h)/4.0;
  new_nbr[2][2].h = (src_nbrhd[1][1].h+src_nbrhd[1][2].h +
		     src_nbrhd[2][1].h+src_nbrhd[2][2].h)/4.0;

  new_nbr[0][0].d[0] = new_nbr[0][2].d[0] = 
    new_nbr[2][0].d[0] = new_nbr[2][2].d[0] = g_cnr;

  /* the sides */
  new_nbr[0][1].v = (src_nbrhd[0][1].v+src_nbrhd[1][1].v)/2.0;
  new_nbr[1][0].v = (src_nbrhd[1][0].v+src_nbrhd[1][1].v)/2.0;
  new_nbr[1][2].v = (src_nbrhd[1][2].v+src_nbrhd[1][1].v)/2.0;
  new_nbr[2][1].v = (src_nbrhd[2][1].v+src_nbrhd[1][1].v)/2.0;

  new_nbr[0][1].h = (src_nbrhd[0][1].h+src_nbrhd[1][1].h)/2.0;
  new_nbr[1][0].h = (src_nbrhd[1][0].h+src_nbrhd[1][1].h)/2.0;
  new_nbr[1][2].h = (src_nbrhd[1][2].h+src_nbrhd[1][1].h)/2.0;
  new_nbr[2][1].h = (src_nbrhd[2][1].h+src_nbrhd[1][1].h)/2.0;

  new_nbr[0][1].d[0] = new_nbr[1][0].d[0] = 
    new_nbr[1][2].d[0] = new_nbr[2][1].d[0] = g_side;

  /* the center */
  new_nbr[1][1].v = src_nbrhd[1][1].v;
  new_nbr[1][1].h = src_nbrhd[1][1].h;
  new_nbr[1][1].d[0] = g_ctr;

  /* now bilinearly interpolate the value at each of the src nbrhood subpts
   * and weight that subpixel's value to the contribution to the total in a 
   * gaussian manner.
   */
  total = 0.0;
  for( v=0; v<3; v++ ) {
    for( h=0; h<3; h++ ) {
      total += new_nbr[v][h].d[0] * 
	interp_1(new_nbr[v][h].v, new_nbr[v][h].h);
    }
  }
  return total;
}


/* initialize the 3x3 coordinates of the neighborhood src pixels
 * returns the avg radius for the neighborhood 
 * of the src's for the current neighborhood of dst or
 * return zero if the entire src neighborhood lies outside
 * the source image.
 * 
 * ASSUMES that src_pnt.v and src_pnt.h have already been initialized.
 * uses the current dst_pnt.h,v and src_pnt.h,v
 */
static double init_src_nbrhd()
{
  int v,h,cnt;
  double *vp,*hp, rad, total_rad, dist_v, dist_h;
  
  total_rad = 0.0;
  cnt = 0;
  for( v=0; v<3; v++ ) {
    for( h=0; h<3; h++ ) {
      vp = &(src_nbrhd[v][h].v);
      hp = &(src_nbrhd[v][h].h);
      if( (v==1) && (h==1) ) {
        *vp = src_pnt.v;
        *hp = src_pnt.h;
      }
      else {
	dst_to_src_once_per_pixel((int)dst_pnt.v+v-1,(int)dst_pnt.h+h-1,vp,hp);

	if( INSIDE( src.vdim, src.hdim, *vp, *hp) ) {
	  dist_v = src_pnt.v - *vp;
	  dist_h = src_pnt.h - *hp;
	  rad = sqrt(dist_v*dist_v + dist_h*dist_h);
	  total_rad += rad;
	  cnt++;
	}
      }
    }
  }
  if( cnt > 0 )
    return total_rad / (double)cnt;
  else
    return 0.0;
}

/* calculate the summed intensity and number of
 * pixels that are inside the given quadralateral, 
 * using scan conversion.
 */
static void scan_convert_src_area(quad_type src_quad,
				  double *sum,
				  long *cnt )
{
  double top,bot,lft,rgt,x;
  int i,j,h;
  point_type *p,*q;

  /* find y limits of quad */
  top = bot = src_quad[0][0].v;
  for( i=1; i<4; i++ ) {
    p = &src_quad[i/2][i%2];
    if( p->v < top ) 
      top = p->v;
    if( p->v > bot ) 
      bot = p->v;
  }

  *sum = 0.0;
  *cnt = 0L;

  /* find lft and rgt x intersects for this scan line, j 
   * by trying to interect this scan line with all 4 edges.
   * once an intersection is found, traverse the source
   * scan to accumulate summed intensity and total pixel count.
   */
  for( j=RND(top); j<=RND(bot); j++ ) {

    lft = 99999.99;
    rgt = -lft;
    
    for( h=0; h<2; h++ ) { 
      for( i=0; i<2; i++ ) {
	if( h == 0 ) { /* structured vertical edge */
	  p = &src_quad[i][0];
	  q = &src_quad[i][1];
	}
	else {         /* structured horizontal edge */
	  p = &src_quad[0][i];
	  q = &src_quad[1][i];
	}
	
	if( ((p->v >= j) && (q->v <= j)) ||((q->v >= j) && (p->v <= j)) ) {
	  x = linear_interp(p->v,p->h, q->v,q->h, (double)j);
	  if( x<lft )
	    lft = x;
	  if( x>rgt )
	    rgt = x;
	}
      }
    }
    /* this integer scan doesn't intersect quad */
    if( lft > rgt )
      continue;

    /* otherwise increment the pixel count and 
     * gather total summed intensity along this scan */
    else {
      for( i=RND(lft); i<=RND(rgt); i++ ) {
	*sum += get_src( j, i );
	*cnt += 1;
      }
    }
  } /* j */

  /* if no integer scans intersect the quad, then 
   * this was a micropolygon. Let's at least
   * bilinearly interpolate the intensity at the 
   * center of the quad which is the current src pixel.
   */
  if( *cnt == 0 ) {
    *sum = interp_1( src_pnt.v, src_pnt.h );
    *cnt = 1;
  }
}