/* FILENAME      : TNavFilter.cpp
 * AUTHOR        : Debbie Meduna
 * DATE          : 05/02/09
 * 
 * LAST MODIFIED : 11/30/10
 * MODIFIED BY   : Debbie Meduna
 * -----------------------------------------------------------------------------
 * Modification History
 * -----------------------------------------------------------------------------
 ******************************************************************************/

#include "TNavFilter.h"

TNavFilter::TNavFilter(char *mapName, char *vehicleSpecs, 
                       char *directory, const double *windowVar)
{
   int i;
	if(!USE_OCTREE)
		terrainMap = new TerrainMap(mapName);
	else
		terrainMap = NULL;
   vehicle = new vehicleT(vehicleSpecs);
   if(USE_COMPASS_BIAS)
      compassBias = new compassBiasT;
   else
      compassBias = NULL;
   saveDirectory = directory;
   //this->octreeMap = NULL;
   
   //initialize currVar and initWindowVar
   for(i=0;i<36;i++)
   {
      initWindowVar[i] = windowVar[i];
   }
   currVar[0] = windowVar[0];
   currVar[1] = windowVar[2];

   initVariables();
}

TNavFilter::~TNavFilter()
{
   if(terrainMap != NULL)
      delete terrainMap;
   terrainMap = NULL;
   
   //if(octreeMap != NULL)
   		//delete octreeMap;
   //octreeMap = NULL;

   if(lastNavPose != NULL)
      delete lastNavPose;
   lastNavPose = NULL;

   if(vehicle != NULL)
      delete vehicle;
   vehicle = NULL;

   if(compassBias != NULL)
      delete compassBias;
   compassBias = NULL;

#ifdef USE_MATLAB
   engClose(matlabEng);
#endif

}

bool TNavFilter::withinRefMap()
{
   double north, east;
   north = this->lastNavPose->x;
   east = this->lastNavPose->y;

   return terrainMap->withinRefMap(north, east);
}

bool TNavFilter::withinValidMapRegion(const double& north, const double& east)
{
   double dx = 0.8*MAX_RANGE;//0.1*1.5*totalAttemptTime+MAX_RANGE;
   if(terrainMap->withinValidMapRegion(north, east) 
      && terrainMap->withinValidMapRegion(north+sqrt(fabs(initWindowVar[0]))+dx, east)
      && terrainMap->withinValidMapRegion(north-sqrt(fabs(initWindowVar[0]))-dx, east) 
      && terrainMap->withinValidMapRegion(north, east+sqrt(fabs(initWindowVar[2]))+dx)
      && terrainMap->withinValidMapRegion(north, east-sqrt(fabs(initWindowVar[2]))-dx) 
      && terrainMap->withinValidMapRegion(north+sqrt(fabs(initWindowVar[0]))+dx, east+sqrt(fabs(initWindowVar[2]))+dx)
      && terrainMap->withinValidMapRegion(north-sqrt(fabs(initWindowVar[0]))-dx, east+sqrt(fabs(initWindowVar[2]))+dx)
      && terrainMap->withinValidMapRegion(north+sqrt(fabs(initWindowVar[0]))+dx, east-sqrt(fabs(initWindowVar[2]))-dx)
      && terrainMap->withinValidMapRegion(north-sqrt(fabs(initWindowVar[0]))-dx, east-sqrt(fabs(initWindowVar[2]))-dx))
      return true;  

   return false;
}

bool TNavFilter::withinValidMapRegionPoint(const double& north, const double& east)
{
	if(terrainMap->withinValidMapRegion(north,east))
		return true;
	
	return false;
}

bool TNavFilter::isConverged()
{
   return converged;
}

bool TNavFilter::findMeasSensorIndex(int measType, int &sensorIndex)
{
   sensorIndex = 0;

   //associate measurement with correct sensor type
   while((measType != vehicle->sensors[sensorIndex].type) & 
         (sensorIndex < vehicle->numSensors))
      sensorIndex++;
  
   //if there was no corresponding sensor for this vehicle,
   //return false
   if(sensorIndex >= vehicle->numSensors)
      return false;

   return true;  
}

void TNavFilter::applyVecRotation(const double* attitude, double* vecVF, double* vecIF)
{
   double cphi = cos(attitude[0]);
   double sphi = sin(attitude[0]);
   double ctheta = cos(attitude[1]);
   double stheta = sin(attitude[1]);
   double cpsi = cos(attitude[2]);
   double spsi = sin(attitude[2]);
   double stheta_sphi = stheta*sphi;
   double stheta_cphi = stheta*cphi;

   vecIF[0] = cpsi*ctheta*vecVF[0] + (-spsi*cphi + cpsi*stheta_sphi)*vecVF[1] + 
      (spsi*sphi + cpsi*stheta_cphi)*vecVF[2];
   vecIF[1] = spsi*ctheta*vecVF[0] + (cpsi*cphi + spsi*stheta_sphi)*vecVF[1] +
      (-cpsi*sphi + spsi*stheta_cphi)*vecVF[2];
   vecIF[2] = -stheta*vecVF[0] + ctheta*sphi*vecVF[1] + ctheta*cphi*vecVF[2];
}

void TNavFilter::applyVecRotationIFtoVF(const double* attitude, double* vecVF, double* vecIF)
{
   double cphi = cos(attitude[0]);
   double sphi = sin(attitude[0]);
   double ctheta = cos(attitude[1]);
   double stheta = sin(attitude[1]);
   double cpsi = cos(attitude[2]);
   double spsi = sin(attitude[2]);
   double stheta_sphi = stheta*sphi;
   double stheta_cphi = stheta*cphi;

   vecVF[0] = cpsi*ctheta*vecIF[0] + spsi*ctheta*vecIF[1] - stheta*vecIF[2];
   vecVF[1] = (-spsi*cphi + cpsi*stheta_sphi)*vecIF[0] + (cpsi*cphi + spsi*stheta_sphi)*vecIF[1] +
      ctheta*sphi*vecIF[2];
   vecVF[2] = (spsi*sphi + cpsi*stheta_cphi)*vecIF[0] + (-cpsi*sphi + spsi*stheta_cphi)*vecIF[1] + ctheta*cphi*vecIF[2];
}

Matrix TNavFilter::applyRotation(const double* attitude,  const Matrix &beamsVF)
{
   Matrix beamsMF = beamsVF;  
   double cphi = cos(attitude[0]);
   double sphi = sin(attitude[0]);
   double ctheta = cos(attitude[1]);
   double stheta = sin(attitude[1]);
   double cpsi = cos(attitude[2]);
   double spsi = sin(attitude[2]);
   double stheta_sphi = stheta*sphi;
   double stheta_cphi = stheta*cphi;
   int i;

   double R11 = cpsi*ctheta;
   double R12 = spsi*ctheta;
   double R13 = -stheta;
   double R21 = -spsi*cphi + cpsi*stheta_sphi;
   double R22 = cpsi*cphi + spsi*stheta_sphi;
   double R23 = ctheta*sphi;
   double R31 = spsi*sphi + cpsi*stheta_cphi;
   double R32 = -cpsi*sphi + spsi*stheta_cphi;
   double R33 = ctheta*cphi;

   for(i = 1; i <= beamsVF.Ncols(); i++)
   {
      beamsMF(1,i) = R11*beamsVF(1,i) + R21*beamsVF(2,i) + R31*beamsVF(3,i);
      
      beamsMF(2,i) = R12*beamsVF(1,i) + R22*beamsVF(2,i) + R32*beamsVF(3,i);
      
      beamsMF(3,i) = R13*beamsVF(1,i) + R23*beamsVF(2,i) + R33*beamsVF(3,i);
   }

   return beamsMF;
}

bool TNavFilter::getTerrainMap(mapT &currMap)
{
   if(terrainMap->map.xpts != NULL)
      currMap = terrainMap->map;
   else
      return false;

   return true;
}

bool TNavFilter::getTerrainMapBounds(double *currMapBounds)
{
   if(terrainMap->map.xpts != NULL)
   {
      currMapBounds[0] = terrainMap->map.xpts[0];
      currMapBounds[1] = terrainMap->map.xpts[terrainMap->map.numX-1];
      currMapBounds[2] = terrainMap->map.ypts[0];
      currMapBounds[3] = terrainMap->map.ypts[terrainMap->map.numY-1];
   }
   else
      return false;

   return true;
}


void TNavFilter::increaseInitSearchWin(double *windowVarIncrement)
{
   int i;
   for(i = 0; i < 36; i++)
      this->initWindowVar[i] += windowVarIncrement[i];

   return;
}


void TNavFilter::initVariables()
{   
   vehicle->displayVehicleInfo();
   lastNavPose = NULL;
   interpMapMethod = 0;
   interpMeasAttitude = false;
   converged = false;
   timeLastDvlValid = 0;
   measVariance = -1;
   totalAttemptTime = 0;
   int i;
   for(i = 0; i < 3; i++)
      currentVel[i] = 0.0;

   //initialize distribution type to Uniform
   initDistribType = 0;

#ifdef USE_MATLAB
   //start Matlab engine
   if (!(matlabEng = engOpen("\0"))) 
   {
      fprintf(stderr, "\nCan't start MATLAB engine\n");
      exit(0);
   }
#endif

   //determine dvl sensor attitude
   int sensorIndex = 0;

   //associate measurement with correct sensor type
   while((1 != vehicle->sensors[sensorIndex].type) & 
         (sensorIndex < vehicle->numSensors))
      sensorIndex++;
  
   dvlAttitude = vehicle->T_sv[sensorIndex].rotation;
   dvlRotMatrix = getRotMatrix(dvlAttitude);

}

bool TNavFilter::projectMeasVF(Matrix &beamsVF, measT& currMeas)
{
   Matrix dr_sv(3,1);
   Matrix Rsv;
   Matrix beamsSF(3, currMeas.numMeas);
   int measSensor = 0;
   int i;

   //check that a valid sensor index can be found for current measurement
   if(!findMeasSensorIndex(currMeas.dataType, measSensor))
   { 
      output("TerrainNav:: Invalid measurement type. Unable to add"
	     " measurement\n");
      return false;
   }

   //project beams into sensor frame
   if(projectMeasSF(beamsSF, currMeas))
   {
      //project beams into vehicle frame
      //--------------------------------------------------------------------------
      Rsv = getRotMatrix(vehicle->T_sv[measSensor].rotation);
      dr_sv << vehicle->T_sv[measSensor].dr;
      
      beamsVF.ReSize(3,beamsSF.Ncols());
      for(i = 1; i <= beamsSF.Ncols(); i++)
         beamsVF.SubMatrix(1,3,i,i) = Rsv.t()*beamsSF.SubMatrix(1,3,i,i) + dr_sv;

      return true;
   }
   else
      return false;
}

bool TNavFilter::projectMeasSF(Matrix &beamsSF, measT& currMeas)
{
   int measSensor = 0;
   int numGoodBeams = 0;
   int i;
   double theta, psi;
   Matrix dr_bs(3,1);
    
   //check that a valid sensor index can be found for current measurement
   if(!findMeasSensorIndex(currMeas.dataType, measSensor))
   { 
      output("TerrainNav:: Invalid measurement type. Unable to add"
	     " measurement\n");
      return false;
   }

   //translational vector between beam and sensor frames
   dr_bs << vehicle->sensors[measSensor].T_bs[0].dr;

   //project beams into sensor frame
   //---------------------------------------------------------------------------
   for(i = 1; i <= currMeas.numMeas; i++)
   {
      //only include beams that are reasonable/good
      if(!currMeas.measStatus[i-1])
         continue;

      switch(currMeas.dataType)
      {
      case 1:
         theta = vehicle->sensors[measSensor].T_bs[i-1].rotation[1];
         psi = vehicle->sensors[measSensor].T_bs[i-1].rotation[2];
	  
         //project beams into sensor frame
         beamsSF(1,numGoodBeams+1) = sin(theta)*cos(psi)*currMeas.ranges[i-1];
         beamsSF(2,numGoodBeams+1) = sin(theta)*sin(psi)*currMeas.ranges[i-1];
         beamsSF(3,numGoodBeams+1) = cos(theta)*currMeas.ranges[i-1];

         //overwrite measT data to ensure first numGoodBeams entries are valid
         if(i != numGoodBeams+1)
            currMeas.ranges[numGoodBeams] = currMeas.ranges[i-1];
         break;

      case 2:
         //ignore beams with missing/NaN data values
         /*if(isnan(currMeas.crossTrack[i-1]))
           continue;*/
	  
         beamsSF(1,numGoodBeams+1) = currMeas.alongTrack[i-1];
         beamsSF(2,numGoodBeams+1) = currMeas.crossTrack[i-1];
         beamsSF(3,numGoodBeams+1) = currMeas.altitudes[i-1];

         //overwrite measT data to ensure first numGoodBeams entries are valid
         if(i != numGoodBeams+1)
         {
            currMeas.alongTrack[numGoodBeams] = currMeas.alongTrack[i-1];
            currMeas.crossTrack[numGoodBeams] = currMeas.crossTrack[i-1];
            currMeas.altitudes[numGoodBeams] = currMeas.altitudes[i-1];
         }     
         break;

      case 3:
         beamsSF(1,numGoodBeams+1) = cos(currMeas.theta)*currMeas.ranges[0];
         beamsSF(2,numGoodBeams+1) = 0;
         beamsSF(3,numGoodBeams+1) = sin(currMeas.theta)*currMeas.ranges[0];
         break;

      default:        
         output("TerrainNav::Invalid measurement type specified. Exiting...\n");
         exit(0);
         break;
      }
	
      //translation component of transformation into sensor frame
      beamsSF.SubMatrix(1,3,numGoodBeams+1,numGoodBeams+1) += dr_bs;

      //overwrite measT data to ensure first numGoodBeams entries are all valid
      if(i != numGoodBeams+1)
      {
         currMeas.covariance[numGoodBeams] = currMeas.covariance[i-1];
         currMeas.measStatus[numGoodBeams] = currMeas.measStatus[i-1];
      }

      numGoodBeams++;
   }

   //If no beams were valid, return false
   if(numGoodBeams == 0)
   {
      output("TerrainNav::Measurement from time = %.2f sec. not included; there"
             " are no good beams from the sonar.\n", currMeas.time);
      return false;
   }

   //If some beams were rejected, display message to the screen
   if(numGoodBeams != currMeas.numMeas)
      output("TerrainNav::Excluded %i beam(s) from correlation due to poor"
             " sonar data.\n", currMeas.numMeas - numGoodBeams);
   
   //Take only subset of beamsSF that has valid beams.
   beamsSF = beamsSF.Columns(1,numGoodBeams);
   
   return true;
}

Matrix TNavFilter::getRotMatrix(const double* attitude)
{
   Matrix R(3,3);
   double cphi = cos(attitude[0]);
   double sphi = sin(attitude[0]);
   double ctheta = cos(attitude[1]);
   double stheta = sin(attitude[1]);
   double cpsi = cos(attitude[2]);
   double spsi = sin(attitude[2]);
   double stheta_sphi = stheta*sphi;
   double stheta_cphi = stheta*cphi;

   R(1,1) = cpsi*ctheta;
   R(1,2) = spsi*ctheta;
   R(1,3) = -stheta;
   R(2,1) = -spsi*cphi + cpsi*stheta_sphi;
   R(2,2) = cpsi*cphi + spsi*stheta_sphi;
   R(2,3) = ctheta*sphi;
   R(3,1) = spsi*sphi + cpsi*stheta_cphi;
   R(3,2) = -cpsi*sphi + spsi*stheta_cphi;
   R(3,3) = ctheta*cphi;

   return R;
}


Matrix TNavFilter::applyDVLrotation(const Matrix &beamsSF)
{
   Matrix beamsVF = beamsSF;  
   int i;

   for(i = 1; i <= beamsSF.Ncols(); i++)
   {
      beamsVF(1,i) = dvlRotMatrix(1,1)*beamsSF(1,i) + dvlRotMatrix(2,1)*beamsSF(2,i) + dvlRotMatrix(3,1)*beamsSF(3,i);
      
      beamsVF(2,i) = dvlRotMatrix(1,2)*beamsSF(1,i) + dvlRotMatrix(2,2)*beamsSF(2,i) + dvlRotMatrix(3,2)*beamsSF(3,i);
      
      beamsVF(3,i) = dvlRotMatrix(1,3)*beamsSF(1,i) + dvlRotMatrix(2,3)*beamsSF(2,i) + dvlRotMatrix(3,3)*beamsSF(3,i);
   }

   return beamsVF;
}


void TNavFilter::modifyBeamDir(Matrix &beamsMF, const double* vehPose)
{
   int i;
   Matrix H(1,2);
   double mapIntersect[3];   
   double n[3];
   double p, phi, alpha, d;
   double norm, normU;
   double testVal, zVar;
   double minorAxis[3];
   double majorAxis[3];
   double u[3];

   for(i = 1; i <= beamsMF.Ncols(); i++)
   {
      //find initial mapIntersect in N,E,D frame
      mapIntersect[0] = beamsMF(1,i)+vehPose[0];
      mapIntersect[1] = beamsMF(2,i)+vehPose[1];
      mapIntersect[2] = 0.0;
      
      //Assess terrain normal
      interpolateDepthAndGradient(mapIntersect[0], mapIntersect[1], 
                                  mapIntersect[2], zVar, H); 
      if(isnan(H(1,1)) || isnan(H(1,2)))
         continue;

      n[0] = H(1,1);
      n[1] = H(1,2);
      n[2] = -1.0;

      //normalize terrain normal
      norm = sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);
      n[0] = n[0]/norm;
      n[1] = n[1]/norm;
      n[2] = n[2]/norm;
            
      //Define original norm
      normU = sqrt(beamsMF(1,i)*beamsMF(1,i) + beamsMF(2,i)*beamsMF(2,i)
                   + beamsMF(3,i)*beamsMF(3,i));
    
      //Determine if normal vector is within the beam cone (if yes,
      //this is the closest-direction.
      testVal = -n[0]*beamsMF(1,i)-n[1]*beamsMF(2,i) -n[2]*beamsMF(3,i) - 
         normU*cos(vehicle->sensors[0].beamWidth/2.0);
     
      if(testVal >= 0)
      {
         p = -((mapIntersect[0]-vehPose[0])*n[0] + 
               (mapIntersect[1]-vehPose[1])*n[1] +
               (mapIntersect[2]-vehPose[2])*n[2]);

         //reweight beamsMF to have same length as before
         beamsMF(1,i) = -n[0]*normU;
         beamsMF(2,i) = -n[1]*normU;
         beamsMF(3,i) = -n[2]*normU;
         continue;
      }

      //If closest point is not within the beam cone, we will use
      //the closest point within the intersection ellipse
      
      //minorAxis = nxu,  majorAxis = minorUxn
      u[0] = beamsMF(1,i)/normU;
      u[1] = beamsMF(2,i)/normU;
      u[2] = beamsMF(3,i)/normU;

      //compute minor axis
      computeArrayCrossProd(n,u,minorAxis);
      norm = sqrt(minorAxis[0]*minorAxis[0] + minorAxis[1]*minorAxis[1] +
                  minorAxis[2]*minorAxis[2]);
      minorAxis[0] = minorAxis[0]/norm;
      minorAxis[1] = minorAxis[1]/norm;
      minorAxis[2] = minorAxis[2]/norm;
      
      //compute major axis
      computeArrayCrossProd(minorAxis,n,majorAxis);
      norm = sqrt(majorAxis[0]*majorAxis[0] + majorAxis[1]*majorAxis[1] +
                  majorAxis[2]*majorAxis[2]);
      majorAxis[0] = majorAxis[0]/norm;
      majorAxis[1] = majorAxis[1]/norm;
      majorAxis[2] = majorAxis[2]/norm;
      
      //determine angle between u and major axis
      phi = acos(-(u[0]*majorAxis[0] + u[1]*majorAxis[1] + u[2]*majorAxis[2]));
      if(phi > PI/2)
      {
         majorAxis[0] = -majorAxis[0];
         majorAxis[1] = -majorAxis[1];
         majorAxis[2] = -majorAxis[2];
         phi = PI-phi;
      }
      
      //determine length of major axis
      alpha = PI-phi-vehicle->sensors[0].beamWidth/2.0;
      d = sin(vehicle->sensors[0].beamWidth/2.0)*normU/sin(alpha);
      
      //redefine beamsMF
      beamsMF(1,i) += d*majorAxis[0];
      beamsMF(2,i) += d*majorAxis[1];
      beamsMF(3,i) += d*majorAxis[2];
      
      //reweight beamsMF to have same length as before
      norm = sqrt(beamsMF(1,i)*beamsMF(1,i) + beamsMF(2,i)*beamsMF(2,i)
              + beamsMF(3,i)*beamsMF(3,i));
      beamsMF(1,i) = beamsMF(1,i)*normU/norm;
      beamsMF(2,i) = beamsMF(2,i)*normU/norm;
      beamsMF(3,i) = beamsMF(3,i)*normU/norm;
   }  
}


void TNavFilter::modifyBeamDirExtensive(Matrix &beamsMF, const double* vehPose)
{
   int i,j,k;
   int indexX, indexY;
   double norm, normU;
   double testVal, r, var;
   double u[3];
   double u_new[3];
   double nearestPt[3];
   double mapIntersect[3];
   double testPt[3];
   double minDist = 0;

   for(i = 1; i <= beamsMF.Ncols(); i++)
   { 
      //Define original norm
      normU = sqrt(beamsMF(1,i)*beamsMF(1,i) + beamsMF(2,i)*beamsMF(2,i)
                   + beamsMF(3,i)*beamsMF(3,i));
      //Define normalized direction vector
      u[0] = beamsMF(1,i)/normU;
      u[1] = beamsMF(2,i)/normU;
      u[2] = beamsMF(3,i)/normU;

      //find initial mapIntersect in N,E,D frame
      computeMapRayIntersection(vehPose, u, r,var);
      mapIntersect[0] = u[0]*r + vehPose[0];
      mapIntersect[1] = u[1]*r + vehPose[1];
      mapIntersect[2] = u[2]*r + vehPose[2];
      indexX = closestPtUniformArray(mapIntersect[0], terrainMap->map.xpts[0], 
                                     terrainMap->map.xpts[terrainMap->map.numX-1],terrainMap->map.numX);
      indexY = closestPtUniformArray(mapIntersect[1], terrainMap->map.ypts[0], 
                                     terrainMap->map.ypts[terrainMap->map.numY-1],terrainMap->map.numY);
      //output("indexX: %i, indexY: %i, range: %f\n", indexX, indexY, normU);

      //Find closest point in the terrain within beam cone
      nearestPt[0] = terrainMap->map.xpts[0] - vehPose[0];
      nearestPt[1] = terrainMap->map.ypts[0] - vehPose[1];
      nearestPt[2] = terrainMap->map.depths(1,1) - vehPose[2];
      minDist = sqrt(nearestPt[0]*nearestPt[0] + nearestPt[1]*nearestPt[1]
                     + nearestPt[2]*nearestPt[2]);
      for(j = max(indexX-int(normU),0); j < indexX+int(normU) && 
             j < terrainMap->map.numX; j++)
      {
         for(k = max(indexY-int(normU),0); k < indexY+int(normU) && 
                k < terrainMap->map.numY; k++)
         {
            testPt[0] = terrainMap->map.xpts[j] - vehPose[0];
            testPt[1] = terrainMap->map.ypts[k] - vehPose[1];
            testPt[2] = terrainMap->map.depths(j+1,k+1)-vehPose[2];
            norm = sqrt(testPt[0]*testPt[0] + testPt[1]*testPt[1]
                        + testPt[2]*testPt[2]);

            //check if test point is within the cone
            testVal = testPt[0]*u[0] + testPt[1]*u[1] + testPt[2]*u[2] - 
               norm*cos(vehicle->sensors[0].beamWidth/2.0);
            if(testVal >= 0)
            {
               if(norm < minDist)
               {
                  minDist = norm;
                  nearestPt[0] = testPt[0];
                  nearestPt[1] = testPt[1];
                  nearestPt[2] = testPt[2];
               }
            }
         }
      }

      u_new[0] = nearestPt[0]/minDist;
      u_new[1] = nearestPt[1]/minDist;
      u_new[2] = nearestPt[2]/minDist;

      beamsMF(1,i) = u_new[0]*normU; 
      beamsMF(2,i) = u_new[1]*normU;
      beamsMF(3,i) = u_new[2]*normU;
   }

}


int TNavFilter::loadSubMap(const double xcen, const double ycen, 
			   double* mapWidth)
{
   int mapStatus = this->terrainMap->extractSubMap(xcen, ycen, mapWidth);

   switch(mapStatus) {

   case MAPBOUNDS_OUT_OF_BOUNDS:     
      output("TerrainNav:: Vehicle is operating outside of the given reference"
             " map.\n");
      break;

   case MAPBOUNDS_OK: 
      break;

   case MAPBOUNDS_NEAR_EDGE:
      output("TerrainNav:: Vehicle is operating near the reference map boundary"
             "; correlation area may be truncated\n");     
      break;
      
   default:
      output("TerrainNav:: No valid map status code returned from extract map"
             " function\n");
      mapStatus = MAPBOUNDS_OUT_OF_BOUNDS;
      break;      
   }

   return mapStatus;

}


bool TNavFilter::computeMapRayIntersection(const double* position, double *u, double &r, 
                                           double &var)
{
   double length, diff, z;
   double xI[3];
   int maxIter = 100;
   int numIter;
   double tol = 0.001;
   xI[0] = position[0];
   xI[1] = position[1];
   xI[2] = position[2];
   r = 0;

   //verify that u has unit length
   length = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
   if(length != 1.0)
   {
      u[0] = u[0]/length;
      u[1] = u[1]/length;
      u[2] = u[2]/length;
   }

   //compute initial intersection with the terrain and delta_z difference
   interpolateDepth(xI[0],xI[1],z,var);
   diff = xI[2]-fabs(z);
   numIter = 1;
   
   //iterate until converged or until max iterations
   while(fabs(diff) > tol && numIter < maxIter)
   {
      //step along direction vector 
      xI[0] -= diff*u[0];
      xI[1] -= diff*u[1];
      xI[2] -= diff*u[2];
      
      //recalculate terrain intersection
      interpolateDepth(xI[0],xI[1],z,var);

      //if interpolated depth is NaN, return false
      if(isnan(z))
      {
         r = fabs(z);
         return false;
      }

      diff = xI[2]-fabs(z);
      numIter++;
   }
  
   /*if(numIter == maxIter)
   {
      output("reached max iter; diff = %f \n", diff);
      exit(0);
      }*/
  
   r = sqrt((xI[0]-position[0])*(xI[0]-position[0]) +
            (xI[1]-position[1])*(xI[1]-position[1]) +
            (xI[2]-position[2])*(xI[2]-position[2]));

   return true;
}

void TNavFilter::interpolateDepth(double xi, double yi, double &zi, double &var)
{
   //Check that a map has been extracted
   if((terrainMap->map.xpts == NULL))
   {
      output("ERROR: tried to access map values without first extracting map"
	     " information");
      return;
   }  

   ColumnVector W;
   int* xIndices = NULL;
   int* yIndices = NULL;
   int numPts;
   double h_sq;

   //define pointer to an interpolation function
   void (*pt2interpFunction)(double*, double*, const Matrix&, double, double, 
                             double&, int*, int*, ColumnVector&) = NULL;

   //determine number of interpolation points and proper interpolation function
   switch(this->interpMapMethod)
   {
   case 0:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;

   case 1:
      numPts = 4;
      pt2interpFunction = &bilinearInterp;
      break;

   case 2:
      numPts = 16;
      pt2interpFunction = &bicubicInterp;
      break;

   case 3:
      numPts = 16;
      pt2interpFunction = &splineInterp;
      break;

   default:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;
   }

   //initialize indices arrays and Weights matrix
   xIndices = new int[numPts];
   yIndices = new int[numPts];
   W.ReSize(numPts);

   //Perform interpolation
   (*pt2interpFunction)(terrainMap->map.xpts, terrainMap->map.ypts, 
                        terrainMap->map.depths, xi, yi, zi,
                        xIndices, yIndices, W);

   //If returned depth is NaN, extract data from low resolution map
   if(isnan(zi) && terrainMap->refMap->lowResSrc != NULL)
   {
      double xPt, yPt;
      zi = terrainMap->getNearestLowResMapPoint(xi,yi,xPt,yPt);
      h_sq = pow(xPt - xi,2) + pow(yPt - yi,2);
      var = terrainMap->map.depthVariance(xIndices[0]+1,yIndices[0]+1)+
         terrainMap->mapVariogram.evalVariogram(sqrt(h_sq));
   }
   else
   {
      //Compute variance associated with interpolation method 
      if(W.Nrows() == 1)
      {
         //If only using nearest neighbor, weight variance based on distance of 
         //nearest point to the interpolation point
         h_sq = pow(terrainMap->map.xpts[xIndices[0]] - xi,2) + 
            pow(terrainMap->map.ypts[yIndices[0]] - yi,2);
         var = terrainMap->map.depthVariance(xIndices[0]+1,yIndices[0]+1)+
            terrainMap->mapVariogram.evalVariogram(sqrt(h_sq));
      }
      else
         var = computeInterpDepthVariance(xIndices, yIndices, W);
   }

   delete [] xIndices;
   delete [] yIndices;
   return;
}

void TNavFilter::interpolateDepthMat(double* xi, double* yi, Matrix &zi, 
				     Matrix &var)
{
   //Check that a map has been extracted
   if((terrainMap->map.xpts == NULL))
   {
      output("ERROR: tried to access map values without first extracting map"
	     " information");
      return;
   }  

   int N = zi.Nrows();
   int M = zi.Ncols();
   int numPts, i, j;
   ColumnVector W;
   int* xIndices = NULL;
   int* yIndices = NULL;
   double h_sq;
   
   //define pointer to an interpolation function
   void (*pt2interpFunction)(double*, double*, const Matrix&, double, double, 
                             double&, int*, int*, ColumnVector&) = NULL;

   //determine number of interpolation points and proper interpolation function
   switch(this->interpMapMethod)
   {
   case 0:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;

   case 1:
      numPts = 4;
      pt2interpFunction = &bilinearInterp;
      break;

   case 2:
      numPts = 16;
      pt2interpFunction = &bicubicInterp;
      break;

   case 3:
      numPts = 16;
      pt2interpFunction = &splineInterp;
      break;

   default:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;
   }

   //initialize indices arrays and Weights matrix
   xIndices = new int[numPts];
   yIndices = new int[numPts];
   W.ReSize(numPts);

   for(i = 0; i < N; i++)
   {
      for(j = 0; j < M; j++)
      {
         (*pt2interpFunction)(terrainMap->map.xpts, terrainMap->map.ypts, 
                              terrainMap->map.depths, xi[i], yi[j], zi(i+1,j+1)
                              , xIndices, yIndices, W);
         //If returned depth is NaN, extract data from low resolution map
         if(isnan(zi(i+1,j+1)) && terrainMap->refMap->lowResSrc != NULL)
         {
            double xPt, yPt;
            zi(i+1,j+1) = terrainMap->getNearestLowResMapPoint(xi[i],yi[j],
                                                               xPt,yPt);
            h_sq = pow(xPt - xi[i],2) + pow(yPt - yi[j],2);
            var(i+1,j+1) = terrainMap->map.depthVariance(xIndices[0]+1,
                                                          yIndices[0]+1)+
               terrainMap->mapVariogram.evalVariogram(sqrt(h_sq));
         }
         else
         {
            //Compute variance associated with interpolation method 
            if(W.Nrows() == 1)
            {
               //If only using nearest neighbor, weight variance based on 
               //distance of nearest point to the interpolation point
               h_sq = pow(terrainMap->map.xpts[xIndices[0]] - xi[i],2) + 
                  pow(terrainMap->map.ypts[yIndices[0]] - yi[j],2);
               var(i+1,j+1) = terrainMap->map.depthVariance(xIndices[0]+1,
                                                             yIndices[0]+1)
                  + terrainMap->mapVariogram.evalVariogram(sqrt(h_sq));
            }
            else
               var(i+1,j+1) = computeInterpDepthVariance(xIndices, yIndices, W);
         }
      }
   }
   delete [] xIndices;
   delete [] yIndices;

   return;
}


double TNavFilter::computeInterpDepthVariance(int* xIndices, int* yIndices, 
                                              ColumnVector Weights)
{
   double var;
   Matrix varValue;
   int N = Weights.Nrows();
   SymmetricMatrix VarMat(N);
   ColumnVector VarVec(N);
   double dx, dy, hsq, z1, z2;
   int i, j;

   VarMat = 0.0;

   for(i = 0; i < N; i++)
   {
      VarVec(i+1) = terrainMap->map.depthVariance(xIndices[i]+1,yIndices[i]+1);
      z1 = terrainMap->map.depths(xIndices[i]+1,yIndices[i]+1);
      
      //Compute cross-variance terms using variogram
      for(j = i; j < N; j++)
      {
         dx = terrainMap->map.xpts[xIndices[i]]-
            terrainMap->map.xpts[xIndices[j]];
         dy = terrainMap->map.ypts[yIndices[i]]-
            terrainMap->map.ypts[yIndices[j]];
         z2 = terrainMap->map.depths(xIndices[j]+1,yIndices[j]+1);
         hsq = dx*dx + dy*dy;
         VarMat(i+1,j+1) = 0.5*pow(z1-z2,2)-
            terrainMap->mapVariogram.evalVariogram(sqrt(hsq));

         if(isnan(VarMat(i+1,j+1)))
            VarMat(i+1,j+1) = 0.0;
      }
   }

   //compute total variance value for current map point
   varValue = Weights.t()*VarVec + Weights.t()*VarMat*Weights;
   var = varValue.AsScalar(); 

   //check that the variance is positive and finite
   if(isnan(var) || var < 0)
   {
      varValue = Weights.t()*VarVec;
      var = varValue.AsScalar();
   }

   return var;
}


void TNavFilter::interpolateGradient(double xi, double yi, Matrix &gradient)
{
   ColumnVector W;
   int* xIndices = NULL;
   int* yIndices = NULL;
   int numPts = 0;
   double zi;

   //define pointer to an interpolation function
   void (*pt2interpFunction)(double*, double*, const Matrix&, double, double, 
                             double&, int*, int*, ColumnVector&) = NULL;

   //determine number of interpolation points and proper interpolation function
   switch(this->interpMapMethod)
   {
   case 0:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;

   case 1:
      numPts = 4;
      pt2interpFunction = &bilinearInterp;
      break;

   case 2:
      numPts = 16;
      pt2interpFunction = &bicubicInterp;
      break;

   case 3:
      numPts = 16;
      pt2interpFunction = &splineInterp;
      break;

   default:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;
   }

   //initialize indices arrays and Weights matrix
   xIndices = new int[numPts];
   yIndices = new int[numPts];
   W.ReSize(numPts);

   //Perform interpolation to find xIndices and yIndices
   (*pt2interpFunction)(terrainMap->map.xpts, terrainMap->map.ypts, 
                        terrainMap->map.depths, xi, yi, zi,
                        xIndices, yIndices, W);

   //Compute terrain gradient based on interpolation scheme and returned 
   //weights/indices
   computeInterpTerrainGradient(xIndices, yIndices, xi, yi, gradient);

   delete [] xIndices;
   delete [] yIndices;
}


void TNavFilter::interpolateDepthAndGradient(double xi, double yi, double &zi,
                                             double &var, Matrix &gradient)
{
   ColumnVector W;
   int* xIndices = NULL;
   int* yIndices = NULL;
   int numPts = 0;
   double h_sq;

   //define pointer to an interpolation function
   void (*pt2interpFunction)(double*, double*, const Matrix&, double, double, 
                             double&, int*, int*, ColumnVector&) = NULL;

   //determine number of interpolation points and proper interpolation function
   switch(this->interpMapMethod)
   {
   case 0:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;

   case 1:
      numPts = 4;
      pt2interpFunction = &bilinearInterp;
      break;

   case 2:
      numPts = 16;
      pt2interpFunction = &bicubicInterp;
      break;

   case 3:
      numPts = 16;
      pt2interpFunction = &splineInterp;
      break;

   default:
      numPts = 1;
      pt2interpFunction = &nearestInterp;
      break;
   }

   //initialize indices arrays and Weights matrix
   xIndices = new int[numPts];
   yIndices = new int[numPts];
   W.ReSize(numPts);

   //Perform interpolation to find zi
   (*pt2interpFunction)(terrainMap->map.xpts, terrainMap->map.ypts, 
                        terrainMap->map.depths, xi, yi, zi,
                        xIndices, yIndices, W);

   //If returned depth is NaN, extract data from low resolution map
   if(isnan(zi) && terrainMap->refMap->lowResSrc != NULL)
   {
      double xPt, yPt;
      zi = terrainMap->getNearestLowResMapPoint(xi,yi,xPt,yPt);
      h_sq = pow(xPt - xi,2) + pow(yPt - yi,2);
      var = terrainMap->map.depthVariance(xIndices[0]+1,yIndices[0]+1)+
         terrainMap->mapVariogram.evalVariogram(sqrt(h_sq));
   }
   else
   {
      //Compute variance associated with interpolation method 
      if(W.Nrows() == 1)
      {
         //If only using nearest neighbor, weight variance based on distance of 
         //nearest point to the interpolation point
         h_sq = pow(terrainMap->map.xpts[xIndices[0]] - xi,2) + 
            pow(terrainMap->map.ypts[yIndices[0]] - yi,2);
         var = terrainMap->map.depthVariance(xIndices[0]+1,yIndices[0]+1)+
            terrainMap->mapVariogram.evalVariogram(sqrt(h_sq));
      }
      else
         var = computeInterpDepthVariance(xIndices, yIndices, W);
   }

   //Compute terrain gradient based on interpolation scheme and returned 
   //weights/indices
   computeInterpTerrainGradient(xIndices, yIndices, xi, yi, gradient);

   delete [] xIndices;
   delete [] yIndices;
}


void TNavFilter::computeInterpTerrainGradient(int* xIndices, int* yIndices, 
                                              double xi, double yi,
                                              Matrix &gradient)
{
   double dx, dy;
   double b2, b3, b4;
   double z11, z12, z21, z22;

   dx = terrainMap->map.dx;
   dy = terrainMap->map.dy;

   //If using bilinear interpolation, compute terrain gradient based on the
   //bilinear interpolation function
   if(this->interpMapMethod == 1)
   {
      // The four points used for bilinear interp are numbered as:
      //          z11   z12
      //          z21   z22
      // ----------------------------------------------------      
      z11 = terrainMap->map.depths(xIndices[0]+1,yIndices[0]+1);
      z21 = terrainMap->map.depths(xIndices[1]+1,yIndices[1]+1);
      z12 = terrainMap->map.depths(xIndices[2]+1,yIndices[2]+1);
      z22 = terrainMap->map.depths(xIndices[3]+1,yIndices[3]+1);

      // The resulting bilinear interpolation function is given
      // by:
      //        z(x,y)_interp = b1 + b2x + b3y + b4xy
      // where b2:b4 are defined as (don't need b1 for gradient):
      // ----------------------------------------------------      
      b2 = (1/(dx*dy))*(yIndices[0]*(z12-z22) + 
                        yIndices[1]*(z21-z11));
      b3 = (1/(dx*dy))*(xIndices[0]*(z21-z22) + 
                        xIndices[2]*(z12-z11));
      b4 = (1/(dx*dy))*(z11-z12-z21+z22);

      // The associated derivatives are:
      //        dz/dx = b2 + b4y
      //        dz/dy = b3 + b4x
      // ----------------------------------------------------      
      gradient(1,1) = b2 + b4*yi;
      gradient(1,2) = b3 + b4*xi;    
   }

   //Use simple forward/backward differencing for gradient calculation
   else      
   {
      //Compute X gradient
      //If we are at the lower bound, use a forward difference
      if(xIndices[0] == 0)
         gradient(1,1) = (1.0/dx)*(terrainMap->map.depths(xIndices[0]+2,yIndices[0]+1) - terrainMap->map.depths(xIndices[0]+1, yIndices[0]+1));                 
      else
      {
         //If we are at the upper bound, use a backward difference, 
         //otherwise do central difference
         if(xIndices[0] == terrainMap->map.numX-1)
            gradient(1,1) = (1.0/dx)*(terrainMap->map.depths(xIndices[0]+1,yIndices[0]+1) - terrainMap->map.depths(xIndices[0],yIndices[0]+1)); 
         else
         {
            gradient(1,1) = (1.0/(2.0*dx))*(terrainMap->map.depths(xIndices[0]+2,yIndices[0]+1) - terrainMap->map.depths(xIndices[0],yIndices[0]+1));
         }
      }


      //Compute Y gradient
      //If we are at the lower bound, use a forward difference
      if(yIndices[0] == 0)
         gradient(1,2) = (1/dy)*(terrainMap->map.depths(xIndices[0]+1,yIndices[0]+2) - terrainMap->map.depths(xIndices[0]+1,yIndices[0]+1));                 
      else
      {
         //If we are at the upper bound, use a backward difference,
         //otherwise do central difference
         if(yIndices[0] == terrainMap->map.numY-1)
            gradient(1,2) = (1.0/dy)*(terrainMap->map.depths(xIndices[0]+1,yIndices[0]+1) - terrainMap->map.depths(xIndices[0]+1,yIndices[0])); 
         else
         {
            gradient(1,2) = (1.0/(2.0*dy))*(terrainMap->map.depths(xIndices[0]+1,yIndices[0]+2) - terrainMap->map.depths(xIndices[0]+1,yIndices[0]));
         }
      }      
   }


}
