#include "img.h"

/* 
 * linear interp solves for y given x1,y1,x2,y2 and x
 *
 *          |  
 *     y1   +    *         
 *      y   -    |         o
 *          |    |         |                      
 *     y2   +    |         |             *
 *          |    |         |             |
 *          |    |         |             |
 *          +----+---------+-------------+------
 *               x1        x              x2
 */
double linear_interp(double x1,double y1,
		     double x2,double y2,
		     double x)
{
  double y;
  if( x2 != x1 )
    y = y1+(x-x1)*(y2-y1)/(x2-x1);
  else
    y = y1;
  return y;
}


/*
 * bilinear interpolation using a reference rectangle.
 * each point has dims of data measures that need to be interpolated.
 */
void bilinear_interp(point_type ref[2][2],
		     point_type *src_pos,
		     int dims)
{
  int i;
  double lft_d,rgt_d;

  for( i=0; i<dims; i++ ) {
    lft_d = linear_interp(ref[0][0].v,ref[0][0].d[i],
			  ref[1][0].v,ref[1][0].d[i],
			  src_pos->v);
    rgt_d = linear_interp(ref[0][1].v,ref[0][1].d[i],
			  ref[1][1].v,ref[1][1].d[i],
			  src_pos->v);

    src_pos->d[i] = linear_interp(ref[0][0].h,lft_d,
				  ref[0][1].h,rgt_d,
				  src_pos->h);
  }
}

/* Cubic Spline interpolated warping. 
 * Fits a (separable) cubic function to a 4x4 neighborhood.
 * assumes 4x4 reference positions are -1..2 int and pos is 0..1 real
 */

void bicubic_interp(point_type ref[4][4],
		    point_type *pos,
		    int dims)
{
  register int i,h,v;
  double temp;
  double frac, frac2, frac3;
  double hcoeff[4];
  double vcoeff[4];

#ifdef DEBUG
  if( (ref[0][0].h != -1.0) || (ref[0][0].v != -1.0) || 
      (pos->v < 0.0) || (pos->v > 1.0) || 
      (pos->h < 0.0) || (pos->h > 1.0) ) {
    fprintf(stdout,"bicubic_interp ERROR: bad positions\n");
    return;
  }
#endif

  frac = pos->h;
  frac2 = frac*frac;
  frac3 = frac*frac2;
  
  hcoeff[0] = (frac2/2.0 - frac/3.0 - frac3/6.0); /* h coeff at -1 */
  hcoeff[1] = (1.0 - frac/2.0 - frac2 + frac3/2.0);
  hcoeff[2] = (frac + frac2/2.0 - frac3/2.0);
  hcoeff[3] = (frac3/6.0 - frac/6.0);
  
  frac = pos->v;
  frac2 = frac*frac;
  frac3 = frac*frac2;

  vcoeff[0] = (frac2/2.0 - frac/3.0 - frac3/6.0); /* v coeff at -1 */
  vcoeff[1] = (1.0 - frac/2.0 - frac2 + frac3/2.0);
  vcoeff[2] = (frac + frac2/2.0 - frac3/2.0);
  vcoeff[3] = (frac3/6.0 - frac/6.0);

  /*   d[1][1] = v[1][4] * ( R[4][4] * h[4][1] )
   * 
   *   where :
   *     d is the cubic interpolated data value
   *     v is the vector of vt coefficients evaluated at the input position
   *     R is a matrix of data values of the neighborhood
   *     h is the vector of hz coefficients evaluated at the input position
   */

  for( i=0; i<dims; i++ ) {
    pos->d[i] = 0.0;
    for( v=0; v<4; v++ ) {
      temp = 0.0;
      for( h=0; h<4; h++ )
	temp += ref[v][h].d[i] * hcoeff[h];
      pos->d[i] += vcoeff[v] * temp;
    }
  }
}