#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include <iomanip>
#include <time.h>
#include <semaphore.h>

#ifdef use_namespace
using namespace std;
#endif

#include "TerrainNav.h"
#include "matrixArrayCalcs.h"
#include "structDefs.h"
#include "TNavParticleFilter.h"
#include "genFilterDefs.h"
#include "ParticlePlot.h"

#define NANOSEC_PER_SEC (1000000000L)

void runTerrainNav(const Matrix &dataKft, const Matrix &dataMeas,
                   const Matrix &watVel, char* mapPath, 
                   int interpMethod, int kSubSample, 
                   int mSubSample, poseT *tercomEst, poseT *mmseEst, 
                   bool realTime, int filterType, ofstream &distribFile, 
                   char* savePath, int dataK_init, ParticlePlot &plotter);
Matrix readDataFromFile(const char *fileName, const int &numRows, 
			const int &numCols);
bool assignKearfottEstimate(poseT* currEstimate, Matrix pose);
bool assignWatVelocity(poseT* currEstimate, Matrix watVel);
void assignDVLMeasurement(measT* currMeas, Matrix meas);
void assignMBMeasurement(measT* currMeas, Matrix meas);
void assignAltMeasurement(measT* currMeas, Matrix meas);
void updatePlotterData(ParticlePlot &plotter, poseT *currEstimate, 
                       poseT *mmseEst, TNavFilter* tNavFilter, mapT &currMap, 
                       const double filterType, const double startTime);
void saveRunParameters(const char* savePath);

int main( int argc, char **argv )
{
	//octreeTest();
   ParticlePlot plotter("TRN Viewer");
   plotter.spawnAsThread();

   bool realTime = false;
   int i,j;
   int numRepeat = 1;

   ofstream trnfile, mmsefile, distribfile, pmfdistribfile;
  
   for (i = 1; i < argc; i++)
   {
      //If the user passes an argument starting with -r, run the computation
      //loop below in real time.
      if( !strncmp(argv[i], "-r", 2) ) realTime = true;
      
      //If the user passes an argument starting with -N, set the number of
      //runs to the next input argument
      if( !strncmp(argv[i], "-N", 2) ) 
         numRepeat = atoi(argv[i+1]);
   } 

  //Define paths where the data is stored
  char fileName[512];
  char dataPath[256] = 
     //"/home/shouts/Dropbox/ARL/ForJenny/";
     //"/home/shouts/Documents/dataFiles/";
     "/mnt/public/Projects/mbari/diveData/";
     //"/home/shouts/Documents/ARL_SVN/ArlSoftware/terrainNavigation/simAUVrun/outputFiles/";
  char mapPath[256] = 
     //"/home/shouts/Dropbox/ARL/ForJenny/";
     //"/home/shouts/Documents/dataFiles/Maps/";
		 "/mnt/public/Projects/mbari/mbariMaps/";
  char map_file[256] = //"SoquelCanyon/SoquelCanyonMAUVUTMTopo.grd";
  	 //"GOCMappingAUVTopo_C1_1m_UTM_new.grd";
  	 //"GOC_UTM.grd";
  	 //"SoquelCanyon/SoquelCanyonMAUVUTMTopo_NoNan.grd";
     //"SoquelCanyon/soquelCanyonGW_101309_1mRes.grd";
     //"SoquelCanyon/SoquelCanyon1998em300_20m.grd";
     //"SoquelCanyon/SoquelCanyon1998em300_10m.grd";
     "SoquelCanyon/SoquelCanyonMAUVUTMTopo_061709cut.grd";
     //"SoquelCanyon/SoquelCanyonTopoUTM.grd";
     //"PortugueseLedge/PortugueseLedge20080424TopoUTM_NoNan.grd";
     //"PortugueseLedge/PortLedge1998em300_20m.grd";
     //"em300/datalist-1.grd";
  

 //initialize memory for different tests
  int numTests = 1;
  int num = 0;
  int* dataK_numRows = new int[numTests];
  int* dataMeas_numRows = new int[numTests];
  int* init_dataK = new int[numTests];
  char** dataK_files = new char*[numTests];
  char** watVel_files = new char*[numTests];
  char** dataMeas_files = new char*[numTests];
  char** savePath = new char*[numTests];
 
  for(i = 0; i < numTests; i++)
  {
     dataK_files[i] = new char[256];
     watVel_files[i] = new char[256];
     dataMeas_files[i] = new char[256];
     savePath[i] = new char[256];
  }

  //Define specific file information
  
  //TEST Baja: Dvl data from ROV dive in Gulf of Cortez
  /*strcpy(dataK_files[num],"dataKft_bajatest.txt");
  strcpy(dataMeas_files[num],"measData_bajatest.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 10;
  dataK_numRows[num] = 16848;
  dataMeas_numRows[num] = 16848;*/
  
  //TEST modTRN: Dvl data from 12/05/12 dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2012_1205auv/dataFromDive/dataKft_test09all_120512dive.txt");
  strcpy(dataMeas_files[num],"Dive_2012_1205auv/dataFromDive/measData_test09all_rangeCorrected_120512dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump");
  init_dataK[num] = 1000;
  dataK_numRows[num] = 9880;
  dataMeas_numRows[num] = 9190;*/
  
  //TEST OA: Dvl data from OA BIAUV/MAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"2010.334.18/dataKft_OAtest.txt");
  strcpy(dataMeas_files[num],"2010.334.18/measData_all_OAtest.txt");
  strcpy(savePath[num],"/home/shouts/Documents/dataFiles/results/auv2012/OAtrn/");
  init_dataK[num] = 10;
  dataK_numRows[num] = 9702;
  dataMeas_numRows[num] = 8825;*/
  
  //TEST 0: Dvl data from 7/22/08 dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2008_0722auv/dataFromDive/dataKft_test04all_080722dive.txt");
  strcpy(dataMeas_files[num],"Dive_2008_0722auv/dataFromDive/measData_test04all_080722dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 15;
  dataK_numRows[num] = 29129;
  dataMeas_numRows[num] = 11302;*/
  /*strcpy(dataK_files[num],"2008_07_22diveFiles/dataKft_test04all.txt");
  strcpy(dataMeas_files[num],"2008_07_22diveFiles/measData_test04all.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1500;
  dataK_numRows[num] = 29129;
  dataMeas_numRows[num] = 11302;*/
  
  //TEST 0: Dvl data from 5/17/11 BIAUV/MAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2011_0411auv/dataFromDive/dataKft_test02all_051711dive.txt");
  strcpy(dataMeas_files[num],"Dive_2011_0411auv/dataFromDive/measData_test02all_051711dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 10;
  dataK_numRows[num] = 10464;
  dataMeas_numRows[num] = 9015;*/
  
  //TEST -1: Dvl data from 6/17/09 BIAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2009_0617auv/dataFromDive/dataKft_test06all_090617dive.txt");
  strcpy(dataMeas_files[num],"Dive_2009_0617auv/dataFromDive/measData_test06all_rangeCorrected_090617dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 8370;
  dataMeas_numRows[num] = 2397;*/
  
  //TEST -2: Dvl data from 6/29/09 BIAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2009_0625auv/dataFromDive/dataKft_test02all_090629dive.txt");
  strcpy(dataMeas_files[num],"Dive_2009_0625auv/dataFromDive/measData_test02all_rangeCorrected_090629dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 9470;
  dataMeas_numRows[num] = 2792;*/
  
  //TEST -3: Dvl data from 10/20/09 BIAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2009_1020auv/dataFromDive/dataKft_test07all_091020dive.txt");
  strcpy(dataMeas_files[num],"Dive_2009_1020auv/dataFromDive/measData_test07all_rangeCorrected_091020dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 11628;
  dataMeas_numRows[num] = 3100;*/
  
  //TEST -3: Dvl data from 12/15/09 BIAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2009_1215auv/dataFromDive/dataKft_test04all_091215dive.txt");
  strcpy(dataMeas_files[num],"Dive_2009_1215auv/dataFromDive/measData_test04all_rangeCorrected_091215dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 10196;
  dataMeas_numRows[num] = 2786;*/
  
  //TEST -4: Dvl data from 02/10/10 BIAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2010_0210auv/dataFromDive/dataKft_test08all_100210dive.txt");
  strcpy(dataMeas_files[num],"Dive_2010_0210auv/dataFromDive/measData_test08all_rangeCorrected_100210dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 11685;
  dataMeas_numRows[num] = 3278;*/
  
  //TEST -5: Dvl data from 03/30/10 BIAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2010_0330auv/dataFromDive/dataKft_test07all_033010dive.txt");
  strcpy(dataMeas_files[num],"Dive_2010_0330auv/dataFromDive/measData_test07all_rangeCorrected_033010dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 12073;
  dataMeas_numRows[num] = 2598;*/
  
  //TEST -6: Dvl data from 07/19/10 BIAUV dive at Soquel Canyon
  /*strcpy(dataK_files[num],"Dive_2010_0719auv/dataFromDive/dataKft_test11all_071910dive.txt");
  strcpy(dataMeas_files[num],"Dive_2010_0719auv/dataFromDive/measData_test11all_rangeCorrected_071910dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 13493;
  dataMeas_numRows[num] = 3226;*/
  
  //TEST 1: Dvl data from 8/04/08 MAUV dive at Portuguese Ledge
  //strcpy(dataK_files[num],"2008_08_04diveFiles/dataKft_test04all.txt");
  //strcpy(dataMeas_files[num],"2008_08_04diveFiles/measData_test04all.txt");
  /*strcpy(dataK_files[num],"Dive_2008_0804auv/dataFromDive/dataKft_test04all_080804dive.txt");
  strcpy(dataMeas_files[num],"Dive_2008_0804auv/dataFromDive/measData_test04all_rangeCorrected_080804dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 15;
  dataK_numRows[num] = 9285;
  dataMeas_numRows[num] = 3721;*/

  //TEST 2: Dvl data from 11/18/10 BIAUV/MAUV dive at Soquel Canyon
  strcpy(dataK_files[num],"Dive_2010_1118auv/dataFromDive/dataKft_test03all_111810dive.txt");
  strcpy(dataMeas_files[num],"Dive_2010_1118auv/dataFromDive/measData_test03all_rangeCorrected_111810dive.txt");
  strcpy(savePath[num],"/home/shouts/Documents/ResultsDump/");
  init_dataK[num] = 1;
  dataK_numRows[num] = 9776;
  dataMeas_numRows[num] = 8697;
    
  //initialize data structures
  poseT *tercomEst = new poseT;
  poseT *mmseEst = new poseT;
  Matrix dataKft, dataMeas, watVel;

  for(i = 0; i < numTests; i++)
  {
     trnfile.open(charCat(fileName,savePath[i],"allMle.txt"));
     mmsefile.open(charCat(fileName,savePath[i],"allMmse.txt"));
     distribfile.open(charCat(fileName,savePath[i],"allFinalHist.txt"));
     pmfdistribfile.open(charCat(fileName,savePath[i],"allFinalPDF.txt"));
 
     dataMeas = readDataFromFile(charCat(fileName,dataPath,dataMeas_files[i]),
                                 dataMeas_numRows[i], 62);
     dataKft = readDataFromFile(charCat(fileName,dataPath,dataK_files[i]),
                                dataK_numRows[i], 22);
     //watVel = readDataFromFile(charCat(fileName,dataPath,watVel_files[i]),
     //                          dataK_numRows[i], 3);
     sprintf(fileName,"%s%s",mapPath,map_file);
     
     for(j = 0; j < numRepeat; j++)
     {  
        plotter.reinitPlotter();
        runTerrainNav(dataKft, dataMeas, watVel, fileName, 0, 15, 15, 
                      tercomEst, mmseEst, realTime, 2, distribfile, 
                      savePath[i], init_dataK[i], plotter);  //filter type 2 is PF, 1 is PMF
        trnfile << setprecision(15) << tercomEst->x << "\t"<<tercomEst->y
                << "\t" << tercomEst->z <<"\t" << tercomEst->phi 
                << "\t" << tercomEst->theta <<"\t" << tercomEst->psi
                << "\t" << tercomEst->time << endl;
        mmsefile << setprecision(15) << mmseEst->x << " " << mmseEst->y
                 << " " << mmseEst->z << "\t" << mmseEst->phi << "\t" 
                 << mmseEst->theta << "\t" << mmseEst->psi << "\t" 
                 << mmseEst->covariance[0] << "\t" << mmseEst->covariance[2] 
                 << "\t"<< mmseEst->covariance[1] << "\t" 
                 << mmseEst->covariance[5] << "\t" << mmseEst->covariance[9]
                 << "\t" << mmseEst->covariance[14] << "\t" 
                 << mmseEst->covariance[20] << endl;
     }
     
     saveRunParameters(savePath[i]);

     trnfile.close();
     mmsefile.close();
     distribfile.close();
     pmfdistribfile.close();
  }

  //delete allocated memory on the heap
  delete tercomEst;
  delete mmseEst;
  for(i = 0; i < numTests; i++)
  {
     delete dataK_files[i];
     delete watVel_files[i];
     delete dataMeas_files[i];
     delete savePath[i];
  }
  delete [] dataMeas_files;
  delete [] dataK_files;
  delete [] watVel_files;
  delete [] savePath;
  delete [] dataK_numRows;
  delete [] dataMeas_numRows;
  delete [] init_dataK;

  cout << "enter a key: " << endl;
  char c;
  cin >> c;
  return 0;
}


// run terrainNav algorithm for a particular set of data
void runTerrainNav(const Matrix &dataKft, const Matrix &dataMeas, 
                   const Matrix &watVel, char* mapPath, 
                   int interpMethod, int kSubSample, 
		   int mSubSample, poseT *tercomEst, poseT *mmseEst, 
                   bool realTime, int filterType, ofstream &distribFile, 
                   char* savePath, int dataK_init, ParticlePlot &plotter)
{
  //initialize measurment and pose structures
  poseT *currEstimate = new poseT;
  measT *currMeas = new measT;
  int dataType;
  currMeas->ranges = new double[4];
  currMeas->alongTrack = new double[20];
  currMeas->crossTrack = new double[20];
  currMeas->altitudes = new double[20];
  currMeas->measStatus = new bool[20]; 
  int N = dataKft.Nrows();
  int M = dataMeas.Nrows(); 
  int i_init = dataK_init;
  int j_init = 1;
  int i,j;
  double start_t = dataKft(1,1);
  mapT currMap;
  double minNorth, maxNorth, minEast, maxEast;
  sem_t semaphore;
  struct timespec semTime;
  bool successMap = true;
  bool useConstMap = false;
  int mapType = 1;
  
  sem_init( &semaphore, 0, 0 );

  //set up files for saving data 
  bool saveResults = true;
  
  char fileName[200];
  ofstream tfile, mfile, pfile, ffile;
  if(saveResults)
    {
       if(filterType == 2)
      {
         tfile.open(charCat(fileName,savePath,"tercomEst_pf.txt"));
         mfile.open(charCat(fileName,savePath,"mmseEst_pf.txt"));
         pfile.open(charCat(fileName,savePath,"propPoses_pf.txt"));
         ffile.open(charCat(fileName,savePath,"allParticles.txt"));
      }
      else
      {
         tfile.open(charCat(fileName,savePath,"tercomEst.txt"));
         mfile.open(charCat(fileName,savePath,"mmseEst.txt"));
         pfile.open(charCat(fileName,savePath,"propPoses.txt"));
         ffile.open(charCat(fileName,savePath,"likeSurfs.txt"));
      }
    }
  
  
  //initialize terrainNav object and load map
//  TerrainNav* tercom = new TerrainNav(mapPath, (char*)"imagingAUV_specs.cfg",
//                                      filterType, savePath);
  TerrainNav* tercom = new TerrainNav(mapPath, (char*)"mappingAUV_specs.cfg",
                                      filterType, mapType, savePath);
  //set random number generator for test cases
  srand(2000);
  tercom->tNavFilter->setMapInterpMethod(interpMethod);
  tercom->tNavFilter->setInterpMeasAttitude(true);
  tercom->setModifiedWeighting(USE_MODIFIED_WEIGHTING);
  tercom->setFilterReinit(ALLOW_FILTER_REINIT);
  printf("Terrain navigation object initialized.\n");

  //extract a map segment to cover the full data set:
  TerrainMap relevantMap(const_cast<char*>(mapPath));
  double mapSize[2];
  mapSize[0] = 300;
  mapSize[1] = 300;
  if(useConstMap)
  {
     Matrix northVals = dataKft.SubMatrix(i_init,N,7,7);
     Matrix eastVals = dataKft.SubMatrix(i_init,N,8,8);
     minNorth = northVals.Minimum();
     maxNorth = northVals.Maximum();
     minEast = eastVals.Minimum();
     maxEast = eastVals.Maximum();
     relevantMap.extractSubMap((maxNorth+minNorth)/2.0, (minEast+maxEast)/2.0, mapSize);
     currMap = relevantMap.map;
  }

  mapT currPDF;
    
  //Run terrainNav over all measurements and odometry
  printf("Initial Conditions: North: %.2f, East %.2f\n", dataKft(2,7),
  	 dataKft(2,8));
  printf("data loaded...\n");

  //Get the current time:
  clock_gettime(CLOCK_REALTIME, &semTime );
  struct timespec startTime, lastTime, now;
  double localTime, elapsed;
  startTime.tv_sec  = semTime.tv_sec;
  startTime.tv_nsec = semTime.tv_nsec;

  // Sampling interval:
  short TsMsec = 500;
  i = i_init;
  j = j_init;
  while(i <= N)
  {	
     // Compute the next wake-up time:
     clock_gettime(CLOCK_REALTIME, &semTime);
     
     lastTime.tv_sec  = semTime.tv_sec;
     lastTime.tv_nsec = semTime.tv_nsec;
     
     localTime = semTime.tv_sec  - startTime.tv_sec +
	(semTime.tv_nsec - startTime.tv_nsec)/1.e9;
     if(realTime) printf("Time since start = %.2f sec\n", localTime);
     
     // NOTE: Need to subtract a millisecond so that sem_timedwait() times out
     // at the expected interval.
     long deltaMsec = TsMsec - 1;
     
     semTime.tv_sec += (deltaMsec / 1000);
     semTime.tv_nsec += ((deltaMsec % 1000)*1000000);
     
     // Check for nanosecond rollover
     if (semTime.tv_nsec > NANOSEC_PER_SEC) 
     {
        semTime.tv_sec += (int )(semTime.tv_nsec / NANOSEC_PER_SEC);
        semTime.tv_nsec = semTime.tv_nsec % NANOSEC_PER_SEC;
     }  

     // Perform motion/measurement updates in time order
     if(j > M || (i <=N && dataKft(i,1) <= dataMeas(j,2)))
     { 
        printf("Motion Update.. (t = %.2f)\n", dataKft(i,1));
        
        assignKearfottEstimate(currEstimate, dataKft.Row(i));
        //assignWatVelocity(currEstimate, watVel.Row(i));

        tercom->motionUpdate(currEstimate);
        i = i+kSubSample;
        
        //ensure that we always perform the final motion update
        if(i > N && (i-kSubSample) < N)
           i = N;
        
        //compute tercom MLE pose estimate
        tercom->estimatePose(tercomEst, 1);
        
        //compute tercom MMSE pose estimate 
        tercom->estimatePose(mmseEst, 2);
        
        //plot particle distribution over map
        if(!useConstMap)
           successMap = tercom->tNavFilter->getTerrainMap(currMap);
        if(successMap)
        {
           relevantMap.extractSubMap(mmseEst->x, mmseEst->y, mapSize);
           currMap = relevantMap.map;
           updatePlotterData(plotter, currEstimate, mmseEst, tercom->tNavFilter, currMap, tercom->getFilterType(), start_t);
        }

     }
     else
     {
        if( j <= M)
        {
           dataType = int(dataMeas(j,1));   
           
           //read in current measurement
           switch(dataType)
           {
           case 1:
              assignDVLMeasurement(currMeas, 
                                   dataMeas.SubMatrix(j,j,2,dataMeas.Ncols()));
              break;
              
           case 2:
              assignMBMeasurement(currMeas,  
                                  dataMeas.SubMatrix(j,j,2,dataMeas.Ncols()));
              currMeas->psi = currEstimate->psi;
              currMeas->x = currEstimate->x;
              currMeas->y = currEstimate->y;
              currMeas->z = currEstimate->z;
              
              break;
              
           case 3:
              assignAltMeasurement(currMeas,
                                   dataMeas.SubMatrix(j,j,2,dataMeas.Ncols()));
              break;
              
           default:
              printf("No valid datatype specified.  Exiting...\n");
              return;
           }
           
           printf("Measurement Update...\n");
           tercom->measUpdate(currMeas, dataType);
           
           j=j+mSubSample;
           
           //If measurement update happens first or measurement update 
           //unsucessful, skip pose estimation and file saving
           if(i > i_init && tercom->lastMeasSuccessful())
           {	  
              //compute tercom MLE pose estimate
              tercom->estimatePose(tercomEst, 1);
              
              //compute tercom MMSE pose estimate 
              tercom->estimatePose(mmseEst, 2);
              
              //plot particle distribution over map
              if(!useConstMap)
                 successMap = tercom->tNavFilter->getTerrainMap(currMap);
              if(successMap)
              {
                 relevantMap.extractSubMap(mmseEst->x, mmseEst->y, mapSize);
                 currMap = relevantMap.map;
                 updatePlotterData(plotter, currEstimate, mmseEst, tercom->tNavFilter, currMap, tercom->getFilterType(), start_t);                 
              }

              
              //display tercom estimate biases
              printf("Estimation Bias (Max. Likelihood): (t = %.2f)\n", 
                     tercomEst->time);
              printf("North: %.4f, East: %.4f, Depth: %.4f\n", 
                     tercomEst->x - currEstimate->x, tercomEst->y
                     -currEstimate->y, tercomEst->z - currEstimate->z);
              printf("Estimation Bias (Mean): (t = %.2f)\n", mmseEst->time);
              printf("North: %.4f, East: %.4f, Depth: %.4f\n",
                     mmseEst->x - currEstimate->x, 
                     mmseEst->y - currEstimate->y,
                     mmseEst->z - currEstimate->z);
              if(filterType == 2)
                 printf("Psi Bias & Sigma: %.2f +/- %.3f\n", 
                        (mmseEst->psi - currEstimate->psi)*180.0/PI, 
                        sqrt(mmseEst->covariance[20])*180.0/PI);
              
              printf("North Sigma: %.2f, East Sigma: %.2f, Depth Sigma: %.2f\n\n", 
                     sqrt(mmseEst->covariance[0]),
                     sqrt(mmseEst->covariance[2]),
                     sqrt(mmseEst->covariance[5]));
              
              if(saveResults)
              {
                 tfile << setprecision(15) << tercomEst->x << " "
                       << tercomEst->y << " " << tercomEst->z << " "
                       << tercomEst->phi << " " << tercomEst->theta
                       << " " << tercomEst->psi << " " << tercomEst->time 
                       << endl;
                 pfile << setprecision(15) << currEstimate->x << " "
                       << currEstimate->y << " " << currEstimate->z << " "
                       << currEstimate->phi << " " << currEstimate->theta 
                       << " " << currEstimate->psi << " " 
                       << currEstimate->time << endl;
                 mfile << setprecision(15) << mmseEst->x << " " 
                       << mmseEst->y << " " << mmseEst->z << " " 
                       << mmseEst->phi << " " << mmseEst->theta << " " 
                       << mmseEst->psi;
                 if(SEARCH_GYRO_BIAS)
                    mfile << " " << mmseEst->wy << " " << mmseEst->wz;
                 mfile << " " << mmseEst->covariance[0]
                       << " " << mmseEst->covariance[2]
                       << " " << mmseEst->covariance[1]
                       << " " << mmseEst->covariance[5]
                       << " " << mmseEst->covariance[9]
                       << " " << mmseEst->covariance[14]
                       << " " << mmseEst->covariance[20];
                 if(SEARCH_GYRO_BIAS)
                 {
                    mfile << "\t" << mmseEst->covariance[27];
                    mfile << "\t" << mmseEst->covariance[35];
                 }
                 mfile << endl;
                 
                 if(filterType == 2 && SAVE_PARTICLES)
                    tercom->tNavFilter->saveCurrDistrib(ffile);
              }
           }
        }
     }             
     
      //get current clock time
      clock_gettime(CLOCK_REALTIME, &now);
      elapsed = now.tv_sec  - lastTime.tv_sec +
         (now.tv_nsec - lastTime.tv_nsec)/1.e9;
      if(realTime) printf("Computation time = %.2f msec\n", elapsed*1000);
  }
  
  //save filter and estimates if created
  if(i > i_init && saveResults)
  {
     //compute tercom MLE pose estimate
     tercom->estimatePose(tercomEst, 1);
     
     //compute tercom MMSE pose estimate 
     tercom->estimatePose(mmseEst, 2);
     
     //compute and save tercom MLE pose estimate
     tfile << setprecision(15) << tercomEst->x << " "
           << tercomEst->y << " " << tercomEst->z << " "
           << tercomEst->phi << " " << tercomEst->theta
           << " " << tercomEst->psi << " " << tercomEst->time 
           << endl;
     
     pfile << setprecision(15) << currEstimate->x << " "
           << currEstimate->y << " " << currEstimate->z << " "
           << currEstimate->phi << " " << currEstimate->theta 
           << " " << currEstimate->psi << " " 
           << currEstimate->time << endl;
     mfile << setprecision(15) << mmseEst->x << " " 
           << mmseEst->y << " " << mmseEst->z << " " 
           << mmseEst->phi << " " << mmseEst->theta << " " 
           << mmseEst->psi;
     if(SEARCH_GYRO_BIAS)
        mfile << " " << mmseEst->wy << " " << mmseEst->wz;
     mfile << " " << mmseEst->covariance[0]
           << " " << mmseEst->covariance[2]
           << " " << mmseEst->covariance[1]
           << " " << mmseEst->covariance[5]
           << " " << mmseEst->covariance[9]
           << " " << mmseEst->covariance[14]
           << " " << mmseEst->covariance[20];
     if(SEARCH_GYRO_BIAS)
     {
        mfile << "\t" << mmseEst->covariance[27];
        mfile << "\t" << mmseEst->covariance[35];
     }
     mfile << endl;
     
     if(filterType == 2 && SAVE_PARTICLES)
        tercom->tNavFilter->saveCurrDistrib(ffile);
     
     //save final filter distribution      
     tercom->tNavFilter->saveCurrDistrib(distribFile);
     
  }

  elapsed = now.tv_sec  - startTime.tv_sec +
	(now.tv_nsec - startTime.tv_nsec)/1.e9;
  printf("Total Elapsed Time: = %.2f sec\n", elapsed);

  pfile.close();
  tfile.close();
  mfile.close();
  ffile.close();

  //delete allocated memory
  delete tercom;
  delete currMeas;
  delete currEstimate;
}
   

//Load data from a given file into a matrix structure
Matrix readDataFromFile(const char *fileName, const int &numRows, 
			const int &numCols)
{
  Matrix data(numRows,numCols);
	
  ifstream datafile;
  printf("Loading %s...\n", fileName);

  datafile.open(fileName);

  if(datafile.is_open())
    {
      int row = 1;
      //char *num;
      char num[128];
      num[127] = '\0';
      while(!datafile.eof() && row <=numRows)	//check that not at end of file
	{
	  for (int col = 1; col <= numCols; col++)
	    {
	      datafile >> setprecision(15) >> num;

	      data(row,col) = atof(num);
	      //printf("%.2f  ", data(row,col));
	    }
	  datafile.ignore(256,'\n');
	  row++;
	  //printf("\n");
	} 

      datafile.close();

      return data;
    }
  else
    {
      printf("Unable to open file %s.  Exiting...\n", fileName);
      exit(0);
    }
  return data;
}

// assign a kearfott pose matrix into a poseT structure.
bool assignKearfottEstimate(poseT* currEstimate, Matrix kftPose)
{
  currEstimate->time = kftPose(1,1);
  currEstimate->dvlValid = kftPose(1,2);
  currEstimate->gpsValid = kftPose(1,3);
  currEstimate->bottomLock = kftPose(1,4);
  currEstimate->x = kftPose(1,7);	//North
  currEstimate->y = kftPose(1,8);	//East
  currEstimate->z = kftPose(1,9); //Depth
  currEstimate->phi = kftPose(1,10);//-0.74*PI/180;
  currEstimate->theta = kftPose(1,11);//-1.13*PI/180;
  currEstimate->psi = kftPose(1,12);
  currEstimate->vx = kftPose(1,13);
  currEstimate->vy = kftPose(1,14);
  currEstimate->vz = kftPose(1,15);
  currEstimate->ax = kftPose(1,16);
  currEstimate->ay = kftPose(1,17);
  currEstimate->az = kftPose(1,18);
  currEstimate->wx = kftPose(1,19);
  currEstimate->wy = kftPose(1,20);
  currEstimate->wz = kftPose(1,21);
  
  return true;
}

// assign a water velocity data into poseT structure
bool assignWatVelocity(poseT* currEstimate, Matrix watVel)
{
  currEstimate->vw_x = watVel(1,1);
  currEstimate->vw_y = watVel(1,2);
  currEstimate->vw_z = watVel(1,3);

  return true;
}

//assign dvl measurement into a measT structure
void assignDVLMeasurement(measT* currMeas, Matrix meas)
{
  currMeas->dataType = 1;
  currMeas->time = meas(1,1);
  currMeas->ranges[0] = meas(1,16);
  currMeas->ranges[1] = meas(1,17);
  currMeas->ranges[2] = meas(1,18);
  currMeas->ranges[3] = meas(1,19);
  currMeas->numMeas = 4;
  currMeas->phi = meas(1,14);
  currMeas->theta = meas(1,13);
  currMeas->psi = meas(1,15);
  currMeas->measStatus[0] = meas(1,22);
  currMeas->measStatus[1] = meas(1,23);
  currMeas->measStatus[2] = meas(1,24);
  currMeas->measStatus[3] = meas(1,25);
  currMeas->x = meas(1,26);
  currMeas->y = meas(1,27);
  currMeas->z = meas(1,28);

}

//assign multibeam measurement into a measT structure
void assignMBMeasurement(measT* currMeas, Matrix meas){

  currMeas->dataType = 2;
  if(AVERAGE)
    {
      currMeas->numMeas = 2;
      int j = 0;
      for(int i = 10; i < 12; i++)
	{
	  currMeas->alongTrack[j] = meas(1,(i-1)*3+2);
	  currMeas->crossTrack[j] = meas(1,(i-1)*3+3);
	  currMeas->altitudes[j] = meas(1,(i-1)*3+4);
	  j++;
	}
    }
  else
    {
      currMeas->numMeas = (meas.Ncols() - 1)/3;
      int j = 0;
      for(int i = 2; i < meas.Ncols(); i=i+3)
	{
	  currMeas->alongTrack[j] = meas(1,i);
	  currMeas->crossTrack[j] = meas(1,i+1);
	  currMeas->altitudes[j] = meas(1,i+2);
	  j++;
	}
    }

  for(int i  =0; i < currMeas->numMeas; i++)
    currMeas->measStatus[i] = true;

  //measurement frame accounts for phi and theta, but not for psi
  currMeas->phi = 0;
  currMeas->theta = 0;
  currMeas->psi = 0;
  currMeas->time = meas(1,1);

}

//assign altimeter measurement into a measT structure
void assignAltMeasurement(measT* currMeas, Matrix meas)
{
   int i;
   currMeas->dataType = 3;
   currMeas->time = meas(1,1);
   currMeas->ranges[0] = meas(1,3);
  
   for(i = 1; i < 4; i++)
      currMeas->ranges[i] = 0.0;
   currMeas->numMeas = 1;
   currMeas->measStatus[0] = meas(1,4);
   currMeas->theta = -meas(1,2);
 }

//update data in plotter window
void updatePlotterData(ParticlePlot &plotter, poseT *currEstimate, 
                       poseT *mmseEst, TNavFilter* tNavFilter, mapT &currMap, 
                       const double filterType, const double startTime)
{
   char temp[256];
   particleT* particles;
   double mapBounds[4];

   plotter.addReferencePath(currEstimate->x, currEstimate->y);
   plotter.addVehiclePath(mmseEst->x, mmseEst->y);
   if(filterType == 2)
   {
      //get current particle set
      particles = ((TNavParticleFilter *)(tNavFilter))->getParticles();

      //if relevant map area is reasonably sized, plot particles on top of map
      if(currMap.depths.Nrows() <= 350 && currMap.depths.Ncols() <= 350)
      {
        //plot submap boundary
         tNavFilter->getTerrainMapBounds(mapBounds);
         plotter.setMapBoundary(mapBounds[0],mapBounds[1],mapBounds[2],
                                mapBounds[3]);
         plotter.setData(currMap.depths,currMap.xpts,currMap.ypts,particles,
                         currEstimate->x, currEstimate->y);
      }
   }
   else
   {
      //get current PMF distribution
      if(((TNavPointMassFilter *)(tNavFilter))->getCurrPDF(currMap))
         plotter.setDataMapOnly(currMap.depths,currMap.xpts,currMap.ypts,currEstimate->x, currEstimate->y);
   }

   sprintf(temp,"Estimation Bias (Mean): (t = %.2f)\n, North: %.4f, East: %.4f, Depth: %.4f\n", 
           mmseEst->time - startTime, mmseEst->x -currEstimate->x, 
           mmseEst->y - currEstimate->y, 
           mmseEst->z - currEstimate->z);
   plotter.setString(temp);
}


//save simulation parameters
void saveRunParameters(const char* savePath)
{
   char fileName[256];
   FILE * sfile;
   sfile = fopen(charCat(fileName,savePath,"filterParams.txt"),"w");
  
   //if unable to open parameter file, exit function.
   if(sfile == NULL)
      return;
   
   //Save TNavFilter settings
   fprintf(sfile, "\n\nTNavFilter Settings:\n");
   fprintf(sfile, "--------------------\n");     
   fprintf(sfile, "VEL_PER_ERROR: \t\t %.2f \n", VEL_PER_ERROR);
   fprintf(sfile, "WATER_VEL_PER_ERROR: \t %.2f \n", WATER_VEL_PER_ERROR);
   fprintf(sfile, "VEL_STDDEV: \t\t %.2f \n", VEL_STDDEV);
   fprintf(sfile, "USE_MAP_NAN: \t\t %i \n", USE_MAP_NAN);
   fprintf(sfile, "USE_COMPASS_BIAS: \t %i \n", USE_COMPASS_BIAS);
   fprintf(sfile, "HOMER_RANGE_PER_ERROR: \t %.2f \n", HOMER_RANGE_PER_ERROR);
   fprintf(sfile, "USE_RANGE_CORR: \t %i \n", USE_RANGE_CORR);
   fprintf(sfile, "USE_ACCEL: \t\t %i \n", USE_ACCEL);

   //Save TerrainNav settings
   fprintf(sfile, "\n\nTerrainNav Settings:\n");
   fprintf(sfile, "--------------------\n");
   fprintf(sfile, "MEAS_BUFFER_SIZE: \t %i \n", MEAS_BUFFER_SIZE);
   fprintf(sfile, "MAX_INTERP_TIME: \t %.2f \n", MAX_INTERP_TIME);
   fprintf(sfile, "MAX_RANGE: \t\t %.2f \n", MAX_RANGE);
   fprintf(sfile, "MIN_RANGE: \t\t %.2f \n", MIN_RANGE);
   fprintf(sfile, "MAX_VEL: \t\t %.2f \n", MAX_VEL);
   fprintf(sfile, "MAX_ACCEL: \t\t %.2f \n", MAX_ACCEL);
   fprintf(sfile, "MAX_DRDT: \t\t %.2f \n", MAX_DRDT);

   //Save TerrainMap settings
   fprintf(sfile, "\n\nTerrainMap Settings:\n");
   fprintf(sfile, "--------------------\n");
   fprintf(sfile, "VARIOGRAM FRACTAL DIM: \t\n");
   fprintf(sfile, "VARIOGRAM ALPHA: \t\n");

   //Save ParticleFilterDefs
   fprintf(sfile, "\n\nParticle Filter Defs:\n");
   fprintf(sfile, "-----------------------\n");
  
   fprintf(sfile, "MAX_PARTICLES: \t\t %i \n", MAX_PARTICLES);
   fprintf(sfile, "MOVING_TERRAIN: \t %i \n",MOVING_TERRAIN);
   fprintf(sfile, "USE_AUG_MCL: \t\t %i \n",USE_AUG_MCL);
   fprintf(sfile, "USE_CONTOUR_MATCHING: \t %i \n",USE_CONTOUR_MATCHING);
   fprintf(sfile, "INTEG_PHI_THETA: \t %i \n", INTEG_PHI_THETA);
   fprintf(sfile, "ALLOW_ATTITUDE_SEARCH: \t %i \n", ALLOW_ATTITUDE_SEARCH);
   fprintf(sfile, "ALLOW_FILTER_REINIT: \t %i \n", ALLOW_FILTER_REINIT);
   fprintf(sfile, "USE_MODIFIED_WEIGHTING: \t %i \n", USE_MODIFIED_WEIGHTING);
   fprintf(sfile, "SEARCH_COMPASS_BIAS: \t %i \n", SEARCH_COMPASS_BIAS); 
   fprintf(sfile, "SEARCH_ALIGN_STATE: \t %i \n", SEARCH_ALIGN_STATE); 
   fprintf(sfile, "SEARCH_GYRO_BIAS: \t %i \n", SEARCH_GYRO_BIAS);
   fprintf(sfile, "SEARCH_GYRO_Y: \t\t %i \n", SEARCH_GYRO_Y);
   fprintf(sfile, "SEARCH_WAT_VEL: \t %i \n", SEARCH_WAT_VEL); 
   fprintf(sfile, "SEARCH_DVL_ERRORS: \t %i \n", SEARCH_DVL_ERRORS);
   fprintf(sfile, "SAVE_PARTICLES: \t %i \n", SAVE_PARTICLES);
   fprintf(sfile, "X_STDDEV_INIT: \t\t %.2f \n", X_STDDEV_INIT);
   fprintf(sfile, "Y_STDDEV_INIT: \t\t %.2f \n", Y_STDDEV_INIT);
   fprintf(sfile, "Z_STDDEV_INIT: \t\t %.2f \n", Z_STDDEV_INIT); 
   fprintf(sfile, "PHI_STDDEV_INIT (^o): \t\t %.2f \n", PHI_STDDEV_INIT*180.0/PI); 
   fprintf(sfile, "THETA_STDDEV_INIT (^o): \t %.2f \n", THETA_STDDEV_INIT*180.0/PI);
   fprintf(sfile, "PSI_STDDEV_INIT (^o): \t\t %.2f \n", PSI_STDDEV_INIT*180.0/PI); 
   fprintf(sfile, "COMPASS_BIAS_STDDEV_INIT (^o): \t %.2f \n", COMPASS_BIAS_STDDEV_INIT*180.0/PI); 
   fprintf(sfile, "PHI_ALIGN_ERROR_STDDEV_INIT (^o): \t %.2f \n", PHI_ALIGN_ERROR_STDDEV_INIT*180.0/PI); 
   fprintf(sfile, "THETA_ALIGN_ERROR_STDDEV_INIT (^o): \t %.2f \n",  THETA_ALIGN_ERROR_STDDEV_INIT*180.0/PI);
   fprintf(sfile, "PSI_ALIGN_ERROR_STDDEV_INIT (^o): \t %.2f \n", PSI_ALIGN_ERROR_STDDEV_INIT*180.0/PI); 
   fprintf(sfile, "GYRO_BIAS_STDDEV_INIT (^o/s): \t %.2f \n", GYRO_BIAS_STDDEV_INIT*180.0/PI); 
   fprintf(sfile, "WAT_VEL_STDDEV_INIT (m/s): \t %.2f \n", WAT_VEL_STDDEV_INIT); 
   fprintf(sfile, "DVL_SF_STDDEV_INIT (m/s): \t %.2f \n", DVL_SF_STDDEV_INIT); 
   fprintf(sfile, "DVL_BIAS_STDDEV_INIT (m/s): \t %.2f \n", DVL_BIAS_STDDEV_INIT); 
   fprintf(sfile, "DZ_STDDEV: \t\t %.2f \n", DZ_STDDEV); 
   fprintf(sfile, "DPHI_STDDEV (^o): \t %.2f \n", DPHI_STDDEV*180.0/PI); 
   fprintf(sfile, "DTHETA_STDDEV (^o): \t %.2f \n", DTHETA_STDDEV*180.0/PI); 
   fprintf(sfile, "DPSI_STDDEV (^o): \t %.2f \n", DPSI_STDDEV*180.0/PI); 
   fprintf(sfile, "DPSI_RATE_FACTOR_STDDEV: \t %.2f \n", DPSI_RATE_FACTOR_STDDEV*180.0/PI); 
   fprintf(sfile, "DALIGN_STDDEV: \t\t\t %.2f \n",  DALIGN_STDDEV*180.0/PI); 
   fprintf(sfile, "DGBIAS_ERROR (^o/sqrt(s)): \t %.6f \n", DGBIAS_ERROR*180.0/PI); 
   fprintf(sfile, "DWAT_VEL_STDDEV: \t %.2f \n",  DWAT_VEL_STDDEV); 
   fprintf(sfile, "DDVLSF_STDDEV: \t\t %.4f \n",  DDVLSF_STDDEV); 
   fprintf(sfile, "DDVLBIAS_STDDEV: \t %.4f \n", DDVLBIAS_STDDEV);
   fprintf(sfile, "MIN_EFF_SAMP_SIZE: \t %.2f \n",  MIN_EFF_SAMP_SIZE); 

   fclose(sfile);
}