/* INTERP_wts.c: interpolates a function whose values are known on a hypergrid
 *               using either multidimensional linear interpolation or 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 interp.g by  
	         computing a probability vector that represents the interpolation
		 as a convex combination of the values of the function on the
		 grid hypercube that contains the point s, i.e. it computes a
		 vector p satisfying, p'vf=v(s), where p is nonnegative and sums
		 to 1.

	         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.
    p:       probability weight vector to be computed. An (nx1) vector, same dimension as vf
    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 (this is the actual return of this function)
    p:         vector that represents the interpolation, p'vf=v(s) (this is implicitly
               returns since p is passed as an input argument and its values are overwritten
	       with the correct p vector, treated as an additional output)
    
    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
    vertwt.c     computes the probability weights that represent s as a convex
                 combination of the vertices of the grid hypercube that contains
		 s, the point to be interpolated

    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_wts(double *s,double *p,double *vf,double *grid,int *gridnum,int d,int intmeth)
{
        int i,j,ci,li,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];
        }

	for (i=0; i<n; i++)
        {
           p[i]=0.0;
        }

	/* 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];
        }

	k=kseq(arg,gridnum,d);

	ivf=vf[k];
	p[k]=1.0;
	li=k;

	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];
            }
	    ci=kseq(arg,gridnum,d);
	    cvf=vf[ci];
	    ivf=ivf+(1-swts[k])*(cvf-lvf);
	    p[ci]=1.0-swts[k];
	    p[li]=p[li]-(1.0-swts[k]);
	    li=ci;
	    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];
               }

  	       ci=kseq(arg,gridnum,d);
  	       vsav[i]=vf[ci];
	       p[ci]=vertwt(wts,indx,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);
   }
}