#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"
#include "octreeDefs.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, int dataK_init);
Matrix readDataFromFile(const char *fileName, const int &numRows, 
			const int &numCols);
bool assignKearfottEstimate(poseT* currEstimate, Matrix pose);
void assignDVLMeasurement(measT* currMeas, Matrix meas);
void assignMBMeasurement(measT* currMeas, Matrix meas);
void assignAltMeasurement(measT* currMeas, Matrix meas);

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

  
  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]);
  }
  
 
 
 //initialize memory for different tests
  int numTests = 2;
  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** map_files = new char*[numTests];
	double** mmseResults = new double*[numTests];
  bool* successful = new bool[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];
     map_files[i] = new char[256];
  }


    //Define paths where the data is stored
  char fileName[512];

#if 0
#ifdef _QNX
  char dataPath[256] = 
     "/home/dorado1"
     "/auv-qnx/auv/altex/onboard/terrainNav/dataFiles/";
  char mapPath[256] = 
     "/home/dorado1"
     "/auv-qnx/auv/altex/onboard/terrainNav/Maps/";
#else
  char dataPath[256] = 
		"/home/shouts/camelot/Projects/mbari/diveData/";
  char mapPath[256] = 
		"/home/shouts/camelot/Projects/mbari/mbariMaps/";
#endif
#endif


  char dataPath[256] = 
     "/home/dorado1"
     "/auv-qnx/auv/altex/onboard/terrainNav/dataFiles/";
  char mapPath[256] = 
     "/home/dorado1"
     "/auv-qnx/auv/altex/onboard/terrainNav/Maps/";


  //Define specific file information
 
  //TEST 1: Dvl data from 8/04/08 MAUV dive at Portuguese Ledge
  strcpy(dataK_files[num],"2008_08_04diveFiles/dataKft_test04all_080804dive.txt");
  strcpy(dataMeas_files[num],"2008_08_04diveFiles/measData_test04all_080804dive.txt");
  if(USE_OCTREE)
  	strcpy(map_files[num],"PortugueseLedge/PortugueseLedge3dPointsDEM.txt");
  else
  	strcpy(map_files[num],"PortugueseLedge/PortugueseLedge20080424TopoUTM_NoNan.grd");
  init_dataK[num] = 100;
  dataK_numRows[num] = 9285;//5761;
  dataMeas_numRows[num] = 3721;//2356;
  double currResults1[] = {12,-11,0.3};
  mmseResults[num] = currResults1;
  num++;
 

  //TEST 2: 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");
  if(USE_OCTREE)
  	strcpy(map_files[num],"SoquelCanyon/SoquelCanyon3dPointsDEM.txt");
  else
  	strcpy(map_files[num],"SoquelCanyon/SoquelCanyonMAUVUTMTopo_061709cut.grd");
  init_dataK[num] = 100;
  dataK_numRows[num] = 10464;
  dataMeas_numRows[num] = 9015;
  double currResults2[] = {0.0,0.0,0.0};
  if(USE_OCTREE){
    currResults2[0] = 19;
    currResults2[1] = -15;
    currResults2[2] = -3.7;}
  else{
  	currResults2[0] = 13.8;
    currResults2[1] = -6.2;
    currResults2[2] = -2.7;}

  mmseResults[num] = currResults2;
  num++;

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

  for(i = 0; i < numTests; i++)
  {
     
     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_files[i]);
     
     for(j = 0; j < numRepeat; j++)
     {  
        runTerrainNav(dataKft, dataMeas, watVel, fileName, 0, 15, 15, 
                      tercomEst, mmseEst, realTime, 2, init_dataK[i]);
        
     }
     
     //check results against expected results
     idx = closestPtUniformArray(mmseEst->time, dataKft(1,1),dataKft(dataK_numRows[i],1),dataK_numRows[i]);
      if((fabs(mmseEst->x - dataKft(idx,7) - mmseResults[i][0]) 
	  			<= 1.5*sqrt(mmseEst->covariance[0])) & 
	 				(fabs(mmseEst->y - dataKft(idx,8) - mmseResults[i][1]) 
	  			<= 1.5*sqrt(mmseEst->covariance[2])))
         successful[i] = true;
      else
         successful[i] = false;
  }
	//print results of each test
  int numPassed = 0;
  for(i = 0; i < numTests; i++)
    {
      if(successful[i])
	{
	  printf("Test #%i passed\n", i+1);
	  numPassed++;
	}
      else
	printf("Test #%i failed\n", i+1);     
    }
  printf("%i of %i tests passed\n", numPassed, numTests);

  //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 [] dataK_files;
  delete [] dataMeas_files;
  delete [] map_files;
  delete [] watVel_files;
  delete [] dataK_numRows;
  delete [] dataMeas_numRows;
  delete [] init_dataK;
  delete [] mmseResults;
  delete [] successful;

  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, int dataK_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 i_init = dataK_init;
  int j_init = 1;
  int i,j,k;
  sem_t semaphore;
  struct timespec semTime;

  sem_init( &semaphore, 0, 0 );


  //initialize terrainNav object and load map
  TerrainNav* tercom = new TerrainNav(mapPath, (char*)"imagingAUV_specs.cfg",
                                      filterType);

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

  //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;
  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));

          
          tercom->motionUpdate(currEstimate);
	  i = i+kSubSample;

          //ensure that we always perform the final motion update
          if(i > N && (i-kSubSample < N))
             i = N;

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

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


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