/* FILENAME      : matrixArrayCalcs.cpp
 * AUTHOR        : Debbie Meduna
 * DATE          : 01/01/08
 * 
 * LAST MODIFIED : 03/30/10
 * MODIFIED BY   : Debbie Meduna
 * -----------------------------------------------------------------------------
 * Modification History
 * -----------------------------------------------------------------------------
 ******************************************************************************/

#include "matrixArrayCalcs.h"

#ifdef USE_MATLAB
Engine* matlabEng;
#endif

static Real cubWeights[] = {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0
                            ,0,-0.5,0,0,0,0,0,0,0,0.5,0,0,0,0,0,0
                            ,0,1.0,0,0,0,-2.5,0,0,0,2.0,0,0,0,-0.5,0,0
                            ,0,-0.5,0,0,0,1.5,0,0,0,-1.5,0,0,0,0.5,0,0
                            ,0,0,0,0,-0.5,0,0.5,0,0,0,0,0,0,0,0,0
                            ,0.25,0,-0.25,0,0,0,0,0,-0.25,0,0.25,0,0,0,0,0
                            ,-0.5,0,0.5,0,1.25,0,-1.25,0,-1,0,1,0,0.25,0,-0.25,0
                            ,0.25,0,-0.25,0,-0.75,0,0.75,0,0.75,0,-0.75,0,
                            -0.25,0,0.25,0
                            ,0,0,0,0,1,-2.5,2,-0.5,0,0,0,0,0,0,0,0
                            ,-0.5,1.25,-1,0.25,0,0,0,0,0.5,-1.25,1,-0.25,0,0,0,0
                            ,1,-2.5,2,-0.5,-2.5,6.25,-5,1.25,2,-5,4,-1,-0.5,
                            1.25,-1,0.25
                            ,-0.5,1.25,-1,0.25,1.5,-3.75,3,-0.75,-1.5,3.75,-3
                            ,0.75,0.5,-1.25,1,-0.25
                            ,0,0,0,0,-0.5,1.5,-1.5,0.5,0,0,0,0,0,0,0,0
                            ,0.25,-0.75,0.75,-0.25,0,0,0,0,-0.25,0.75,-0.75,
                            0.25,0,0,0,0
                            ,-0.5,1.5,-1.5,0.5,1.25,-3.75,3.75,-1.25,-1,3,-3,
                            1,0.25,-0.75,0.75,-0.25
                            ,0.25,-0.75,0.75,-0.25,-0.75,2.25,-2.25,0.75,0.75
                            ,-2.25,2.25,-0.75,-0.25,0.75,-0.75,0.25};


#ifdef _QNX
int round(const double &num)
{
  double rem = num - floor(num);
  if(rem >= 0.5)
    return int(ceil(num));
  else
    return int(floor(num));
}
#endif

int minVal(const int* values, const int numValues)
{
   int minVal = 10000;
   for(int i = 0; i < numValues; i++)
   {
      if(values[i] < minVal)
         minVal = values[i];
   }

   return minVal;
}

int maxVal(const int* values, const int numValues)
{
   int maxVal = -10000;
   for(int i = 0; i < numValues; i++)
   {
      if(values[i] > maxVal)
         maxVal = values[i];
   }

   return maxVal;
}

Matrix conv2(const Matrix &A, const Matrix &H)
{
   int filterSizeX, filterSizeY;
   Matrix tempMAT = H;
   Matrix newMAT = A;

   //filterSize corresponds to the number of rows/cols in the filter H
   filterSizeX = 2*(int(floor(H.Nrows()/2.0)));
   filterSizeY = 2*(int(floor(H.Ncols()/2.0)));
 
#if 0 //slower method of convolution, not currently being used
   int subIndices[4];
   //Compute convolution by applying filter H over all points in A
   for(int row = 1; row <= A.Nrows(); row++)
   {
      for(int col = 1; col <= A.Ncols(); col++)
      {
         //subIndices describes portion of H which has related values in A.
         subIndices[0] = 2-min(1,row-filterSize/2);
         subIndices[1] = filterSize/2+1+min(filterSize/2,A.Nrows()- row);
         subIndices[2] = 2-min(1,col-filterSize/2);
         subIndices[3] = filterSize/2+1+min(filterSize/2,A.Ncols() - col);
	  
         tempMAT = 0.0;
         tempMAT.SubMatrix
	    (subIndices[0], subIndices[1], subIndices[2], subIndices[3])
	    = SP(H.SubMatrix(subIndices[0],subIndices[1],subIndices[2],
			     subIndices[3]),
		 A.SubMatrix(max(row-filterSize/2,1),min(row+filterSize/2, 
							 A.Nrows()), 
			     max(col-filterSize/2,1),min(col+filterSize/2,
							 A.Ncols())));
         newMAT(row, col) = tempMAT.Sum();
      }
   }
#endif


   //Alternative method: apply A to all points in H
   tempMAT.ReSize(A.Nrows()+filterSizeX, A.Ncols()+filterSizeY);
   tempMAT = 0.0;
   tempMAT.SubMatrix(filterSizeX/2+1,A.Nrows()+filterSizeX/2,filterSizeY/2+1,
                     A.Ncols()+filterSizeY/2) = A;
   newMAT = 0.0;
   for(int row = 1; row <= H.Nrows(); row++)
   {
      for(int col = 1; col <= H.Ncols(); col++)
      {
         newMAT += H(row,col)*
	    tempMAT.SubMatrix(row, A.Nrows()+row-1,col,A.Ncols()+col-1);
      }
   }
     
   return newMAT;
}

void interp2(double* xpts, double* ypts, const Matrix Z, double* xi, 
	     double* yi, double* zi, int numPts)
{
   int yIndices[4];
   int xIndices[4];
   ColumnVector Weights(4);

   for(int i = 0; i < numPts; i++)
      bilinearInterp(xpts, ypts, Z, xi[i], yi[i], zi[i], xIndices, 
                     yIndices, Weights);

   return;
}

void interp2mat(double* xpts, double* ypts, const Matrix Z, double* xi, 
		double* yi, Matrix &zi)
{
   double znum;
  
   for(int i = 0; i < zi.Nrows(); i++)
   {
      for(int j = 0; j < zi.Ncols(); j++)
      {
         interp2(xpts,ypts,Z,&xi[i],&yi[j],&znum,1);
         zi(i+1,j+1) = znum;
      }
   }

   return;
}


void nearestInterp(double* xpts, double* ypts, const Matrix &zvals, 
		   double xi, double yi, double &zi, int* xIndices, 
		   int* yIndices, ColumnVector &Weights)
{
   Weights.ReSize(1);
   Weights(1) = 1;
   
   xIndices[0] = closestPtUniformArray(xi, xpts[0], xpts[zvals.Nrows()-1], zvals.Nrows());
   yIndices[0] = closestPtUniformArray(yi, ypts[0], ypts[zvals.Ncols()-1], zvals.Ncols());

   zi = zvals(xIndices[0]+1, yIndices[0]+1);

   //if the nearest value in the array is NaN, search radially
   //outward from the nearest point to find the closest non-NaN
   //grid value
   
   if(isnan(zi))
   {
      xIndices[0]++;
      yIndices[0]++;
      findNearestValid(zvals, xIndices[0], yIndices[0]);
      
      xIndices[0]--;
      yIndices[0]--;
      zi = zvals(xIndices[0]+1, yIndices[0]+1);
   }
   
}

bool findNearestValid(const Matrix &zvals, int &xIndex, int &yIndex)
{
   int i, j, r, rPower, rBase, maxRadius;
   i = 1; 
   j = 1;
   r = 1; //radius of search away from nominal point
   rPower = 0; //radius power
   rBase = 2; //radius base 
   maxRadius = 32;

   if(!isnan(zvals(xIndex, yIndex)))
      return true;
   
   r = (int)(pow(rBase,rPower));
   //   while ((r < zvals.Nrows()) || (r < zvals.Ncols()))
   while(r < maxRadius)
   {
    //search side columns
    for(i = -r; i <= r; i++)
    {
       //check that we are indexing within the grid
       if((xIndex+i < 1) || (xIndex+i > zvals.Nrows()))
          continue;
       
       for(j = -r; j <= r; j = j+2*r)
       {       
          //check that we are indexing within the grid
          if((yIndex+j < 1) || (yIndex+j > zvals.Ncols()))
             continue;

          if(!isnan(zvals(xIndex+i,yIndex+j)))
          {
             xIndex += i;
             yIndex += j;
             return true;
          }
       }
    }  

    //search top and bottom rows
    for(j = -r+1; j <= r-1; j++)
    {
       //check that we are indexing within the grid
       if((yIndex+j < 1) || (yIndex+j > zvals.Ncols()))
          continue;
       
       for(i = -r; i <= r; i = i+2*r)
       { 
          //check that we are indexing within the grid
          if((xIndex+i < 1) || (xIndex+i > zvals.Nrows()))
             continue;

          if(!isnan(zvals(xIndex+i,yIndex+j)))
          { 
             xIndex += i;
             yIndex += j;
             return true;
          }
       }
    }
    
    rPower++;
    r = (int)(pow(rBase,rPower));
 }

 return false;
}


void bilinearInterp(double* xpts, double* ypts, const Matrix &zvals,
		    double xi, double yi, double &zi, int* xIndices, 
		    int* yIndices, ColumnVector &Weights)
{
   double dx, dy, t, u;
   double lowerX, lowerY, upperX, upperY;
   int i,x1, y1;

   Weights.ReSize(4);

   //find the lowest closest point in the reference data to desired point
   x1 = lowerBound(xi, xpts, zvals.Nrows());
   y1 = lowerBound(yi, ypts, zvals.Ncols());

   //ensure that the bounding box falls within the extracted map size
   //if the bounding box is outside the map, use nearestInterp
   if((x1 >= zvals.Nrows()-1) || (x1 < 0) || (y1 >= zvals.Ncols()-1) || (y1 < 0))
   { 
      nearestInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, 
                    yIndices, Weights);
      for(i = 1; i < 4; i++)
      {
         xIndices[i] = 0;
         yIndices[i] = 0;
      }
      return;
   }

   //define surrounding four corners in the grid
   lowerX = xpts[x1];
   upperX = xpts[x1+1];
   lowerY = ypts[y1];
   upperY = ypts[y1+1];

   dx = upperX-lowerX;
   dy = upperY-lowerY;

   /*Define interpolation indices into zvals
    *The four interpolation points are labeled as follows:
    * 0  2
    * 1  3 */
   xIndices[0] = x1;
   yIndices[0] = y1;
   xIndices[1] = x1+1;
   yIndices[1] = y1;
   xIndices[2] = x1+1;
   yIndices[2] = y1+1;
   xIndices[3] = x1;
   yIndices[3] = y1+1;

   //Define bilinear interpolation weights
   t = (xi - lowerX)/dx;
   u = (yi - lowerY)/dy;
   Weights(1) = (1 - t)*(1 - u);
   Weights(2) = t*(1 - u);
   Weights(3) = t*u;
   Weights(4) = (1 - t)*u;

   //Compute interpolated zi by weighted sum of the four z points
   zi = 0.0;
   for(i = 0; i < 4; i++)
      zi += Weights(i+1)*zvals(xIndices[i]+1, yIndices[i]+1);
 
   //Check that the interpolated value is not NaN, otherwise, try nearestInterp
   if(isnan(zi))
   {
      nearestInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, 
                    yIndices, Weights);
      
      for(i = 1; i < 4; i++)
      {
         xIndices[i] = 0;
         yIndices[i] = 0;
      }
   }

   return;
}

void bicubicInterp(double* xpts, double* ypts, const Matrix &zvals, 
		   double xi, double yi, double &zi, int* xIndices, 
		   int* yIndices, ColumnVector &Weights)
{
   int i, j, x1, y1; 
   Matrix W(16,16);
   ColumnVector b(16);
   double dx, dy, t, u;
   Weights.ReSize(16);

   //find the lowest closest point in the reference data to desired point
   x1 = lowerBound(xi, xpts, zvals.Nrows());
   y1 = lowerBound(yi, ypts, zvals.Ncols());

   /*Define interpolation indices into zvals
    *The sixteen interpolation points are labeled as follows:
    *12 13 14 15
    * 8  9 10 11
    * 4  5  6  7
    * 0  1  2  3*/
   xIndices[5] = x1;
   yIndices[5] = y1;

   //ensure that the bounding box falls within the extracted map size
   //if bounding box is outside the map, attempt bilinear interpolation
   if((x1+2 > zvals.Nrows()-1) || (x1-1 < 0) || (y1+2 > zvals.Ncols()-1) 
      || (y1-1 < 0))
   { 
      bilinearInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, 
                     yIndices, Weights);
      for(i = 4; i < 16; i++)
         {
            xIndices[i] = 0;
            yIndices[i] = 0;
         }
      return;
   }

   //define indices of interpolation z points
   for(i = 0; i < 16; i=i+4)
      xIndices[i] = xIndices[5]-1;
   for(i = 1; i < 16; i=i+4)
      xIndices[i] = xIndices[5];
   for(i = 2; i < 16; i=i+4)
      xIndices[i] = xIndices[5]+1;
   for(i = 3; i < 16; i=i+4)
      xIndices[i] = xIndices[5]+2;

   for(i = 0; i < 4; i++)
      yIndices[i] = yIndices[5]-1;
   for(i = 4; i < 8; i++)
      yIndices[i] = yIndices[5];
   for(i = 8; i < 12; i++)
      yIndices[i] = yIndices[5]+1;
   for(i = 12; i < 16; i++)
      yIndices[i] = yIndices[5]+2;

   //define t and u parameters to be used for calculating interpolation
   //weights
   dx = xpts[xIndices[6]]-xpts[xIndices[5]];
   dy = ypts[yIndices[9]]-ypts[yIndices[5]];  
   t = (xi - xpts[x1])/dx;
   u = (yi - ypts[y1])/dy;
   

   //Define interpolation weights
   for(i = 1; i <= 4; i++)
   {
      for(j = 1; j <= 4; j++)
         b((i-1)*4+j) = pow(t,i-1)*pow(u,j-1);
   }
   
   W << cubWeights;   
   Weights = W.t()*b;
     
   //Determine z value based on interpolation weights:
   zi = 0.0;
   for(i = 0; i < 16; i++)
      zi += Weights(i+1)*zvals(xIndices[i]+1, yIndices[i]+1);

   //Check that the interpolated value is not NaN, otherwise,try bilinearInterp
   if(isnan(zi))
   {
      bilinearInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, 
                     yIndices, Weights);
      for(i = 4; i < 16; i++)
      {
         xIndices[i] = 0;
         yIndices[i] = 0;
      }
   }
   return;
}

void splineInterp(double* xpts, double* ypts, const Matrix &zvals, 
		  double xi, double yi, double &zi, int* xIndices, 
		  int* yIndices, ColumnVector &Weights)
{  

#ifdef USE_MATLAB
   mxArray *Z = NULL;
   mxArray *X = NULL;
   mxArray *Y = NULL;
   mxArray *Xi;
   mxArray *Yi;
   mxArray *result = NULL;

   //copy contents of Surf into Matlab variable A
   Z = mxCreateDoubleMatrix(zvals.Ncols(), zvals.Nrows(), mxREAL);
   memcpy((void *)mxGetPr(Z), (void *) zvals.Store(), zvals.Storage()*sizeof(double));

   X = mxCreateDoubleMatrix(1, zvals.Nrows(), mxREAL);
   memcpy((void *)mxGetPr(X), (void *) xpts, sizeof(xpts));  

   Y = mxCreateDoubleMatrix(1, zvals.Ncols(), mxREAL);
   memcpy((void *)mxGetPr(Y), (void *) ypts, sizeof(ypts));
  
   Xi = mxCreateDoubleMatrix(1, 1, mxREAL);
   memcpy(mxGetPr(Xi), (void *)&xi, sizeof(double));

   Yi = mxCreateDoubleMatrix(1, 1, mxREAL);
   memcpy(mxGetPr(Yi), (void *)&yi, sizeof(double));

   //put data into Matlab workspace
   engPutVariable(matlabEng, "Z", Z);
   engPutVariable(matlabEng, "X", X);
   engPutVariable(matlabEng, "Y", Y);
   engPutVariable(matlabEng, "Yi", Yi);
   engPutVariable(matlabEng, "Xi", Xi);

   //compute interpolation in Maltab
   engEvalString(matlabEng, "[Xm,Ym] = meshgrid(X,Y);");
   engEvalString(matlabEng, "Zi = interp2(Xm,Ym,Z,Xi,Yi,'spline');");
   engEvalString(matlabEng, "figure(10);");
   engEvalString(matlabEng, "surf(Zi, 'Linestyle', 'none');");
  
   //extract interpolated value from Matlab
   result = engGetVariable(matlabEng,"Zi");
   memcpy((void *)&zi,(void*) mxGetPr(result), sizeof(double));

   //remove memory in Matlab
   mxDestroyArray(Z);
   mxDestroyArray(X);
   mxDestroyArray(Y);
   mxDestroyArray(Xi);
   mxDestroyArray(Yi);
   mxDestroyArray(result);

#else
   nearestInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, yIndices, Weights);

#endif

}


void nearestInterp_mat(double* xpts, double* ypts, const Matrix &zvals, 
		       double* xi, double* yi, Matrix &zi, Matrix &var)
{
   int* xIndices = NULL;
   int* yIndices = NULL;
   xIndices = new int[1];
   yIndices = new int[1];
   ColumnVector W;

   //perform nearest-neighbor interpolation for each point in (xi,yi)
   for(int i = 1; i <= zi.Nrows(); i++)
   {
      for(int j = 1; j<= zi.Ncols(); j++)
         nearestInterp(xpts, ypts, zvals, xi[i-1], yi[j-1], zi(i,j), 
                       xIndices, yIndices, W);
   }

   delete [] xIndices;
   delete [] yIndices;

   return;
}

void bilinearInterp_mat(double* xpts, double* ypts, const Matrix &zvals, 
			double* xi, double* yi, Matrix &zi, Matrix &var)
{
   int* xIndices = NULL;
   int* yIndices = NULL;
   xIndices = new int[4];
   yIndices = new int[4];
   ColumnVector W;

   //perform nearest-neighbor interpolation for each point in (xi,yi)
   for(int i = 1; i <= zi.Nrows(); i++)
   {
      for(int j = 1; j<= zi.Ncols(); j++)
         bilinearInterp(xpts, ypts, zvals, xi[i-1], yi[j-1], zi(i,j), 
                        xIndices, yIndices, W);
   }

   delete [] xIndices;
   delete [] yIndices;

   return;
}

void bicubicInterp_mat(double* xpts, double* ypts, const Matrix &zvals, 
		       double* xi, double* yi, Matrix &zi, Matrix &var) 
{
   int* xIndices = NULL;
   int* yIndices = NULL;
   xIndices = new int[16];
   yIndices = new int[16];
   ColumnVector W;

   //perform nearest-neighbor interpolation for each point in (xi,yi)
   for(int i = 1; i <= zi.Nrows(); i++)
   {
      for(int j = 1; j<= zi.Ncols(); j++)
         bicubicInterp(xpts, ypts, zvals, xi[i-1], yi[j-1], zi(i,j), 
                        xIndices, yIndices, W);
   }

   delete [] xIndices;
   delete [] yIndices;

   return;
}

void splineInterp_mat(double* xpts, double* ypts, const Matrix &zvals, 
		      double* xi, double* yi, Matrix &zi, Matrix &var) 
{
#ifdef USE_MATLAB
   mxArray *Z = NULL;
   mxArray *X = NULL;
   mxArray *Y = NULL;
   mxArray *Xi;
   mxArray *Yi;
   mxArray *result = NULL;

   //copy contents of Surf into Matlab variable A
   Z = mxCreateDoubleMatrix(zvals.Ncols(), 
                            zvals.Nrows(), mxREAL);
   memcpy((void *)mxGetPr(Z), (void *) zvals.Store(), zvals.Storage()*sizeof(double));

   X = mxCreateDoubleMatrix(1, zvals.Nrows(), mxREAL);
   memcpy((void *)mxGetPr(X), (void *) xpts, zvals.Nrows()*sizeof(double));  

   Y = mxCreateDoubleMatrix(1, zvals.Ncols(), mxREAL);
   memcpy((void *)mxGetPr(Y), (void *) ypts, zvals.Ncols()*sizeof(double));
  
   Xi = mxCreateDoubleMatrix(1, zi.Nrows(), mxREAL);
   Yi = mxCreateDoubleMatrix(zi.Ncols(), 1, mxREAL);

   //put data into Matlab workspace
   engPutVariable(matlabEng, "Z", Z);
   engPutVariable(matlabEng, "X", X);
   engPutVariable(matlabEng, "Y", Y);
   engEvalString(matlabEng, "[Xm,Ym] = meshgrid(X,Y);");
 
   memcpy((void *)mxGetPr(Xi), (void *) xi, 
          zi.Nrows()*sizeof(double));
   memcpy((void *)mxGetPr(Yi), (void *) yi, 
          zi.Ncols()*sizeof(double));
   engPutVariable(matlabEng, "Yi", Yi);
   engPutVariable(matlabEng, "Xi", Xi);
   engEvalString(matlabEng, "[Xd,Yd] = meshgrid(Xi,Yi);");

   engEvalString(matlabEng, 
                 "Zi = interp2(Xm,Ym,Z,Xi,Yi,'spline');");
   result = engGetVariable(matlabEng,"Zi");
   memcpy((void *)zi.Store(), (void *) mxGetPr(result), 
          zi.Storage()*sizeof(double));

   //remove memory in Matlab
   mxDestroyArray(Z);
   mxDestroyArray(X);
   mxDestroyArray(Y);
   mxDestroyArray(Xi);
   mxDestroyArray(Yi);
   mxDestroyArray(result);

#endif

   nearestInterp_mat(xpts, ypts, zvals, xi, yi, zi, var);

   return;
}

int closestPt(double key, const double *base, size_t nmemb) {
    
   int i = 0, idx = 0, j;
   double a, dt, dt0, minValue, maxValue;
   maxValue = *(base + (nmemb - 1));
   minValue = *base;
    
   //If key is larger than the largest value in the array OR
   //the key is smaller than the smallest value, return the 
   //largest or smallest array value accordingly
   if (key > maxValue)
      idx = nmemb - 1;
   else if (key < minValue)
      idx = 0;
   else 
   {
      dt0 = maxValue;
      for (j = 0; j < int(nmemb); j++) 
      {
         a = *(base + j);
         dt = fabs(key - a);
         if (dt <= dt0) 
         {
            dt0 = dt;
            idx = i;
         }
         else
            break;
	    
         ++i;
      }
   }

   return idx;
}


int lowerBound(double val, const double* vec, int numVals)
{
  int nearestIndex;
  int lowerIndex;
  double diff;

  nearestIndex = closestPtUniformArray(val, vec[0], vec[numVals-1], numVals);
  diff = val - vec[nearestIndex];

  if(diff >= 0)
      lowerIndex = nearestIndex;
  else
      lowerIndex = nearestIndex-1;

  return lowerIndex;
}

double randn(double mean, double stddev)
{
   double rand1, rand2, gauss1, w;
   static int use_last = 0;
   static double gauss2;

   //If we already have a random variable waiting to be used, use it
   if (use_last)		
   {
      gauss1 = gauss2;
      use_last = 0;
   }
   //Otherwise, generate two new random numbers
   else
   {
      //Use the Polar Form of the Box-Muller transformation:
      //1.Generate two uniform random variables within the unit circle
      do {
         //Generate two uniform random variables between -1 and 1
         rand1 = unif(0,1);
         rand2 = unif(0,1);
         w = rand1*rand1 + rand2*rand2;
      } while ( w >= 1.0 );
      
      //2.Convert random variables to gaussian variables, N(0,1).
      w = sqrt(-2.0*log(w)/w);
      gauss1 = rand1 * w;
      gauss2 = rand2 * w;

      //Set flag that we have an extra variable that can be used
      use_last = 1;
   }
   
   return (mean + stddev*gauss1);

}


char* charCat(char* dest, const char* front, const char* back)
{
  sprintf(dest,"%s%s",front, back);
  
  return dest;
}


double computeKLdiv_gaussian_mat(double* xpts, double* ypts, 
                                 const Matrix &refPDF, 
                                 double* mu, const Matrix &Cov)
{
   Matrix A(Cov.Nrows(), Cov.Ncols());
   Matrix Value(1,1);
   ColumnVector dx(2);   
   double q;
   int i, j;
   double eta;
   double kl = 0;
   
   //compute gaussian normalization factor
   A = 2.0*PI*Cov;  
   eta = pow(A.Determinant(),-0.5);

   //compute inverse of covariance for gaussian calculation
   A = Cov.i();

   //sum KL over all entries in refPDF
   for(i = 1; i <= refPDF.Nrows(); i++)
   {
      dx(1) = xpts[i-1] - mu[0];
      for(j = 1; j <= refPDF.Ncols(); j++)
      {
         //compute current guassian probability
         dx(2) = ypts[j-1] - mu[1];
         Value = dx.t()*A*dx;
         q = eta*exp(Value.AsScalar()*-0.5);

         //add current kl entry
         if(refPDF(i,j)/q > 1e-50 && refPDF(i,j)/q < 1e50)
            kl += refPDF(i,j)*log(refPDF(i,j)/q);
      }
   }

   return kl;
}

SymmetricMatrix computeMatrixSqrt(const SymmetricMatrix &A)
{
   SymmetricMatrix Asqrt;
   DiagonalMatrix D;
   Matrix V;
   int i;

   //compute eigenvalue decomposition
   Jacobi(A,D,V);

   //compute sqrt of eigenvalue matrix
   for(i = 1; i <= A.Nrows(); i++)
   {
      //ensure positive definite matrix prior to taking the sqrt
      if(D(i) < 0)
      {
         output("Error: Tried to take the sqrt of a non-positive definite "
                "matrix. Exiting...\n");
         exit(0);
      }
      D(i) = sqrt(D(i));
   }

   //reconstruct sqrt matrix
   Asqrt << V*D*V.t();

   return Asqrt;
}


