/* 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
 * 2014/??/??: Henthorn. Updated to support client/server architecture.
 * 2016/12/19: Henthorn. Updated to pass log dir name and particle file name to
 *             the server during initialization.
 ******************************************************************************/

#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.
#define N_COVAR 36               //XYZ, phi, theta, psi, wy, wz covariance
struct poseT {

  double x, y, z;                //North, East, Down position (m)
  double vx, vy, vz, ve;         //Vehicle velocity wrto iceberg, coordinatized in Body Frame(m/s)
  double vw_x, vw_y, vw_z;       //Vehicle velocity wrto water, coordinatized in Body (m/s)
  double vn_x, vn_y, vn_z;       //Vehicle velocity wrto an inertial frame, coordinatized in Body (m/s)
  double wx, wy, wz;             //Vehicle angular velocity wrto an inertial frame, coordinatized in Body (rad/sec)
  double ax, ay, az;             //Vehicle aceleration wrto an inertial frame coordinatized in Body (m/s^2)
  double phi, theta, psi;        //3-2-1 Euler angles relating the B frame to an inertial NED frame (rad).
  double psi_berg, psi_dot_berg; //TRN states

  double time;		  		 				 //Time (s)

  bool dvlValid;							  //Validity flag for dvl motion measurment
  bool gpsValid;							  //Validity flag for GPS measurement
  bool bottomLock;						  //Validity flag for DVL lock onto seafloor

  double covariance[N_COVAR];   //XYZ, phi, theta, psi, wy, wz covariance (passively stable in roll) (see above units)
  
  poseT();
  poseT& operator=(poseT& rhs);
  poseT& operator-=(poseT& rhs);
  poseT& operator+=(poseT& rhs);

  int   serialize(char *buf, int buflen);
  int unserialize(char *buf, int buflen);
};

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

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

  int   serialize(char *buf, int buflen);
  int unserialize(char *buf, int buflen);
};

//!The mbT struct stores multibeam sonar measurement information.
struct mbT {
  double time;  //Measurement time (s)
  int numBeams;
  float* ranges;

  mbT();
  ~mbT();
  void clean();
  mbT& operator=(mbT& rhs);

  int   serialize(char *buf, int buflen);
  int unserialize(char *buf, int buflen);
};

//!The transformT struct stores a rotation and translation vector.
struct transformT
{
	//TODO: perhaps also add a precomputed transformation matrix?
  double rotation[3];
  double translation[3];
  int dr;
  void displayTransformInfo();
};

//!The sensorT struct stores sonar sensor-specific information.
struct sensorT
{
  char name[256];
  char filename[300];
  int numBeams;
  double percentRangeError;
  double beamWidth;
  int type;  //should match type of associated measT measurements
  transformT* T_bs;			//TODO:Beam to Sensor transform (TODO:rename)

  sensorT();
  sensorT(char* fileName);
  ~sensorT();
  void parseSensorSpecs(char* fileName);  // Returns zero on success
  void displaySensorInfo();
};

//!The vehicleT struct stores vehicle-specific information.
struct vehicleT
{
  char name[256];
  int numSensors;
  double driftRate;
  transformT* T_sv;			//TODO:Sensor to Vehicle transform (TODO:rename)
  sensorT* sensors;
  
  vehicleT();
  vehicleT(char* fileName);
  ~vehicleT();
  void parseVehicleSpecs(char* fileName); // Returns zero on success
  void displayVehicleInfo();
};

///////////////////////////////////////////////////////////////
// Definitions for client-server comms
//
#define TRN_MSG_SIZE 2500
#define TRN_PAYLOAD_SIZE (TRN_MSG_SIZE - 2*sizeof(char) - sizeof(unsigned int))

// Structure used to standardize marshalling client-server messages
// Usage:
//   {
//     char comms_buf[TRN_MSG_SIZE];
//     measT  mt;
//     :                                           // Do stuff with measT
//     commsT my_ct(TRN_MEAS, 0, 0, &mt);          // Create commsT
//     my_ct.serialize(comms_buf, TRN_MSG_SIZE);   // Flatten into buffer
//     send(sockfd, comms_buf, TRN_MSG_SIZE, 0);   // Send to server
//     recv(sockfd, comms_buf, TRN_MSG_SIZE, 0);   // Response
//     my_ct.unserialize(comms_buf, TRN_MSG_SIZE); // Look for ack/nack
//     if (my_ct.msg_type == TRN_ACK);             // OK
//     if (my_ct.msg_type == TRN_NACK);            // Problem
//   }
//
struct commsT
{
  char msg_type;
  char parameter;
  float vdr;
  poseT pt;
  measT mt;
  mbT   mb;
  char *mapname;
  char *cfgname;
  char *particlename;
  char *logname;

  commsT();
  commsT(char msg_type);
  commsT(char msg_type, char parameter);
  commsT(char msg_type, char parameter, float vdr);
  commsT(char msg_type, poseT& pt);
  commsT(char msg_type, char parameter, measT& mt);
  commsT(char msg_type, char parameter, mbT& mb);
  commsT(char msg_type, char parameter, char *map, char *cfg, char *particles, char *logdir);
  ~commsT();

  char* to_s(char *buf, int buflen); // Write a string representation of the object
  void clean();                     // Clear state
  int   serialize(char *buf, int buflen=TRN_MSG_SIZE);
  int unserialize(char *buf, int buflen=TRN_MSG_SIZE);
};

// Conversations are initiated by the client through request messages.
// The server responds to all requests with either the information requested,
// or a ack/nack.
//
// TRN message are simple flat buffers of length TRN_MSG_SIZE.
// The first 6 bytes always consist of "header" information.
//   buf[0]:
//        char         msg_type;              // Message type (defined below)
//   buf[1]:
//        char         parameter;             // Simple parameter (1, 0, etc.)
//   buf[2*sizeof(char)]:
//        unsigned int msg_len                // Length of remaining message
//   buf[2*sizeof(char)+sizeof(unsigned int)]:
//        char         msg[TRN_PAYLOAD_SIZE]; // Payload (defined below)
//
/////////////////////////////////////////////////////////////////////////
// Message descriptions:
//

#define TRN_INIT 'I'

// Initialization messages.
// Client sends 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 '2')
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_MEAS 'M'

// Measure Update message from client to server.
// buf[1] = Sonar measurement type, one byte.
// msg[0] = serialized measT object (variable length = M).
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_MB 'K'

// Multibeam sonar Update message from client to server.
// buf[1] = Sonar measurement type, one byte.
// msg[0] = serialized measT object (variable length = M).
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_MOTN 'N'

// Motion Update messages from client to server drive the process.
// msg[0] = serialized poseT object (constant length = P).
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_MLE 'E'

// MLE Estimated Position messages from client to server are requests for data from
// the server. The requests consist of only the message type (the rest of the
// message is ignored). The server reponds with a 'E' type message with a
// serialized poseT structure containing the requested data, or a nack message.
//
// From server:
// msg[0] = serialized poseT object representing MLE location (constance length = E)
//

#define TRN_MMSE 'S'

// MMSE Estimated Position messages from client to server are requests for data from
// the server. The requests consist of only the message type (the rest of the
// message is ignored). The server reponds with a 'S' type message with a
// serialized poseT structure containing the requested data, or a nack message.
//
// From server:
// msg[0] = serialized poseT object representing MMSE location (constance length = S)
//

#define TRN_SET_MW 'W'

// Set modified weighting message.
// buf[1] = 1 for true, 0 for false
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_SET_FR 'F'

// Set filter reinit message.
// buf[0] = 1 for true, 0 for false
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_SET_IMA 'A'

// Set Interpolate measurement attitude message.
// buf[0] = 1 for true, 0 for false
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_SET_VDR 'D'

// Set drift rate message.
// msg[0] = float value
//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_SET_MIM 'Q'

// Set Map interpolation method message.
// msg[0] = method
// * 0: nearest-neighbor (no interpolation)
// * 1: bilinear
// * 2: bicubic
// * 3: spline
// * Default = 0;

//
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_FILT_GRD 'G'

// Use filter grade message.
// buf[0] = 1 for high, 0 for low
// 
// Server responds with an ack if initialization was successful, otherwise nack.

#define TRN_ACK '+'

// Ack last message. Request succeeded.
// Sent when no data other response is expected.
// 

#define TRN_NACK '-'

// Nack last message. Request failed.
// Sent when no data other response is expected, or a data request failed.
// 

#define TRN_BYE 'B'

// Optional message to close the link.
// No response expected.
// 

#define TRN_OUT_MEAS  'O'

// Server returns a boolean indicating whether there are any
// outstanding measurements not incorporated in the PDF in the
// parameter of the returned message (1 = true, 0 = false).
//

#define TRN_LAST_MEAS  'L'

// Server returns a boolean indicating whether the last sonar
// measurement was successfully incorporated in the filter in
// the parameter of the returned message (1 = true, 0 = false).
//

#define TRN_IS_CONV    'C'

// Server returns a boolean indicating whether the TRN filter
// has converged to an estimate in the
// parameter of the returned message (1 = true, 0 = false).
//

#define TRN_FILT_TYPE  'T'

// Server returns a integer filter type describing the current
// TNavFilter type in the
// parameter of the returned message.
//

#define TRN_FILT_STATE 'H'

// Server returns a integer filter state describing the current
// TNavFilter state in the
// parameter of the returned message.
//

#define TRN_N_REINITS 'R'

// Server returns a integer describing the number
// of reinitializations in the
// parameter of the returned message.
//

#define TRN_FILT_REINIT 'r'

// Server instruct TRN to reinitialize its filter
//

#endif