#include "sensCalcs.h"

bool computeTerrainGradient(Matrix &G, double* northPts, double* eastPts, 
			    const int numPts, const mapT& map)
{
   int xCurr, yCurr;
   int xNearest, yNearest;
  
   for(int i = 1; i <= numPts; i++)
   {
     xCurr = lowerBound(northPts[i-1], map.xpts, map.numX);
     yCurr = lowerBound(eastPts[i-1], map.ypts, map.numY);
     xNearest = closestPt(northPts[i-1], map.xpts, map.numX);
     yNearest = closestPt(eastPts[i-1], map.ypts, map.numY);

     //ensure that the lower bound is less than the total number of rows/cols
     if(xCurr == map.numX-1)
        xCurr--;
     if(yCurr == map.numY-1)
        yCurr--;

     //increment indices because Matrix index starts with 1
     xCurr++;
     yCurr++;
     xNearest++;
     yNearest++;

     //compute gradients
     G(i,1) = (1.0/map.dx)*(fabs(map.depths(xCurr+1, yNearest)) - 
			     fabs(map.depths(xCurr, yNearest)));

     G(i,2) = (1.0/map.dy)*(fabs(map.depths(xNearest, yCurr+1)) - 
			     fabs(map.depths(xNearest, yCurr)));
   }
  
   return true;
}

void findMapIndices(double north, double east, int& indexNorth, int& indexEast,
		    const mapT& map)
{
   indexNorth = nearest(north, map.xpts, map.numX);
   indexEast = nearest(east, map.ypts, map.numY);
}