#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 "genFilterDefs.h"

#define NANOSEC_PER_SEC (1000000000L)

void runTerrainNav(const Matrix &dataKft, const Matrix &dataMeas,
                   const Matrix &watVel, const Matrix &homerMeas,
		   char* mapPath, int interpMethod, int kSubSample,
                   int mSubSample, poseT *tercomEst, poseT *mmseEst, 
                   bool realTime, int filterType, ofstream &distribFile, 
                   char* savePath, int dataK_init, int dataH_init);
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 assignHomerMeasurement(measT* currMeas, Matrix meas);
void saveRunParameters(const char* savePath);

int main( int argc, char **argv )
{
  bool realTime = false;
  int i,j;
  int numRepeat = 1;

  ofstream trnfile, mmsefile, homerMmseFile,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/debbie/Documents/Research_Files/dataFiles/";
  //"/media/Data/mbariDiveData/imagingAUV/2009_10_13/";
  char mapPath[256] = 
     "/home/debbie/Documents/Research_Files/Maps/";
     //"/home/debbie/Research_Files/mbariCode/mapResStudies/uustPaper/";
  char map_file[256] = "SoquelCanyon/SoquelCanyonMAUVUTMTopo_NoNan.grd";
     //"SoquelCanyon/soquelCanyonGW_101309_1mRes.grd";
     //"PortugueseLedge/PortugueseLedge20080424TopoUTM_NoNan.grd";
 
 //initialize memory for different tests
  int numTests = 5;
  int num = 0;
  int* dataK_numRows = new int[numTests];
  int* dataMeas_numRows = new int[numTests];
  int* init_dataK = new int[numTests];
  int* init_dataH = new int[numTests];
  int* homerMeas_numRows = new int[numTests];
  char** dataK_files = new char*[numTests];
  char** watVel_files = new char*[numTests];
  char** homerMeas_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];
     homerMeas_files[i] = new char[256];
     dataMeas_files[i] = new char[256];
     savePath[i] = new char[256];
  }

 
  //Define specific file information
  
  //TEST 05 - 07/19/10
  dataK_numRows[num] = 5804;//373;//5804;
  dataMeas_numRows[num] = 1511;//373;//1511;
  homerMeas_numRows[num] = 231;
  init_dataK[num] = 2400;//1856;//5029;
  init_dataH[num] = 1;//193;//194;
  strcpy(dataK_files[num],"2010_07_19diveFiles/dataKft_test05all_071910dive.txt");
  strcpy(dataMeas_files[num],"2010_07_19diveFiles/measData_test05all_rangeCorrected_071910dive.txt");
  strcpy(homerMeas_files[num],"2010_07_19diveFiles/sonardyneData_test05_071910dive.txt");
  //savePath[num] = "/home/debbie/Documents/Research_Files/dataFiles/2010_07_19diveFiles/results/test05all/";
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/071910_test05all/");
  num++;
  
  
  //TEST 08 - 07/19/10
  dataK_numRows[num] = 5086;//5086;//324;
  dataMeas_numRows[num] = 1259;//1259;//324;
  homerMeas_numRows[num] = 370;
  init_dataK[num] = 800;//1856;//5029;
  init_dataH[num] = 1;//193;//194;
  strcpy(dataK_files[num],"2010_07_19diveFiles/dataKft_test08all_071910dive.txt");
  strcpy(dataMeas_files[num],"2010_07_19diveFiles/measData_test08all_rangeCorrected_071910dive.txt");
  strcpy(homerMeas_files[num],"2010_07_19diveFiles/sonardyneData_test08_071910dive.txt");
  //savePath[num] = "/home/debbie/Documents/Research_Files/dataFiles/2010_07_19diveFiles/results/test08all/";
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/071910_test08all/");
  num++;
  
  
  //TEST 09 - 07/19/10
  dataK_numRows[num] = 5552;//5552;//358;
  dataMeas_numRows[num] = 1477;//1477;//358;
  homerMeas_numRows[num] = 296;
  init_dataK[num] = 1500;//1856;//5029;
  init_dataH[num] = 1;//193;//194;
  strcpy(dataK_files[num],"2010_07_19diveFiles/dataKft_test09all_071910dive.txt");
  strcpy(dataMeas_files[num],"2010_07_19diveFiles/measData_test09all_rangeCorrected_071910dive.txt");
  strcpy(homerMeas_files[num],"2010_07_19diveFiles/sonardyneData_test09_071910dive.txt");
  //savePath[num] = "/home/debbie/Documents/Research_Files/dataFiles/2010_07_19diveFiles/results/test09all/";
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/071910_test09all/");
  num++;
  
  
  //TEST 11 - 07/19/10
  dataK_numRows[num] = 5430;//5430;//358;
  dataMeas_numRows[num] = 1335;//1335;//358;
  homerMeas_numRows[num] = 467;
  init_dataK[num] = 1500;//1856;//5029;
  init_dataH[num] = 1;//193;//194;
  dataK_files[num] = "2010_07_19diveFiles/dataKft_test11all_071910dive.txt";
  dataMeas_files[num] =  "2010_07_19diveFiles/measData_test11all_rangeCorrected_071910dive.txt";
  homerMeas_files[num] = "2010_07_19diveFiles/sonardyneData_test11_071910dive.txt";
  //savePath[num] = "/home/debbie/Documents/Research_Files/dataFiles/2010_07_19diveFiles/results/test11all/";
  savePath[num] = "/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/071910_test11all/";
  num++;  

  
  //TEST 03 - 03/30/10
  dataK_numRows[num] = 9953;
  dataMeas_numRows[num] = 2899;
  homerMeas_numRows[num] = 123;
  init_dataK[num] = 5500;//1856;//5029;
  init_dataH[num] = 1;//193;//194;
  strcpy(dataK_files[num],"2010_03_30diveFiles/dataKft_test03all_033010dive.txt");
  strcpy(dataMeas_files[num],"2010_03_30diveFiles/measData_test03all_rangeCorrected_033010dive.txt");
  strcpy(homerMeas_files[num],"2010_03_30diveFiles/sonardyneData_test03_033010dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/033010_test03all/");
  num++;
  
  //TEST 06 - 03/30/10
  dataK_numRows[num] = 6923;
  dataMeas_numRows[num] = 1509;
  homerMeas_numRows[num] = 175;
  init_dataK[num] = 2300;//1856;//5029;
  init_dataH[num] = 1;//193;//194;
  strcpy(dataK_files[num],"2010_03_30diveFiles/dataKft_test06all_033010dive.txt");
  strcpy(dataMeas_files[num],"2010_03_30diveFiles/measData_test06all_rangeCorrected_033010dive.txt");
  strcpy(homerMeas_files[num],"2010_03_30diveFiles/sonardyneData_test06_033010dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/033010_test06all/");
  num++;
  /*
  
  //TEST 07 - 03/30/10
  dataK_numRows[num] = 6072;
  dataMeas_numRows[num] = 1310;
  homerMeas_numRows[num] = 76;
  init_dataK[num] = 1;//1856;//5029;
  init_dataH[num] = 1;
  strcpy(dataK_files[num],"2010_03_30diveFiles/dataKft_test07all_033010dive.txt");
  strcpy(dataMeas_files[num],"2010_03_30diveFiles/measData_test07all_rangeCorrected_033010dive.txt");
  strcpy(homerMeas_files[num],"2010_03_30diveFiles/sonardyneData_test07_033010dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/033010_test07all/");
  num++;

  //TEST 03 - 02/10/10
  dataK_numRows[num] = 6534;
  dataMeas_numRows[num] = 1917;
  homerMeas_numRows[num] = 203;
  init_dataK[num] = 1856;//5029;
  init_dataH[num] = 1;//140;//141;
  strcpy(dataK_files[num],"2010_02_10diveFiles/dataKft_test03all_021010dive.txt");
  strcpy(dataMeas_files[num],"2010_02_10diveFiles/measData_test03all_rangeCorrected_021010dive.txt");
  strcpy(homerMeas_files[num],"2010_02_10diveFiles/sonardyneData_test03_021010dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/test03all/");
  num++;
  
  //TEST 04 - 02/10/10
  dataK_numRows[num] = 6852;
  dataMeas_numRows[num] = 2045;
  homerMeas_numRows[num] = 270;
  init_dataK[num] = 1800;//1800;//2342;//5342;
  init_dataH[num] = 1;//204;//205;
  strcpy(dataK_files[num],"2010_02_10diveFiles/dataKft_test04all_021010dive.txt");
  strcpy(dataMeas_files[num],"2010_02_10diveFiles/measData_test04all_rangeCorrected_021010dive.txt");
  strcpy(homerMeas_files[num],"2010_02_10diveFiles/sonardyneData_test04_021010dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/test04all/");
  num++;  
  
  //TEST 06 - 10/20/09
  dataK_numRows[num] = 6396;
  dataMeas_numRows[num] = 1864;
  homerMeas_numRows[num] = 218;
  init_dataK[num] = 2000;//2000;//4763;
  init_dataH[num] = 1;//193;//194;
  strcpy(dataK_files[num],"2009_10_20diveFiles/dataKft_test06all_102009dive.txt");
  strcpy(dataMeas_files[num],"2009_10_20diveFiles/measData_test06all_rangeCorrected_102009dive.txt");
  strcpy(homerMeas_files[num],"2009_10_20diveFiles/sonardyneData_test06_102009dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/test06all/");
  num++;
  
  //TEST 07 - 10/20/09
  dataK_numRows[num] = 4529;
  dataMeas_numRows[num] = 1301;
  homerMeas_numRows[num] = 299;
  init_dataK[num] = 1100;//1100;//2828;
  init_dataH[num] = 1;//275;//278;
  strcpy(dataK_files[num],"2009_10_20diveFiles/dataKft_test07all_102009dive.txt");
  strcpy(dataMeas_files[num], "2009_10_20diveFiles/measData_test07all_rangeCorrected_102009dive.txt");
  strcpy(homerMeas_files[num],"2009_10_20diveFiles/sonardyneData_test07_102009dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/test07all/");
  num++;

  //TEST 09 - 10/20/09
  dataK_numRows[num] = 5011;
  dataMeas_numRows[num] = 1580;
  homerMeas_numRows[num] = 209;
  init_dataK[num] = 1800;//1800;//3457;
  init_dataH[num] = 1;//190;//191;
  strcpy(dataK_files[num] = "2009_10_20diveFiles/dataKft_test09all_102009dive.txt";
  strcpy(dataMeas_files[num],"2009_10_20diveFiles/measData_test09all_rangeCorrected_102009dive.txt");
  strcpy(homerMeas_files[num],"2009_10_20diveFiles/sonardyneData_test09_102009dive.txt");
  strcpy(savePath[num],"/home/debbie/Documents/Research_Files/dataFiles/homer50runs/newBeamProjection/test09all/");
  //num++;
  */

  //initialize data structures
  poseT *tercomEst = new poseT;
  poseT *mmseEst = new poseT;
  Matrix dataKft, dataMeas, watVel, homerMeas, currHomer;

  for(i = 0; i < numTests; i++)
  {
     trnfile.open(charCat(fileName,savePath[i],"allMle.txt"));
     mmsefile.open(charCat(fileName,savePath[i],"allMmse.txt"));
     homerMmseFile.open(charCat(fileName,savePath[i],"allHomerMmse.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]+100, 62);
     dataKft = readDataFromFile(charCat(fileName,dataPath,dataK_files[i]),
                                dataK_numRows[i]+600, 22);
     //watVel = readDataFromFile(charCat(fileName,dataPath,watVel_files[i]),
     //                          dataK_numRows[i], 3);
     homerMeas = readDataFromFile(charCat(fileName,dataPath,homerMeas_files[i]),
                                  homerMeas_numRows[i], 4);
         
     for(j = 0; j < numRepeat; j++)
     {  
        sprintf(fileName,"%s%s",mapPath,map_file);
        runTerrainNav(dataKft, dataMeas, watVel, homerMeas, fileName, 0, 5, 3, 
                      tercomEst, mmseEst, realTime, 2, distribfile, 
                      savePath[i], init_dataK[i],init_dataH[i]);
        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
                 << "\t" << 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;
        currHomer = readDataFromFile(charCat(fileName,savePath[i],
                                             "homerMmse.txt"),1,6);
        homerMmseFile << setprecision(15) << currHomer(1,1) << "\t" 
                      << currHomer(1,2) << "\t" << currHomer(1,3) 
                      << "\t" << currHomer(1,4) << "\t"
                      << currHomer(1,5) << "\t" << currHomer(1,6) << endl;
     }
     
     saveRunParameters(savePath[i]);

     trnfile.close();
     mmsefile.close();
     homerMmseFile.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 homerMeas_files[i];
  }
  delete [] dataMeas_files;
  delete [] dataK_files;
  delete [] watVel_files;
  delete [] homerMeas_files;
  delete [] savePath;
  delete [] dataK_numRows;
  delete [] dataMeas_numRows;
  delete [] init_dataK;
  delete [] init_dataH;
  delete [] homerMeas_numRows;

  return 0;
}


// run terrainNav algorithm for a particular set of data
void runTerrainNav(const Matrix &dataKft, const Matrix &dataMeas, 
                   const Matrix &watVel, const Matrix &homerMeas,
		   char* mapPath, int interpMethod, int kSubSample,
                   int mSubSample, poseT *tercomEst, poseT *mmseEst, 
                   bool realTime, int filterType, ofstream &distribFile, 
                   char* savePath, int dataK_init, int dataH_init)
{
  //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 P = homerMeas.Nrows();
  int i_init = dataK_init;
  int j_init = 1;
  int k_init = dataH_init;
  int i,j,k,m;
  sem_t semaphore;
  struct timespec semTime;

  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, "imagingAUV_specs.cfg",
                                      filterType, savePath);

  //set random number generator for test cases
  srand(2000);
  tercom->tNavFilter->setMapInterpMethod(interpMethod);
  tercom->tNavFilter->setInterpMeasAttitude(true);
  printf("Terrain navigation object initialized.\n");

  //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;
  lastTime.tv_sec  = semTime.tv_sec;
  lastTime.tv_nsec = semTime.tv_nsec;

  // Sampling interval:
  short TsMsec = 500;
  i = i_init;
  j = j_init;
  k = k_init;
  int minIdx = 3;
  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
      minIdx = 3;
     
      if(j > M)
      {
         if(k > P || (i <=N && dataKft(i,1) <= homerMeas(k,1)))
            minIdx = 1;
      }
      else
      {
         if(i > N)
         {
            if(k > P || dataMeas(j,2) <= homerMeas(k,1))
               minIdx = 2;
         }
         else
         {
            if(dataKft(i,1) <= dataMeas(j,2))
            {
               if(k > P || dataKft(i,1) <= homerMeas(k,1))
                  minIdx = 1;
            }
            else
            {
               if(k > P || dataMeas(j,2) <= homerMeas(k,1))
                  minIdx = 2;
            }
         }
      }

  
      switch(minIdx)
      {
      
      case 1:
         //apply motion update
          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;

          break;

      case 2:
         //apply meas update
         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 > 1 && tercom->lastMeasSuccessful())
         {	  
            //compute tercom MLE pose estimate
            tercom->estimatePose(tercomEst, 1);
            
            //compute tercom MMSE pose estimate 
            tercom->estimatePose(mmseEst, 2);
            
            //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);
            }
         }
         
         
         break;
         
      case 3:
         //homer measurement
         printf("Homer Update.. (t = %.2f)\n", homerMeas(k,1));        
         
         assignHomerMeasurement(currMeas,
                         homerMeas.SubMatrix(k,k,1,homerMeas.Ncols()));
        
         tercom->measUpdate(currMeas, currMeas->dataType);
         k++;
        
         break;
         
      default:
         continue;
      }
    
      //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);
 }

//assign altimeter measurement into a measT structure
void assignHomerMeasurement(measT* currMeas, Matrix meas)
{
   currMeas->dataType = 4;
   currMeas->time = meas(1,1);
   currMeas->alongTrack[0] = meas(1,2);
   currMeas->crossTrack[0] = meas(1,3);
   currMeas->altitudes[0] = meas(1,4);
   currMeas->numMeas = 1;
   currMeas->measStatus[0] = true;
}

//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, "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);
}