/* VINT2D.C: procedure to interpolate value function computed on a grid
             via  either multdimensional linear interpolation or
	     the 2-dimensional simplicial interpolation algorithm
	     given in Algorithm 6.5 (p. 243) of Judd, 1998,
	     Numerical methods in Economics (MIT Press).

	     John Rust, Georgetown University, May 2013
	   
    vint2d(q,p,vf,ivf,ns,nqgp,npgp,qgrid,pgrid)

    Inputs:

    q:      ns x 1 vector of q-coordinates to interpolate at
    p:      ns x 1 vector of y-coordinates to interpolate at
    ns:     dimension of q and p
    vf:     value function at grid points 
    ivf:    vector of interpolated values (return) of
            dimension nsx1

    qgrid:  vector of points defining grid on x axis 
    pgrid:  vector of points defining grid on y axis
    nqgp:   dimension of qgrid
    npgp:   dimension of pgrid
    intmeth interpolation method:
            1: for bilinear interpolation (recommended)
	    0: for simplicial interpolation

    We assume the elements of vf are stored in column order, i.e.

	    vf[k]=v[q,p], k=q+p*nqgp, q=0,1,...,nqgp,
	                              p=0,1,...,npgp 
 
            i.e. if vf[k] was created in the following do-loop

            k=0;
            for (p=0; q<npgp; q++)
            {
              for (q=0; p<nqgp; p++)
              {
                   vf[k]=v(q,p);
                   k++;
              }
            }

	    qgrid and pgrid are ordered low to high, but uniformly
	    spaced grids are not necessariy required. We also assume
	    that the mininum and maximum values of qgrid and pgrid
	    represent the upper and lower bounds on the state space,
	    which is thus a rectangle in R^2, which is partitioned
	    into subrectangles via the qgrid and pgrid vectors
*/

double vint2d(double q,double p,double *vf,int nqgp,int npgp,double *qgrid,double *pgrid)
{ 
    int j; 
    double lam1,lam2,vxuyu,vxlyu,vxlyl,vxuyl,ivf;
    int ixuyu,ixlyu,ixlyl,ixuyl;
    double xl,xu,yl,yu;
    int ixl,ixu,iyl,iyu;

       if ((q < qgrid[0]) || (q > qgrid[nqgp-1]))
       {
          if (q < qgrid[0])
          {
             q=qgrid[0];
          }
          if (q > qgrid[nqgp-1])
          {
             q=qgrid[nqgp-1];
          }
       }
       if (q <= qgrid[1]) 
       {

	      xl=qgrid[0];
	      xu=qgrid[1];
	      ixl=0;
	      ixu=1; 
       }
       else if (q >= qgrid[nqgp-1]) 
       {
	      xl=qgrid[nqgp-2];

	      xu=qgrid[nqgp-1];
	      ixl=nqgp-2;
	      ixu=nqgp-1; 
       }
       else 
       {

	      for (j=nqgp-1; q <= qgrid[j]; j--);
	      ixu=j+1;
	      ixl=j;
	      xu=qgrid[ixu]; 
	      xl=qgrid[ixl];
       }


       if ((p < pgrid[0]) || (p > pgrid[npgp-1]))
       {
          if (p < pgrid[0])
          {
             p=pgrid[0];
          }
          if (p > pgrid[npgp-1])
          {
             p=pgrid[npgp-1];
          }
       }
       if (p <= pgrid[1]) 
       {
	      yl=pgrid[0];
	      yu=pgrid[1];
	      iyl=0;
	      iyu=1; 
       }
       else if (p >= pgrid[npgp-1]) 
       {
	      yl=pgrid[npgp-2];
	      yu=pgrid[npgp-1];
	      iyl=npgp-2;
	      iyu=npgp-1; 
       }
       else 
       {
	      for (j=npgp-1; p <= pgrid[j];  --j);
	      iyu=j+1;
	      iyl=j;
	      yu=pgrid[iyu]; 
	      yl=pgrid[iyl];
       }

       if (intmeth == 1)  /* bilinear interpolation */
       {
           lam1=(q-xl)/(xu-xl);
           lam2=(p-yl)/(yu-yl);
           ixlyl=iyl*nqgp+ixl;
           ixuyl=iyl*nqgp+ixu;
           ixuyu=iyu*nqgp+ixu;
           ixlyu=iyu*nqgp+ixl;
           vxlyl=vf[ixlyl];
           vxuyl=vf[ixuyl];
           vxuyu=vf[ixuyu];
           vxlyu=vf[ixlyu];
/* printf("lam1=%g lam2=%g vxlyu=%g vxuyu=%g vxlyl=%g vxuyl=%g\n",lam1,lam2,vxlyu,vxuyu,vxlyl,vxuyl);
printf("    ixlyl=%i ixuyl=%i ixuyu=%i ixlyu=%i\n",ixlyl,ixuyl,ixuyu,ixlyu); */

	   ivf=lam2*((1.0-lam1)*vxlyu+lam1*vxuyu)+
	       (1.0-lam2)*((1.0-lam1)*vxlyl+lam1*vxuyl);
        }
        else /*  simplicial interpolation */
        {
            lam1=(xu-q)/(xu-xl);
	    lam2=(yu-p)/(yu-yl);

	    if (lam2 > lam1)
            {
               ixlyl=iyl*nqgp+ixl;
               ixuyl=iyl*nqgp+ixu;
               ixuyu=iyu*nqgp+ixu;
               vxlyl=vf[ixlyl];
               vxuyl=vf[ixuyl];
               vxuyu=vf[ixuyu];

	       ivf=lam1*vxlyl+(lam2-lam1)*vxuyl+(1-lam2)*vxuyu;
            }
	    else
            {
               ixlyl=iyl*nqgp+ixl;
               ixuyu=iyu*nqgp+ixu;
               ixlyu=iyu*nqgp+ixl;
               vxlyl=vf[ixlyl];
               vxuyu=vf[ixuyu];
               vxlyu=vf[ixlyu];

	       ivf=lam2*vxlyl+(lam1-lam2)*vxlyu+(1-lam1)*vxuyu;
            }
        }

    return ivf;
}