/* File: structDefs.h
 * -------------------
 * structDefs defines various structs and their member functions for use in 
 * terrainNav type programs.
 *
 * Dependencies:
 * newmat*.h, myOutput.h, matrixArrayCalcs.h
 *
 * Written by: Debbie Meduna
 ******************************************************************************/

#ifndef _structDefs_h_
#define _structDefs_h_

#include "matrixArrayCalcs.h"
#include "myOutput.h"

#include "math.h"
#include <fstream>
#include "string.h"

#include <newmatap.h>
#include <newmatio.h>

#ifndef PI
#define PI 3.14159265358979
#endif

//!The mapT struct stores data in a gridded Matrix.
struct mapT {
  Matrix depths; //Positive down
  Matrix depthVariance;
  double* xpts; //North
  double* ypts; //East
  double dx, dy;
  double xcen, ycen;
  int numX, numY;

  mapT();
  ~mapT();
  void clean();
  void reSampleMap(const double newRes);
  void subSampleMap(const int subRes);
  void displayMap();
  mapT& operator=(mapT& rhs);
};

//!The poseT struct stores vehicle pose information.
struct poseT {
  double x, y, z; //North, East, Down
  double vx, vy, vz;
  double vw_x, vw_y, vw_z;
  double ax, ay, az;
  double phi, theta, psi;
  double wx, wy, wz;
  double time;
  bool dvlValid;
  bool gpsValid;
  bool bottomLock;
  double covariance[36];
  
  poseT();
  poseT& operator=(poseT& rhs);
  poseT& operator-=(poseT& rhs);
  poseT& operator+=(poseT& rhs);
};

//!The measT struct stores sonar measurement information.
struct measT {
  double time;
  int dataType; //1: DVL, 2: Multibeam, 3: Single Beam,
		//4: Homer Relative Measurement
  double phi, theta, psi;
  double x, y, z;
  double* covariance;
  double* ranges;
  double* crossTrack;
  double* alongTrack;
  double* altitudes;
  bool* measStatus;
  int numMeas;

  measT();
  ~measT();
  void clean();
  measT& operator=(measT& rhs);
};

//!The transformT struct stores a rotation and translation vector.
struct transformT
{
  double rotation[3];
  double dr[3];
  void displayTransformInfo();
};

//!The sensorT struct stores sonar sensor-specific information.
struct sensorT
{
  char name[256];
  int numBeams;
  double percentRangeError;
  double beamWidth;
  int type;  //should match type of associated measT measurements
  transformT* T_bs;

  sensorT();
  sensorT(char* fileName);
  ~sensorT();
  void parseSensorSpecs(char* fileName);
  void displaySensorInfo();
};

//!The vehicleT struct stores vehicle-specific information.
struct vehicleT
{
  char name[256];
  int numSensors;
  double driftRate;
  transformT* T_sv;
  sensorT* sensors;
  
  vehicleT();
  vehicleT(char* fileName);
  ~vehicleT();
  void parseVehicleSpecs(char* fileName);
  void displayVehicleInfo();
};

#endif