/* FILENAME      : TerrainNav.cpp
 * AUTHOR        : Debbie Meduna
 * DATE          : 04/27/09
 * 
 * LAST MODIFIED : 11/30/10
 * MODIFIED BY   : Debbie Meduna
 * -----------------------------------------------------------------------------
 * Modification History
 * -----------------------------------------------------------------------------
 ******************************************************************************/

#include "TerrainNav.h"

/******************************************************************************
 TRANSITION MATRIX
 State 0: Well localized
 State 1: Localizing - medium uncertainty
 State 2: High uncertainty

 Trigger 0: North/East Variance < MIN_FILTER_VAR
 Trigger 1: MIN_FILTER_VAR+VAR_MARGIN < North/East Variance < MAX_FILTER_VAR-
            VAR_MARGIN and filter is either in state 0 or 2
 Trigger 2: North/East Variance > MAX_FILTER_VAR
 Trigger 3: Missing valid range measurements for > MAX_MEAS_OUTAGE OR
            Vehicle does not have bottom lock for > MAX_VEL_OUTAGE
 Trigger 4: Vehicle has been over flat terrain for too long
******************************************************************************/
//normal case
static int transitionMatrix[5][3] = {{0,0,1},{1,1,1},{1,2,2},{2,2,2},{2,2,2}};
//force reinit with well-converged
//static int transitionMatrix[5][3] = {{2,0,1},{1,1,1},{1,2,2},{2,2,2},{2,2,2}};


TerrainNav::TerrainNav(char *mapName)
{
   //initialize pointers
   this->mapFile = mapName;
   this->vehicleSpecFile = (char*)"mappingAUV_specs.cfg";
   this->saveDirectory = NULL;
   this->tNavFilter = NULL;
   this->filterType = 1;
   this->octreeMap = NULL;
   
   //initialize terrainNav private variables
   initVariables();

}

TerrainNav::TerrainNav(char *mapName, char *vehicleSpecs)
{
   //initialize pointers
   this->mapFile = mapName;
   this->vehicleSpecFile = vehicleSpecs;
   this->saveDirectory = NULL;
   this->tNavFilter = NULL;
   this->filterType = 1;
   this->octreeMap = NULL;
   
   //initialize terrainNav private variables
   initVariables();
}

TerrainNav::TerrainNav(char *mapName, char *vehicleSpecs, 
		       const int &filterType)
{
   //initialize pointers
   this->mapFile = mapName;
   this->vehicleSpecFile = vehicleSpecs;
   this->saveDirectory = NULL;
   this->tNavFilter = NULL;
   this->filterType = filterType;
   this->octreeMap = NULL;
     
   //initialize terrainNav private variables
   initVariables();
}

TerrainNav::TerrainNav(char *mapName, char *vehicleSpecs, 
		       const int &filterType, char *directory)
{
   //initialize pointers
   this->mapFile = mapName;
   this->vehicleSpecFile = vehicleSpecs;
   this->saveDirectory = directory;
   this->tNavFilter = NULL;
   this->filterType = filterType;
   this->octreeMap = NULL;
      
   //initialize terrainNav private variables
   initVariables();
}

TerrainNav::~TerrainNav()
{
   if(tNavFilter != NULL)
      delete tNavFilter;
   tNavFilter = NULL;
   
   if(octreeMap != NULL)
   		delete octreeMap;
   octreeMap = NULL;

   output("TerrainNav::Number of reinitializations: %i\n", numReinits);
}

void TerrainNav::estimatePose(poseT* estimate, const int &type)
{	
   //Cannot compute pose estimates if the filter motion has not been initialized
   if(tNavFilter->lastNavPose == NULL)
   {
      output("TerrainNav::Cannot compute pose estimate; motion has not been initialized.\n");
      return;
   }	

   switch(type)
   {
   case 1:
      tNavFilter->computeMLE(estimate);
      break;

   case 2:
      tNavFilter->computeMMSE(estimate);
      //If using a PMF, add on prior estNavOffset for attitude
      if(this->filterType == 1 && ALLOW_ATTITUDE_SEARCH)
      {
         estimate->phi = tNavFilter->lastNavPose->phi + estNavOffset.phi;
         estimate->theta = tNavFilter->lastNavPose->theta + estNavOffset.theta;
         estimate->psi = tNavFilter->lastNavPose->psi + estNavOffset.psi;
         estimate->wy = tNavFilter->lastNavPose->wy + estNavOffset.wz;
         estimate->wz = tNavFilter->lastNavPose->wz + estNavOffset.wz;
      }
      //Update current filter North/East variance
      tNavFilter->currVar[0] = estimate->covariance[0];
      tNavFilter->currVar[1] = estimate->covariance[2];
      //If estimate is confident, save INS predicted offset for reinit
      if(tNavFilter->currVar[0] < 100.0 && tNavFilter->currVar[1] < 100.0)
      {
         this->estNavOffset = *estimate;
         this->estNavOffset -= *this->tNavFilter->lastNavPose;
      }
      break;

   default:
      tNavFilter->computeMLE(estimate);
   }
   //TODO:***************NEED TO FIGURE THIS OUT!!!!! *********************
   //*estimate = *tNavFilter->lastNavPose;
   //*estimate += estNavOffset;

   return;   
}

void TerrainNav::measUpdate(measT* incomingMeas, const int &type) 
{
   //copy incoming measurement to current meas structure;
   measT currMeas;
   int i;
   currMeas = *incomingMeas;

   currMeas.dataType = type;

   //check validity of range data
   checkRangeValidity(currMeas);
  
   //If no motion updates have been performed (no navigation estimates included)
   //the measurement can not be added. 
   if(tNavFilter->lastNavPose == NULL)
   {
      //check if our current measurements are valid
      this->lastMeasValid = false;
      for(i = 0; i < currMeas.numMeas; i++)
      {
         if(currMeas.measStatus[i])
         {
            this->lastMeasValid = true;
            break;
         }
      }
      
      output("TerrainNav::Measurement type %i from time = %.2f sec. not included; "
             "vehicle motion has not been initialized.\n", currMeas.dataType, 
             currMeas.time);
      this->lastMeasSuccess = false;
      return;
   }

   //check if vehicle is within correlation map before including measurement
   if(!USE_OCTREE){
		 if(!this->tNavFilter->withinRefMap())
		 {
		    output("TerrainNav::Measurement type %i from time = %.2f sec. not included; "
		           "vehicle is operating outside the given reference maps.\n", 
		           currMeas.dataType, currMeas.time);
		    this->lastMeasSuccess = false;
		    return;
		 }
   }

   //Fill in the measurement variance based on range percent error
   computeMeasVariance(currMeas);

   //If the current measurement time is ahead of the latest navigation time, 
   //then we need to wait for more recent navigation data before adding the 
   //measurement.
   if((tNavFilter->lastNavPose->time < currMeas.time))
   {
      //add current measurement to the measurement buffer
      this->waitingMeas[this->numWaitingMeas] = currMeas;     
      this->numWaitingMeas++;

      output("TerrainNav::Delayed incorporating measurement type %i from time = %.2f"
             " sec.; waiting for more recent INS data...\n", currMeas.dataType, currMeas.time);
      return;
   }

   //If the current navigation time matches the measurement time, add the 
   //measurement.  Otherwise, ignore the measurement.
   if(tNavFilter->lastNavPose->time == currMeas.time)
   {
      this->lastMeasSuccess = tNavFilter->measUpdate(currMeas);
      if(this->lastMeasSuccess)
      {
         output("TerrainNav:: Measurement type %i successfully incorporated from"
                " time = %.2f sec.\n", currMeas.dataType, currMeas.time);     
         lastMeasSuccessTime = currMeas.time;
      }
      return;
   }
   else
      output("TerrainNav::Did not incorporate measurement type %i from time"
             "= %.2f sec.; no INS pose data available. \n", currMeas.dataType, currMeas.time);

   return;
}


void TerrainNav::motionUpdate(poseT* incomingNav)
{
   poseT currEstimate;
   double dt;
   
   currEstimate = *incomingNav;

   //try to initialize the filter if not already initialized
   if(tNavFilter->lastNavPose == NULL)
   {
      attemptInitFilter(currEstimate);
      return;
   }
   
   //check filter health before applying next motion update
   if(this->allowFilterReinits && !checkFilterHealth())
      return;
		 
   //estimate current acceleration based on delta v
   dt = currEstimate.time- tNavFilter->timeLastDvlValid;
   if(dt > 0)
   {
      currEstimate.ax = (currEstimate.vx - lastValidVel[0])/dt;
      currEstimate.ay = (currEstimate.vy - lastValidVel[1])/dt;
      currEstimate.az = (currEstimate.vz - lastValidVel[2])/dt;
   }

   //check validity of velocity data
   checkVelocityValidity(currEstimate);
   if(currEstimate.bottomLock && currEstimate.dvlValid)
      lastBottomLockTime = currEstimate.time;

   //if using a compass bias correction function, apply here:
   if(tNavFilter->compassBias != NULL)
      currEstimate.psi += -tNavFilter->compassBias->evalCompassBias(currEstimate.psi);
    
   //if dvl velocity data is bad, use last good velocity info
   if(!currEstimate.dvlValid)
   {
      currEstimate.vx = lastValidVel[0];
      currEstimate.vy = lastValidVel[1];
      currEstimate.vz = lastValidVel[2];
      currEstimate.bottomLock = lastVelBotLock;
   }
   else
   {
      //if we just lost bottom lock, reset current velocity estimate
      if(lastVelBotLock && !currEstimate.bottomLock)
      {
         poseT currEst;
         tNavFilter->computeMMSE(&currEst);
         double attitude[3] = {currEst.phi, currEst.theta, 
                               currEst.psi};
         //assuming attitude is ~constant over two time steps, can first 
         //compute estimated current velocity in the body frame and then rotate
         //into inertial.  (v_c = v_w - v_b, where v_c is current velocity,
         //v_w is water-relative velocity, v_b is bottom-relative velocity)
         double estWatVel[3] = {currEstimate.vx-lastValidVel[0], 
                                currEstimate.vy-lastValidVel[1],
                              currEstimate.vz-lastValidVel[2]};
         tNavFilter->applyVecRotation(attitude, estWatVel, 
                                      tNavFilter->currentVel);

         //reinitialize water velocity estimate if searching over water velocity
         if(SEARCH_WAT_VEL)
            ((TNavParticleFilter *)tNavFilter)->reinitWatVelDist(estWatVel);
      }
      lastValidVel[0] = currEstimate.vx;
      lastValidVel[1] = currEstimate.vy;
      lastValidVel[2] = currEstimate.vz;
      lastVelBotLock = currEstimate.bottomLock;
      tNavFilter->timeLastDvlValid = currEstimate.time;
   }

   /*
   //convert measured velocity to vehicle frame (account for vehicle rotation 
   //rate)
   int sensorIndx = 0;
   //look for the dvl sensor in the vehicle info
   if(tNavFilter->findMeasSensorIndex(1, sensorIndx))
   {
      double rx, ry, rz;
      rx = tNavFilter->vehicle->T_sv[sensorIndx].dr[0];
      ry = tNavFilter->vehicle->T_sv[sensorIndx].dr[1];
      rz = tNavFilter->vehicle->T_sv[sensorIndx].dr[2];
      
      currEstimate->vx += currEstimate->wy*rz - currEstimate->wz*ry;
      currEstimate->vy += currEstimate->wz*rx - currEstimate->wx*rz;
      currEstimate->vz += currEstimate->wx*ry - currEstimate->wy*rx;
      }*/

  
   //if measurement is waiting to be added, update motion and add measurement
   if(outstandingMeas())
   {
      //interpolate to find navigation corresponding to measurement
      poseT measPose;
      if(interpolatePoses(*tNavFilter->lastNavPose, currEstimate, measPose, 
                          waitingMeas[numWaitingMeas-1].time))
      {  
         for(int i = 0; i < this->numWaitingMeas; i++)
         {
            interpolatePoses(*tNavFilter->lastNavPose, currEstimate, measPose, 
                             waitingMeas[i].time);

            //check that we are not interpolating over a large time difference
            if(measPose.time - tNavFilter->lastNavPose->time > MAX_INTERP_TIME 
               || currEstimate.time - measPose.time > MAX_INTERP_TIME)
            {
               this->lastMeasSuccess = false;
               output("TerrainNav::Measurement type %i not incorporated from time = "
                      "%.2f sec.; No relevant navigation data available\n", 
                      waitingMeas[i].dataType, measPose.time);
            }
            else
            {
               //perform motion update in navigation filter
               tNavFilter->motionUpdate(measPose);
               
               //update lastNavPose variable
               *tNavFilter->lastNavPose = measPose;
               
               //incoporate measurement 
               this->lastMeasSuccess = tNavFilter->measUpdate(waitingMeas[i]);
               
               if(this->lastMeasSuccess)
               {
                  output("TerrainNav::Measurement type %i successfully incorporated "
                         "from time = %.2f sec.\n", waitingMeas[i].dataType, measPose.time);    
                  lastMeasSuccessTime = measPose.time;
               }
            }
         }


         //set numWaitingMeas to zero because all measurements are included
         this->numWaitingMeas = 0;
      }
   }
   
   //perform motion update in navigation filter
   tNavFilter->motionUpdate(currEstimate);
   
   //update lastNavPose variable
   *tNavFilter->lastNavPose = currEstimate;

   return;
}

void TerrainNav::createFilter(const int filterType, const double *windowVar)
{
   //ensure that the filter object is empty before creating.
   if(tNavFilter != NULL)
      delete tNavFilter;
   tNavFilter = NULL;

   //create new filter based on given filter type
   switch(filterType)
   {
   case 1:
      tNavFilter = new TNavPointMassFilter(this->mapFile, this->vehicleSpecFile,
                                           this->saveDirectory, windowVar);
      break;
      
   case 2:
      tNavFilter = new TNavParticleFilter(this->mapFile, this->vehicleSpecFile,
                                          this->saveDirectory, windowVar);
      break;
      
   case 3:
      tNavFilter = new TNavExtendKalmanFilter(this->mapFile, 
                                              this->vehicleSpecFile,
                                              this->saveDirectory, windowVar);
      break;
      
   case 4:
      tNavFilter = new TNavSigmaPointFilter(this->mapFile, 
                                            this->vehicleSpecFile,
                                            this->saveDirectory, windowVar);
      break;
      
   default:
      tNavFilter = new TNavPointMassFilter(this->mapFile, this->vehicleSpecFile,
                                           this->saveDirectory, windowVar);
   }
   this->filterType = filterType;
   output("TerrainNav::TNavFilter initialized with type %i\n", filterType);
   
   if(USE_OCTREE){
   	setOctreeMap(this->octreeMap);
   }
}

void TerrainNav::initVariables()
{
   //initialize default variance for window size
   double windowVar[36] = {X_STDDEV_INIT*X_STDDEV_INIT, 0.0, Y_STDDEV_INIT*Y_STDDEV_INIT, 
                           0.0, 0.0, Z_STDDEV_INIT*Z_STDDEV_INIT, 
                           0.0, 0.0, 0.0, PHI_STDDEV_INIT*PHI_STDDEV_INIT, 
                           0.0, 0.0, 0.0, 0.0, THETA_STDDEV_INIT*THETA_STDDEV_INIT,
                           0.0, 0.0, 0.0, 0.0, 0.0, PSI_STDDEV_INIT*PSI_STDDEV_INIT, 
                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT, 
                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT};
   
	if(USE_OCTREE)
	{  // TODO:******* Make separate function *****
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);		

		ifstream inputFile (this->mapFile);
		string line;

		vector<float> xVec, yVec, zVec;
		float x, y, z;
		//float xmin, ymin, zmin;

		// Read input data
		if (inputFile.is_open()){
			while ( inputFile.good() ){

				getline (inputFile,line);
				stringstream sline;
				sline<<line;
				sline >> x  >> y >> z; //Input file needs to be NED

				xVec.push_back(x);
				yVec.push_back(y);
				zVec.push_back(z);
			}
		}

		int NumPoints = xVec.size(); 	// Number of points in map

		// Set cloud properties
		cloud->is_dense 	= false;  
		cloud->height 		= 1;
		cloud->width 		= NumPoints;
		cloud->points.resize (cloud->width * cloud->height);
		
		for (int ii=0; ii<NumPoints; ii++)
		{
			cloud->points[ii].x = xVec[ii];
			cloud->points[ii].y = yVec[ii];
			cloud->points[ii].z = zVec[ii];
		}
		
		this->octreeMap = (new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(OCTREE_RESOLUTION));
		this->octreeMap->setInputCloud(cloud);
		this->octreeMap->addPointsFromInputCloud();
		
	}

   //create filter object
   createFilter(this->filterType, windowVar);
   
   filterState = 1;
   
   int i;
   lastMeasSuccess = false;
   numWaitingMeas = 0;
   lastValidVel[0] = 0.0;
   lastValidVel[1] = 0.0;
   lastValidVel[2] = 0.0;
   lastVelBotLock = false;
   lastMeasValid = false;
   lastMeasSuccessTime = -1.0;
   lastInitAttemptTime = -1.0;
   lastBottomLockTime = -1.0;
   for(i = 0; i < 4; i++)
   {
      lastValidRange[i] = 0;
      lastValidRangeTime[i] = 0;
      noValidRange[i] = true;
   }
   numReinits = 0;
}

void TerrainNav::attemptInitFilter(poseT& initEstimate)
{  
   bool withinMap;
   int i;
   static double windowVarInc[36] = {0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0, 0.0, 0.0, 
                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                     0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
   
   if(!USE_OCTREE){
   withinMap = tNavFilter->withinValidMapRegion(initEstimate.x, initEstimate.y);
   }
   else{
   withinMap = true;  // TODO:*********** Need to actually check within map *****************
   }
   checkVelocityValidity(initEstimate); 
   
   //initialize the filter if not already initialized AND if within valid 
   //region of the map AND vehicle has bottom lock AND vehicle has good 
   //measurements AND vehicle is not on the surface
   if(withinMap && initEstimate.bottomLock && lastMeasValid && 
      initEstimate.dvlValid && !initEstimate.gpsValid && initEstimate.z > 1)
   {
      //Incorporate increased search window to account for large initialization
      //waiting times
      for(i=0;i<36;i++)
         windowVarInc[i] *= windowVarInc[i];
      tNavFilter->increaseInitSearchWin(windowVarInc);
      output("TerrainNav::attemptInitFilter is increasing Init Search Window by %f m\n",windowVarInc[0]);
      for(i=0;i<36;i++)
         windowVarInc[i] = 0.0;
      //lastInitAttemptTime = -1;

      //Initialize vehicle motion
      initMotion(initEstimate);
   }
   else
   {
      //increase search region for filter initialization
      if(lastInitAttemptTime > 0)
      {
         double dt = initEstimate.time - lastInitAttemptTime;
         tNavFilter->totalAttemptTime += dt;
         double dx = double (INCREASE_WINDOW)*(0.01*1.5*dt);
         windowVarInc[0] += dx;
         windowVarInc[2] += dx;
      }
      lastInitAttemptTime = initEstimate.time;

      if(!withinMap)
      {
         output("TerrainNav::Filter not initialized - vehicle is currently "
                "within a non-valid region of the reference map\n");
         return;
      }      
      if(initEstimate.gpsValid || initEstimate.z <= 1)
      {
         output("TerrainNav::Filter not initialized - vehicle is currently "
                "on the surface\n");   
         return;
      }
      if(!lastMeasValid)
      {
         output("TerrainNav::Filter not initialized - vehicle currently "
                "does not have good range measurements\n");             
         return;
      }
      output("TerrainNav::Filter not initialized - vehicle currently "
             "does not have bottom lock or good velocity data\n");      
   }
}

void TerrainNav::initMotion(poseT& initEstimate)
{
   tNavFilter->lastNavPose = new poseT;

   //initialize velocity information
   lastValidVel[0] = initEstimate.vx;
   lastValidVel[1] = initEstimate.vy;
   lastValidVel[2] = initEstimate.vz;
   lastVelBotLock = initEstimate.bottomLock;
   tNavFilter->timeLastDvlValid = initEstimate.time;
   
   //set lastNavEstimate to initEstimate
   *tNavFilter->lastNavPose = initEstimate;

   //add on prior knowledge of navigation offset, if available
   initEstimate += estNavOffset;
   initEstimate.time = tNavFilter->lastNavPose->time;
   initEstimate.dvlValid = tNavFilter->lastNavPose->dvlValid;
   initEstimate.gpsValid = tNavFilter->lastNavPose->gpsValid;
   initEstimate.bottomLock = tNavFilter->lastNavPose->bottomLock;

   //initialize filter with initial pose estimate
   tNavFilter->initFilter(initEstimate);

   output("TerrainNav:: vehicle motion has been initialized\n");
     
   return;
}



bool TerrainNav::interpolatePoses(const poseT& pose1, const poseT& pose2, 
                                  poseT& newPose, const double newTime)
{
   double deltaT, newDeltaT;
   
   deltaT = pose2.time - pose1.time;
   newPose.time = newTime;
   newDeltaT = newTime - pose1.time;
   
   if(newTime > pose2.time || newTime < pose1.time)
      return false;
      
   newPose.x = pose1.x + (pose2.x - pose1.x)*newDeltaT/deltaT;
   newPose.y = pose1.y + (pose2.y - pose1.y)*newDeltaT/deltaT;
   newPose.z = pose1.z + (pose2.z - pose1.z)*newDeltaT/deltaT;
   newPose.phi = pose1.phi + (pose2.phi - pose1.phi)*newDeltaT/deltaT;
   newPose.theta = pose1.theta+(pose2.theta - pose1.theta)*newDeltaT/deltaT;
   newPose.psi = pose1.psi + (pose2.psi - pose1.psi)*newDeltaT/deltaT;
   newPose.vx = pose1.vx + (pose2.vx - pose1.vx)*newDeltaT/deltaT;
   newPose.vy = pose1.vy + (pose2.vy - pose1.vy)*newDeltaT/deltaT;
   newPose.vz = pose1.vz + (pose2.vz - pose1.vz)*newDeltaT/deltaT;
   newPose.wx = pose1.wx + (pose2.wx - pose1.wx)*newDeltaT/deltaT;
   newPose.wy = pose1.wy + (pose2.wy - pose1.wy)*newDeltaT/deltaT;
   newPose.wz = pose1.wz + (pose2.wz - pose1.wz)*newDeltaT/deltaT;
   newPose.dvlValid = (pose1.dvlValid && pose2.dvlValid);
   newPose.gpsValid = (pose1.gpsValid && pose2.gpsValid);
   newPose.bottomLock = (pose1.bottomLock && pose2.bottomLock);

   return true;
}

void TerrainNav::computeMeasVariance(measT& currMeas)
{
   int sensorIdx = 0;
   double perError;
   double rangeSq;
   int i;

   //find index of current measurement sensor.  If none match,
   //return.
   if(!tNavFilter->findMeasSensorIndex(currMeas.dataType, sensorIdx))
      return;
   perError = tNavFilter->vehicle->sensors[sensorIdx].percentRangeError;

   //if covariance vector not already intialized, intialize it
   if(currMeas.covariance == NULL)
      currMeas.covariance = new double[currMeas.numMeas];

   //compute variance based on sensor's percent range error
   if(currMeas.dataType == 2) // mb measurement
   {
      for(i = 0; i < currMeas.numMeas; i++)
      {
         rangeSq = pow(currMeas.crossTrack[i],2)+pow(currMeas.alongTrack[i],2)
            +pow(currMeas.altitudes[i],2);
         currMeas.covariance[i] = rangeSq*pow(perError/100.0,2);
      }
   }
   else //dvl or altimeter measurement
   {
      for(i = 0; i < currMeas.numMeas; i++)
         currMeas.covariance[i] = pow(currMeas.ranges[i]*perError/100.0,2);
   }

   return;
}

void TerrainNav::checkVelocityValidity(poseT& currPose)
{
   //check for out of range velocity data - if above max or equal to zero,
   // set dvlValid flag to false
   if((fabs(currPose.vx) > MAX_VEL) || (fabs(currPose.vx) <= 1e-4) ||
      (fabs(currPose.vy) > MAX_VEL) || (fabs(currPose.vz) > MAX_VEL))
      currPose.dvlValid = false; 
   
   //check if predicted ground-based acceleration is too large
   //If this is the first velocity measurement, this check won't be performed
   //as lastVelBotLock = false initially;
   if(currPose.bottomLock && lastVelBotLock && currPose.z > 5)
   {
      if((fabs(currPose.ax) > MAX_ACCEL) || (fabs(currPose.ay) > MAX_ACCEL) ||
         (fabs(currPose.az) > MAX_ACCEL))
      {
         currPose.dvlValid = false;
         //update acceleration based on this new information
         currPose.ax = 0;
         currPose.ay = 0;
         currPose.az = 0;
      }
   }
}


void TerrainNav::checkRangeValidity(measT& currMeas)
{
   int i,j;
   int numEqual = 0;
   double alpha, dr, dt;

   //this range check is only valid for DVL measurements
   if(currMeas.dataType != 1)
      return;
   
   for(i = 0; i < currMeas.numMeas; i++)
   {
      numEqual = 0;
      alpha = currMeas.ranges[i];
      //check if more than two beams are equal
      if(i < 2)
      {
         for(j = i+1; j < currMeas.numMeas; j++)
         {
            if(fabs(alpha-currMeas.ranges[j]) < 0.1)
               numEqual++;
         }
         if(numEqual >= 2)
         {
            //if more than two beams are equal, throw out all beams
            for(j = 0; j < currMeas.numMeas; j++)
               currMeas.measStatus[j] = false;
            return;            
         }
      }

      //check validity of each beam based on NaN or range value
      if(isnan(currMeas.ranges[i]) |  (currMeas.ranges[i] >= MAX_RANGE)
         | (currMeas.ranges[i] <= MIN_RANGE))
         currMeas.measStatus[i] = false;

      //check dr/dt for each beam
      if(currMeas.measStatus[i])
      {
         if(noValidRange[i])
         {
            noValidRange[i] = false;
            lastValidRange[i] = currMeas.ranges[i];
            lastValidRangeTime[i] = currMeas.time;
         }
         else
         {
            dr = currMeas.ranges[i]-lastValidRange[i];
            dt = currMeas.time - lastValidRangeTime[i];
            if((dt > 0) && (fabs(dr/dt) > MAX_DRDT))                  
               currMeas.measStatus[i] = false;
            else
            {
               lastValidRange[i] = currMeas.ranges[i];
               lastValidRangeTime[i] = currMeas.time;
            }
         }
      }
   } 
}

void TerrainNav::reinitFilter(int newState, bool lowInfoTransition)
{
   int interpMapMethod = 1;
   bool interpMeasAttitude = true;
   double driftRate = 1;
   double windowVar[36] = {X_STDDEV_INIT*X_STDDEV_INIT, 0.0, Y_STDDEV_INIT*Y_STDDEV_INIT, 
                           0.0, 0.0, Z_STDDEV_INIT*Z_STDDEV_INIT, 
                           0.0, 0.0, 0.0, PHI_STDDEV_INIT*PHI_STDDEV_INIT, 
                           0.0, 0.0, 0.0, 0.0, THETA_STDDEV_INIT*THETA_STDDEV_INIT,
                           0.0, 0.0, 0.0, 0.0, 0.0, PSI_STDDEV_INIT*PSI_STDDEV_INIT, 
                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT, 
                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, GYRO_BIAS_STDDEV_INIT*GYRO_BIAS_STDDEV_INIT};
   int i;
   poseT* temp = new poseT;
   
   //ensure that tNavFilter is non-empty before accessing and deleting
   if(tNavFilter != NULL)
   {
      //copy relevant data from current filter
      interpMapMethod = tNavFilter->interpMapMethod;
      interpMeasAttitude = tNavFilter->interpMeasAttitude;
      driftRate = tNavFilter->vehicle->driftRate;
      if(tNavFilter->lastNavPose != NULL && !lowInfoTransition) //trying to 
      {
      	tNavFilter->computeMMSE(temp);
      	for (i=0; i<36; i++)
      	{
           windowVar[i] = 1.0*temp->covariance[i];
      	}
      }
      
      //delete filter object
      delete tNavFilter;
      tNavFilter = NULL;
   }
	
   //create new filter
   switch(newState)
   {
   case 0:
      createFilter(2,windowVar);
      break;
      
   case 1:
      createFilter(2,windowVar);
      break;
      
   case 2:
      //If low information transition, reinitialize with uniform distribution
      //and initial search window (larger search window because using PMF)
      if(lowInfoTransition)
      {
         for(i=0;i<36;i++)
         {
            windowVar[i] *= 1.0;  // want to use smaller value if starting w/ init window, but bigger if taking current var
         }
      }
      createFilter(1,windowVar);
      tNavFilter->setInitDistribType(0); //0 is uniform, 1 is gaussian
      output("TerrainNav::reinitializing filter from lowInfoTransition with uniform distribution \n");
      break;
      
   default:
      createFilter(2,windowVar);
   }
   filterState = newState;

   //if not transitioning due to low information, initialize the filter
   //with a Gaussian distribution
   if(!lowInfoTransition)
      tNavFilter->setInitDistribType(1);

      
   //reset filter and terrainNav parameters
   setMapInterpMethod(interpMapMethod);
   setVehicleDriftRate(driftRate);
   setInterpMeasAttitude(interpMeasAttitude);
	
   numWaitingMeas = 0;
   lastMeasSuccessTime = -1.0;
   lastInitAttemptTime = -1.0;
   lastBottomLockTime = -1.0;
   numReinits++;
   delete temp;
}

bool TerrainNav::checkFilterHealth()
{
   bool healthy = true; //1 is healthy, 0 is not healthy and 
                        //needs to be reinitialized
   bool lowInfoTransition = false;
   
   double currVarArea;
   currVarArea = (tNavFilter->currVar[0]+tNavFilter->currVar[1]);
   
   //double larger;
   //larger = max(tNavFilter->currVar[0], tNavFilter->currVar[1]);
   
   //initialize the state to change to as the current state
   int newState = filterState;
		
   //check if the length of time since last successful measurement exceeds 
   //set maximum. Ensure that the filter has been initialized and that there
   //has been at least one successful measurement incorporated
   if (this->lastMeasSuccessTime > 0 && tNavFilter->lastNavPose != NULL 
       && tNavFilter->lastNavPose->time - this->lastMeasSuccessTime > MAX_MEAS_OUTAGE)
   {
      newState = transitionMatrix[3][filterState];
     	healthy = false;
     	output("TerrainNav::No valid range measurements for the past %.1f "
            "seconds. Re-initializing the filter.\n",  MAX_MEAS_OUTAGE);
      lowInfoTransition = true;
      
   }
   
   
   //check if the length of time since last successful bottom velocity meas 
   //exceeds set maximum. Ensure that the filter has been initialized and that 
   //there has been at least one successful measurement incorporated
   if (this->lastBottomLockTime > 0 && tNavFilter->lastNavPose != NULL 
       && tNavFilter->lastNavPose->time - this->lastBottomLockTime > MAX_VEL_OUTAGE)
   {
      newState = transitionMatrix[3][filterState];
      healthy = false;
      lowInfoTransition = true;
      output("TerrainNav::No valid bottom velocity measurements for the past %.1f "
             "seconds.  Re-initializing the filter.\n",
             MAX_VEL_OUTAGE);
   }

   //check if x/y uncertainty of the filter is below a set minimum.  
   //potentially for switching filters in the future.
   // force reinit is for testing the fitler reinitialization only
   if(healthy && currVarArea < MIN_FILTER_VAR)
   //if(healthy && larger < MIN_FILTER_VAR)
   {
      newState = transitionMatrix[0][filterState];
      output("CHECK TerrainNav:: TransitionMatrix is [%i %i %i]",transitionMatrix[0][0],transitionMatrix[0][1],transitionMatrix[0][2]);
      if(newState != filterState) 
      {
      	healthy = false;
      	output("TerrainNav::North/East uncertainty has fallen below the "
             "minimum of %.1f m^2.  Re-initializing the filter.\n",
             MIN_FILTER_VAR);
        lowInfoTransition = true; //only use for force reinit
      }
   }
   
   //check if measurement variance is below a set minimum 
   /*if(tNavFilter->measVariance > 0 && tNavFilter->measVariance < MIN_MEAS_VAR)
   {
   		healthy = false;
   		output("TerrainNav::Measurement variance has fallen below the minimum "
                "of %f .  Re-initializing the filter.\n", MIN_MEAS_VAR);
   }
   */
  
   //check if the x/y uncertainty of the filter exceeds a set maximum
   if(healthy && currVarArea > MAX_FILTER_VAR)
   //if(healthy && larger > MAX_FILTER_VAR)
   {
      newState = transitionMatrix[2][filterState];
      if(newState != filterState) 
      {
      	healthy = false;
      	output("TerrainNav::North/East uncertainty has exceeded the "
             "maximum of %.1f m^2.  Re-initializing the filter.\n",
             MAX_FILTER_VAR);
        output("North: %f East: %f Comb: %f. \n", tNavFilter->currVar[0],
        				 tNavFilter->currVar[1],	currVarArea);
      }
   }
   

   //check if the uncertainty of the filter has changed enough to warrant 
   //changing states
   if(healthy && currVarArea < MAX_FILTER_VAR-1*VAR_MARGIN &&
   		 currVarArea > MIN_FILTER_VAR+0.1*VAR_MARGIN)
   /*if(healthy && larger < MAX_FILTER_VAR-1*VAR_MARGIN && 
   		larger > MIN_FILTER_VAR+0.1*VAR_MARGIN)*/
   {
      newState = transitionMatrix[1][filterState];
      if(newState != filterState) 
      {
      	healthy = false;
      	output("TerrainNav::North/East uncertainty is within the margins "
             "of %.1f m^2.  Re-initializing the filter.\n",
             VAR_MARGIN);
        output("North: %f East: %f Comb: %f. \n", tNavFilter->currVar[0],
        			 tNavFilter->currVar[1],currVarArea);
      }
   }
   
   
   //if filter is not healthy reinitialize
   if(!healthy)
   {
      reinitFilter(newState, lowInfoTransition);
   }

 	return healthy;

}
