/* INTERP.C: procedure to interpolate/extrapolate of function whose values are
             known on a hypergrid via multidimensional linear interpolation
	     or the the simplicial interpolation algorithm given in Algorithm 
	     6.5 (p. 243) of Judd, 1998, Numerical methods in Economics (MIT Press).
		   
             This procedure differs from the procedure interpolate.g by not 
	     computing gradients and not doing any extrapolation if the point
	     s is outside the grid .
	     
	     This function differs from interp_wts.c by *not* returning a vector
	     p that "represents" the interpolation operation. Thus it is slightly
	     faster than interp_wts.c when the p vector is not required.

	     John Rust, University of Maryland, June 2005 

    Inputs:

    s:       point (dx1 vector) the the function is to be evaluated (via interpolation)
    vf:      value function at grid points, arranged as an nx1 vector where n=n1*n2*...*nd
             where nj is the number of grid points in dimension j.
    grid:    gridpoints at which vf is given 
             grid=(x(1,1),...,x(1,n1),x(2,1),...,x(2,n2),....,x(d,1),....,x(d,nd))
    gridnum: dx1 vector with number of grid points used in each dimension
             gridnum=(n1,n2,...,nd)

    intmeth:interpolation method, 1 for simplicial interpolation
				  0 for linear interpolation
    Output:

    V(s):      interpolated value of function at point s
    grad V(s): gradient of the interpolated function at point s.
    
    We assume the elements of grid and vf are stored in column order, i.e.

        k=1;
        do i1=1,...,n1
	do i2=1,...,n2
	   ...
        do id=1,...,nd
	    k=k+1;
	    vf[k]=v(x(i1,1),x(i2,2),...,x(id,d)),

        where the grid points in each dimension satisfy:

	 x(j,1) < x(j,2) < .... < x(j,nj)

    Required procedures:

    binsucc.c    computes binary successor operation
    power.c      computes binary exponential, 2^d
    kseq.c       locates the sequence number in a multidimensional index
    mysort.c     sorts a vector of numbers and returns permuated indices

    Note: this routine is for 2 and higher dimensional problems only.
          Use vint1d.c for 1-dimensional problems. In 1-dimensional 
	  problems there is no difference between linear and simplicial
          interpolation.
*/

double interp(double *s,double *vf,double *grid,int *gridnum,int d,int intmeth)
{
        int i,j,dd,n,k,lb,ub,lbs,twod,*indx,*arg,*argl,*lbound,*ubound;
	double ivf,cvf,lvf,*vsav,*wts,*swts;

	wts=(double *) malloc(d*sizeof(double));
	lbound=(int *) malloc(d*sizeof(int));
	ubound=(int *) malloc(d*sizeof(int));
	arg=(int *) malloc(d*sizeof(int));

	n=1;
	for (i=0; i<d; i++)
        {
          n=n*gridnum[i];
        }

	/* loop through each coordinate s and determine the upper and 
	   lower bounds of the grid slice containing it, or if is is
	   outside the grid, record whether s is the the left (extrap < 0)
	   of the minimum grid point, or to the right (extrap > 0) of the 
	   maximum grid point for the coordinate in question. */

        for (i=0; i<d; i++)
        {
	    ub=0;
	    for (j=0;j<=i;j++)
            {
	      ub=ub+gridnum[j];
	    } 

	    if (i > 0)
            {
              lb=1;
              for (j=0; j<=i-1; j++)
              {
	        lb=lb+gridnum[j];
              }
	    }
	    else
            {
	       lb=1;
	    }

	    lb--;
	    ub--;
	    lbs=lb;

	    if (s[i] > grid[ub])
            {
               printf("Error interp.c: argument out of grid bounds\n");
	       printf("s[%2i]=%10.7f, max grid point=%10.7f\n",i,s[i],grid[ub]);
	       free(wts);
	       free(lbound);
	       free(ubound);
	       free(arg);
	       return 0;
            }
	    if (s[i] < grid[lb])
            {
               printf("Error interp.c: argument out of grid bounds\n");
	       printf("s[%2i]=%10.7f, min grid point=%10.7f\n",i,s[i],grid[lb]);
	       free(wts);
	       free(lbound);
	       free(ubound);
	       free(arg);
	       return 0;
            }

	    for (j=lb; j<=ub; j++)
            {
               if (grid[j] >= s[i])
               {
		   ub=j;
               }
	    }

	    if (ub == lb)
            {
               ub=lb+1;
            }
            else
            {
	      lb=ub-1;
            }

	    lbound[i]=lb-lbs;
	    ubound[i]=ub-lbs;

	    wts[i]=(s[i]-grid[lb])/(grid[ub]-grid[lb]);
	}

/* The following branch uses either simplicial interpolation or
   multidimensional linear interpolation depending on the value of the
   intmeth argument */

   if (intmeth == 1)
   {
        /* Under simplicial interpolation, we start at the "top" of the
	   hypercube (i.e. the largest gridpoint in the vector sense)
	   and then move down the vertices of the hypercube for d more
	   time according to the sorted weight vector, wts, above, and
	   recursively taking convex combinations so that the final
	   interpolated value is a convex combination of d+1 of the 
	   hypercube vectices. The gradient, however, will be discontinuous
	   and constant within sub-simplices of the grid hypercube. */

        swts=(double *) malloc(d*sizeof(double));
	indx=(int *) malloc(d*sizeof(int));
	argl=(int *) malloc(d*sizeof(int));

	for (k=0; k<d; k++)
        {
	  arg[k]=ubound[k];
	  argl[k]=arg[k];
        }

	ivf=vf[kseq(arg,gridnum,d)];

	lvf=ivf;

	k=mysort(wts,swts,indx,d);

	for (k=0; k<d; k++)
        {
	    arg[indx[k]]=argl[indx[k]]-ubound[indx[k]]+lbound[indx[k]];
	    for (j=0; j<d; j++)
            {
	      argl[j]=arg[j];
            }
	    cvf=vf[kseq(arg,gridnum,d)];
	    ivf=ivf+(1-swts[k])*(cvf-lvf);
	    lvf=cvf;
	}

	free(indx);
	free(swts);
	free(argl);

	free(wts);
	free(lbound);
	free(ubound);
	free(arg);

	return(ivf);
   }
   else
   {
       /* Under multidimensional linear interpolation we start by
          retrieving the value of the function on each of the
          corners of the grid hypercube that contains the point
          that we want to interpolate at, and then recursively compute
          convex combinations to arrive at the interpolated value.
	  Multidimensional interpolation is thus subject to a curse of
	  dimensionality since it is a convex combination of all 2^d 
	  values of the hypercube vertices. However the gradient is
	  a continuous function of s. It is constant (i.e. independent
	  of s(1)) for the first coordinate, but the gradient with
	  respect to s(j) depends on the values of s(i) for i < j. */

        twod=power(d);

        vsav=(double *) malloc(twod*sizeof(double));
	indx=(int *) malloc(d*sizeof(int));

	for (k=0; k<d; k++)
        {
          indx[k]=0;
        }

        for (dd=d; dd>0; dd--)
        {
          if (dd == d)
          {
             for(i=0; i<twod; i++)
             {
               for (k=0; k<d; k++)
               {
	         arg[k]=lbound[k]*(1-indx[k])+indx[k]*ubound[k];
               }

  	       vsav[i]=vf[kseq(arg,gridnum,d)];

	       if (i % 2 == 1)
               {
                 k=(i+1)/2;
	         vsav[k-1]=(1-wts[d-1])*vsav[i-1]+wts[d-1]*vsav[i];
	       } 

	       if (i < twod-1)
               {
                 binsucc(indx,d);
               }

             }
          }
          else
          {
	     twod=power(dd);
             for(i=0; i<twod; i++)
             {
                if (i % 2 == 1)
                {
                   k=(i+1)/2;
	           vsav[k-1]=(1-wts[dd-1])*vsav[i-1]+wts[dd-1]*vsav[i];
		}
	     }
	  }
	}

	ivf=vsav[0];

	free(indx);
	free(vsav);

	free(wts);
	free(lbound);
	free(ubound);
	free(arg);

	return(ivf);
   }
}