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

///////////////////////////////////////////////////////////////
// Definitions for client-server comms
//
#define MAX_MSG_SIZE 2000
struct commsT
{
  char msg_type;
  unsigned int msg_length;
  char msg[MAX_MSG_SIZE];
};

int   measT_serialize(struct measT* m, const char *buf, int buflen);
int measT_unserialize(struct measT* m, const char *buf, int buflen);

int   poseT_serialize(struct poseT* m, const char *buf, int buflen);
int poseT_unserialize(struct poseT* m, const char *buf, int buflen);

// Initialization messages. Map file name, vehicle config file name, and 
// filter type.
// msg[0] = map file name, null-terminated string (variable length = M)
// msg[M] = vehicle config file name, null-terminated string (variable length = V)
// msg[M+V] = filter type, one byte.
//
// Example:
//  msg = "mapname.map\0confignamej.cfg\02" (2 is the number 0x02, not
//                                           the character '2')
//
#define TRN_INIT 'I'

// Measure and Motion Update messages from client to server drive the process.
// msg[0] = Sonar measurement type, one byte.
// msg[1] = serialized measT object (variable length = M).
// msg[M+1] = serialized poseT object (constant length = P).
//
#define TRN_MM_UPDATE 'U'

// Estimated Position messages from server to client are generated as a result of
// update messages.
// msg[0] = serialized poseT object representing MLE location (constance length = E)
// msg[S] = serialized poseT object representing MLE location (constance length = S)
//
#define TRN_ES_POS 'P'

// Set modified weighting message.
// msg[0] = 1 for true, 0 for false
//
#define TRN_SET_MW 'W'

// Set filter reinit message.
// msg[0] = 1 for true, 0 for false
//
#define TRN_SET_FR 'F'

// Set Interpolate measurement attitude message.
// msg[0] = 1 for true, 0 for false
//
#define TRN_SET_IMA 'A'

// Set drift rate message.
// msg[0] = float value
//
#define TRN_SET_DR 'D'

// Use high-grade filter message.
//
#define TRN_USE_HGF 'H'

// Use high-grade filter message.
//
#define TRN_USE_LGF 'L'


#endif